Merge pull request #152 from garaemon/add-normal-estimation-to-organi…

…zed-multi-plane-segmentation

Add normal estimation to organized multi plane segmentation

jsk_pcl_ros/catkin.cmake

@@ -68,7 +68,7 @@ macro(jsk_pcl_nodelet _nodelet_cpp _nodelet_class _single_nodelet_exec_name)
     LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
 endmacro(jsk_pcl_nodelet _nodelet_cpp _nodelet_class _single_nodelet_exec_name)
 
-add_definitions("-O2")
+add_definitions("-O2 -g")
 
 # pcl_ros::Filter based class is not working...
 # https://github.com/ros-perception/perception_pcl/issues/9

jsk_pcl_ros/cfg/OrganizedMultiPlaneSegmentation.cfg

@@ -21,6 +21,16 @@ gen.add("max_curvature", double_t, 0, "maximum curvature of organized plane segm
 gen.add("connect_plane_angle_threshold", double_t, 0, "plane angle threshold of connecting the planes", 0.2, 0, pi)
 gen.add("connect_plane_distance_threshold", double_t, 0, "plane distance threshold of connecting the planes", 0.03, 0, 1.0)
 gen.add("connect_distance_threshold", double_t, 0, "distance threshold of connectin the planes", 0.01, 0, 1.0)
+estimation_method_enum = gen.enum([gen.const("AVERAGE_3D_GRADIENT", int_t, 0, "AVERAGE 3D GRADIENT, no curvature is available"),
+                                   gen.const("COVARIANCE_MATRIX", int_t, 1, "COVARIANCE_MATRIX, curvature is available"),
+                                   gen.const("AVERAGE_DEPTH_CHANGE", int_t, 2, "AVERAGE_DEPTH_CHANGE, no curvature is available")],
+                                   "normal estimation method")
+gen.add("max_depth_change_factor", double_t, 0, "max depth change factor", 0.02, 0.0, 1.0)
+gen.add("normal_smoothing_size", double_t, 0, "normal smoothing size parameter", 20.0, 0.0, 100.0)
+gen.add("estimation_method", int_t, 0, "estimation method", 1, 0, 2, edit_method = estimation_method_enum)
+gen.add("depth_dependent_smoothing", bool_t, 0, "use depth dependent smoothing", False)
+gen.add("border_policy_ignore", bool_t, 0, "ignore border policy", True)
+gen.add("publish_normal", bool_t, 0, "publish normal pointcloud", False)
 #gen.add("concave_alpha", double_t, 0, "alpha parameter for concave estimation", 0.1, 0, 1.0)
 
 exit (gen.generate (PACKAGE, "jsk_pcl_ros", "OrganizedMultiPlaneSegmentation"))

jsk_pcl_ros/include/jsk_pcl_ros/organized_multi_plane_segmentation.h

@@ -52,9 +52,11 @@ namespace jsk_pcl_ros
   class OrganizedMultiPlaneSegmentation: public pcl_ros::PCLNodelet
   {
   public:
+    typedef pcl::PointXYZRGBA PointT;
   protected:
     ros::Publisher org_pub_, org_polygon_pub_, org_coefficients_pub_;
     ros::Publisher pub_, polygon_pub_, coefficients_pub_;
+    ros::Publisher normal_pub_;
     ros::Subscriber sub_;
     int min_size_;
     double concave_alpha_;
@@ -64,30 +66,42 @@ namespace jsk_pcl_ros
     double connect_plane_angle_threshold_;
     double connect_plane_distance_threshold_;
     double connect_distance_threshold_;
+    int estimation_method_;
+    bool depth_dependent_smoothing_;
+    double max_depth_change_factor_;
+    double normal_smoothing_size_;
+    bool border_policy_ignore_;
+    bool estimate_normal_;
+    bool publish_normal_;
     typedef jsk_pcl_ros::OrganizedMultiPlaneSegmentationConfig Config;
     boost::shared_ptr <dynamic_reconfigure::Server<Config> > srv_;
     boost::mutex mutex_;
     virtual void segment(const sensor_msgs::PointCloud2::ConstPtr& msg);
+    virtual void estimateNormal(pcl::PointCloud<PointT>::Ptr input,
+                                pcl::PointCloud<pcl::Normal>::Ptr output);
     virtual void configCallback (Config &config, uint32_t level);
-    virtual void pointCloudToPolygon(const pcl::PointCloud<pcl::PointXYZRGBNormal>& input,
+    virtual void pointCloudToPolygon(const pcl::PointCloud<PointT>& input,
                                      geometry_msgs::Polygon& polygon);
     virtual void pclIndicesArrayToClusterPointIndices(const std::vector<pcl::PointIndices>& inlier_indices,
                                                       const std_msgs::Header& header,
                                                       jsk_pcl_ros::ClusterPointIndices& output_indices);
-    virtual void connectPlanesMap(const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr& input,
+    virtual void connectPlanesMap(const pcl::PointCloud<PointT>::Ptr& input,
                                   const std::vector<pcl::ModelCoefficients>& model_coefficients,
                                   const std::vector<pcl::PointIndices>& boundary_indices,
                                   std::vector<std::map<size_t, bool> >& connection_map);
-    virtual void buildConnectedPlanes(const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr& input,
+    virtual void buildConnectedPlanes(const pcl::PointCloud<PointT>::Ptr& input,
                                       const std_msgs::Header& header,
                                       const std::vector<pcl::PointIndices>& inlier_indices,
                                       const std::vector<pcl::PointIndices>& boundary_indices,
                                       const std::vector<pcl::ModelCoefficients>& model_coefficients,
                                       std::vector<std::map<size_t, bool> > connection_map,
                                       std::vector<pcl::PointIndices>& output_indices,
                                       std::vector<pcl::ModelCoefficients>& output_coefficients,
-                                      std::vector<pcl::PointCloud<pcl::PointXYZRGBNormal> >& output_boundary_clouds);
-
+                                      std::vector<pcl::PointCloud<PointT> >& output_boundary_clouds);
+
+    virtual void segmentFromNormals(pcl::PointCloud<PointT>::Ptr input,
+                                    pcl::PointCloud<pcl::Normal>::Ptr normal,
+                                    const std_msgs::Header& header);
   private:
     virtual void onInit();
   };

jsk_pcl_ros/launch/organized_multi_plane_segmentation.launch

@@ -3,12 +3,6 @@
   <arg name="LAUNCH_MANAGER" default="true" />
   <arg name="MANAGER" default="manager" />
   <arg name="ESTIMATE_OCLUSION" default="false" />
-  <group ns="input">
-    <node pkg="nodelet" type="nodelet" name="hz"
-          output="screen"
-          args="load jsk_topic_tools/HzMeasure /$(arg MANAGER)">
-      <remap from="~input" to="$(arg INPUT)" />
-    </node>
   <arg name="MACHINE" default="localhost"/>
   <arg name="GDB" default="false" />
   <machine name="localhost" address="localhost" />
@@ -25,35 +19,26 @@
           unless="$(arg GDB)"
           output="screen"/>
   </group>
-  <node pkg="nodelet" type="nodelet" name="normal_estimate"
-        args="load jsk_pcl/NormalEstimationIntegralImage $(arg MANAGER)"
+  <node pkg="nodelet" type="nodelet" name="input_relay"
         machine="$(arg MACHINE)"
-        output="screen"
-        >
+        args="load jsk_topic_tools/Relay $(arg MANAGER)">
     <remap from="~input" to="$(arg INPUT)" />
-    <!-- <remap from="~output" to="/normal_concat/input" /> -->
-    <rosparam>
-    </rosparam>
-  </node>
-  <node pkg="nodelet" type="nodelet" name="normal_concat"
-        machine="$(arg MACHINE)"
-        args="load jsk_pcl/NormalConcatenater $(arg MANAGER)"
-        clear_params="false">
-    <remap from="~input" to="$(arg INPUT)"/>
-    <remap from="~normal" to="/normal_estimate/output"/>
   </node>
   <node pkg="nodelet" type="nodelet" name="multi_plane_estimate"
         machine="$(arg MACHINE)"
         args="load jsk_pcl/OrganizedMultiPlaneSegmentation $(arg MANAGER)"
         clear_params="false">
-    <remap from="~input" to="/normal_concat/output"/>
+    <remap from="~input" to="input_relay/output"/>
+    <rosparam>
+      estimate_normal: true
+    </rosparam>
   </node>
   <node pkg="nodelet" type="nodelet"
         name="cluster_decomposer"
         machine="$(arg MACHINE)"
         args="load jsk_pcl/ClusterPointIndicesDecomposer $(arg MANAGER)"
         output="screen" clear_params="true">
-    <remap from="~input" to="/normal_concat/output" />
+    <remap from="~input" to="input_relay/output" />
     <remap from="~target" to="/multi_plane_estimate/output" />
     <rosparam>
       publish_clouds: false
@@ -64,7 +49,7 @@
         machine="$(arg MACHINE)"
         args="load jsk_pcl/ClusterPointIndicesDecomposer $(arg MANAGER)"
         output="screen" clear_params="true">
-    <remap from="~input" to="/normal_concat/output" />
+    <remap from="~input" to="input_relay/output" />
     <remap from="~target" to="/multi_plane_estimate/output_nonconnected" />
     <rosparam>
       publish_clouds: false
@@ -76,7 +61,7 @@
         machine="$(arg MACHINE)"
         args="load jsk_pcl/MultiPlaneExtraction $(arg MANAGER)"
         output="screen" clear_params="true">
-    <remap from="~input" to="/normal_concat/output" />
+    <remap from="~input" to="input_relay/output" />
     <remap from="~indices" to="/multi_plane_estimate/output" />
     <remap from="~input_polygons" to="/multi_plane_estimate/output_polygon" />
     <remap from="~input_coefficients" to="/multi_plane_estimate/output_coefficients" />

jsk_pcl_ros/src/multi_plane_extraction_nodelet.cpp

@@ -61,7 +61,7 @@ namespace jsk_pcl_ros
     PCLNodelet::onInit();
     //pub_ = pnh_->advertise<PCLIndicesMsg>("output", 1);
     pub_ = pnh_->advertise<sensor_msgs::PointCloud2>("output", 1);
-    nonplane_pub_ = pnh_->advertise<pcl::PointCloud<pcl::PointXYZRGBNormal> >("output_nonplane_cloud", 1);
+    nonplane_pub_ = pnh_->advertise<pcl::PointCloud<pcl::PointXYZRGB> >("output_nonplane_cloud", 1);
     if (!pnh_->getParam("max_queue_size", maximum_queue_size_)) {
       maximum_queue_size_ = 100;
     }
@@ -93,7 +93,7 @@ namespace jsk_pcl_ros
   {
     boost::mutex::scoped_lock(mutex_);
     // convert all to the pcl types
-    pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>());
+    pcl::PointCloud<pcl::PointXYZRGB>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
     pcl::fromROSMsg(*input, *input_cloud);
 
     // concat indices into one PointIndices
@@ -105,8 +105,8 @@ namespace jsk_pcl_ros
       }
     }
 
-    pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr nonplane_cloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>());
-    pcl::ExtractIndices<pcl::PointXYZRGBNormal> extract_nonplane;
+    pcl::PointCloud<pcl::PointXYZRGB>::Ptr nonplane_cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
+    pcl::ExtractIndices<pcl::PointXYZRGB> extract_nonplane;
     extract_nonplane.setNegative(true);
     extract_nonplane.setInputCloud(input_cloud);
     extract_nonplane.setIndices(all_indices);
