ros node improvements

include/PointCloudClusterer.hpp

@@ -261,9 +261,9 @@ void PointCloudClusterer<PointType>::organizedMultiplaneSegmentation(
 	pcl::OrganizedMultiPlaneSegmentation<PointType, pcl::Normal, pcl::Label> plane_segmentation;
 	plane_segmentation.setInputCloud(cloud);
 	plane_segmentation.setInputNormals(normals);
-	plane_segmentation.setDistanceThreshold(0.03);
+	plane_segmentation.setDistanceThreshold(0.02);
 	//plane_segmentation.setAngularThreshold(pcl::deg2rad(5.0));
-	//plane_segmentation.setMaximumCurvature(0.01);
+	plane_segmentation.setMaximumCurvature(0.001);
 	plane_segmentation.setProjectPoints(true);
 
 	std::vector<pcl::PlanarRegion<PointType>,

launch/detect.launch

@@ -9,6 +9,7 @@
 
     <node pkg="object_recognition_by_parts" name="$(anon object_recognition_by_parts)" type="object_recognition_by_parts_node">
       <param name="model" type="string" value="$(arg model)" />
+      <param name="remove_planes" type="bool" value="false" />
       <remap from="cloud_in" to="camera/depth_registered/points" />
       <remap from="image_rgb_in" to="camera/rgb/image_rect_color" />
       <remap from="image_depth_in" to="camera/depth_registered/image_rect" /> 

ros/Node.cpp

@@ -67,6 +67,7 @@ bool PartsBasedDetectorNode::init(void)
 	// private nodehandle
 	ros::NodeHandle priv_nh("~");
 	priv_nh.getParam("model", modelfile);
+	priv_nh.getParam("remove_planes", remove_planes_);
 
 	string ext = boost::filesystem::path(modelfile).extension().c_str();
 	ROS_INFO("Loading model %s", modelfile.c_str());
@@ -202,8 +203,18 @@ void PartsBasedDetectorNode::detectorCallback(const ImageConstPtr& msg_d,
 	if (cloud_pub_.getNumSubscribers() > 0
 			|| object_pose_pub_.getNumSubscribers() > 0)
 	{
-		PointCloudClusterer::clusterObjects(msg_cloud, bounding_boxes, clusters,
+		if(remove_planes_)
+		{
+			PointCloud::Ptr cloud_no_planes (new PointCloud());
+			PointCloudClusterer::organizedMultiplaneSegmentation(msg_cloud, *cloud_no_planes);
+			PointCloudClusterer::clusterObjects(cloud_no_planes, bounding_boxes, clusters,
 				object_centers);
+		}
+		else
+		{
+			PointCloudClusterer::clusterObjects(msg_cloud, bounding_boxes, clusters,
+				object_centers);
+		}
 	}
 
 	// publish on the various topics (only if there are subscribers)

ros/Node.hpp

@@ -114,6 +114,7 @@ class PartsBasedDetectorNode {
 	MarkerArray bounding_box_markers_;
 	std::string ns_;
 	std::string name_;
+	bool remove_planes_;
 
 	// camera parameters
 	bool depth_camera_initialized_;
@@ -137,6 +138,7 @@ class PartsBasedDetectorNode {
 			pointcloud_sub_(nh_, "cloud_in", 1),
 			sync_(KinectSyncPolicy(50), image_sub_d_, image_sub_rgb_, pointcloud_sub_),
 			ns_("/pbd/"),
+			remove_planes_ (false),
 			depth_camera_initialized_(false) {	}
 
 	// initialisation