@@ -118,11 +118,11 @@ namespace jsk_pcl_ros
     std::set<int> result_set;
     for (size_t plane_i = 0; plane_i < coefficients->coefficients.size(); plane_i++) {
 
-      pcl::ExtractPolygonalPrismData<pcl::PointXYZRGBNormal> prism_extract;
-      pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr hull_cloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>());
+      pcl::ExtractPolygonalPrismData<pcl::PointXYZRGB> prism_extract;
+      pcl::PointCloud<pcl::PointXYZRGB>::Ptr hull_cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
       geometry_msgs::Polygon the_polygon = polygons->polygons[plane_i].polygon;
       for (size_t i = 0; i < the_polygon.points.size(); i++) {
-        pcl::PointXYZRGBNormal p;
+        pcl::PointXYZRGB p;
         p.x = the_polygon.points[i].x;
         p.y = the_polygon.points[i].y;
         p.z = the_polygon.points[i].z;
@@ -132,7 +132,7 @@ namespace jsk_pcl_ros
       prism_extract.setInputCloud(nonplane_cloud);
       prism_extract.setHeightLimits(min_height_, max_height_);
       prism_extract.setInputPlanarHull(hull_cloud);
-      //pcl::PointCloud<pcl::PointXYZRGBNormal> output;
+      //pcl::PointCloud<pcl::PointXYZRGB> output;
       pcl::PointIndices output_indices;
       prism_extract.segment(output_indices);
       // append output to result_cloud
@@ -144,15 +144,15 @@ namespace jsk_pcl_ros
 
     // convert std::set to PCLIndicesMsg
     //PCLIndicesMsg output_indices;
-    pcl::PointCloud<pcl::PointXYZRGBNormal> result_cloud;
+    pcl::PointCloud<pcl::PointXYZRGB> result_cloud;
     pcl::PointIndices::Ptr all_result_indices (new pcl::PointIndices());
     for (std::set<int>::iterator it = result_set.begin();
          it != result_set.end();
          it++) {
       all_result_indices->indices.push_back(*it);
     }
 
-    pcl::ExtractIndices<pcl::PointXYZRGBNormal> extract_all_indices;
+    pcl::ExtractIndices<pcl::PointXYZRGB> extract_all_indices;
     extract_all_indices.setInputCloud(nonplane_cloud);
     extract_all_indices.setIndices(all_result_indices);
     extract_all_indices.filter(result_cloud);