Merge pull request #22 from braincorp/floor_removal_with_simple_extraction_option

Floor removal with simple extraction option

CMakeLists.txt

@@ -137,6 +137,7 @@ SET(rtabmap_ros_lib_src
    src/nodelets/point_cloud_xyz.cpp
    src/nodelets/disparity_to_depth.cpp 
    src/nodelets/obstacles_detection.cpp
+   src/nodelets/point_cloud_aggregator.cpp
    src/MsgConversion.cpp
    src/OdometryROS.cpp
    src/rviz/MapCloudDisplay.cpp

nodelet_plugins.xml

@@ -54,4 +54,13 @@
       This is my nodelet.
     </description>
   </class>
+
+  <class name="rtabmap_ros/point_cloud_aggregator" 
+         type="rtabmap_ros::PointCloudAggregator" 
+         base_class_type="nodelet::Nodelet">
+    <description>
+      This is my nodelet.
+    </description>
+  </class>
+
 </library>

src/nodelets/obstacles_detection.cpp

@@ -70,8 +70,11 @@ public:
 		normalEstimationRadius_(0.05),
 		groundNormalAngle_(M_PI_4),
 		minClusterSize_(20),
-		maxObstaclesHeight_(0),
-		waitForTransform_(false)
+		maxFloorHeight_(-1),
+		maxObstaclesHeight_(1.5),
+		waitForTransform_(false),
+		simpleSegmentation_(false),
+		optimizeForCloseObject_(true)
 	{}
 
 	virtual ~ObstaclesDetection()
@@ -90,20 +93,29 @@ private:
 		pnh.param("ground_normal_angle", groundNormalAngle_, groundNormalAngle_);
 		pnh.param("min_cluster_size", minClusterSize_, minClusterSize_);
 		pnh.param("max_obstacles_height", maxObstaclesHeight_, maxObstaclesHeight_);
+		pnh.param("max_floor_height", maxFloorHeight_, maxFloorHeight_);
 		pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
+		pnh.param("simple_segmentation", simpleSegmentation_, simpleSegmentation_);
+		pnh.param("optimize_for_close_object", optimizeForCloseObject_, optimizeForCloseObject_);
 
 		cloudSub_ = nh.subscribe("cloud", 1, &ObstaclesDetection::callback, this);
 
 		groundPub_ = nh.advertise<sensor_msgs::PointCloud2>("ground", 1);
 		obstaclesPub_ = nh.advertise<sensor_msgs::PointCloud2>("obstacles", 1);
+
+		this->_lastFrameTime = ros::Time::now();
 	}
 
 
 
 	void callback(const sensor_msgs::PointCloud2ConstPtr & cloudMsg)
 	{
-		if(groundPub_.getNumSubscribers() || obstaclesPub_.getNumSubscribers())
+		if (groundPub_.getNumSubscribers() == 0 && obstaclesPub_.getNumSubscribers() == 0)
 		{
+			// no one wants the results
+			return;
+		}
+
 		rtabmap::Transform localTransform;
 		try
 		{
@@ -111,7 +123,7 @@ private:
 			{
 				if(!tfListener_.waitForTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, ros::Duration(1)))
 				{
-						ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), cloudMsg->header.frame_id.c_str());
+					ROS_ERROR("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), cloudMsg->header.frame_id.c_str());
 					return;
 				}
 			}
@@ -121,37 +133,135 @@ private:
 		}
 		catch(tf::TransformException & ex)
 		{
-				ROS_WARN("%s",ex.what());
+			ROS_ERROR("%s",ex.what());
 			return;
 		}
 
-			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
-			pcl::fromROSMsg(*cloudMsg, *cloud);
-			pcl::IndicesPtr ground, obstacles;
-			if(cloud->size())
-			{
-				cloud = rtabmap::util3d::transformPointCloud(cloud, localTransform);
+		pcl::PointCloud<pcl::PointXYZ>::Ptr originalCloud(new pcl::PointCloud<pcl::PointXYZ>);
+		pcl::fromROSMsg(*cloudMsg, *originalCloud);
 
-				if(maxObstaclesHeight_ > 0)
+		//Even if the original cloud is empty, we need to publish the empty cloud,
+		//Otherwise, the aggregator of point cloud would wait indefinitely to get a valid pointcloud
+		if(originalCloud->size() == 0)
 		{
-					cloud = rtabmap::util3d::passThrough(cloud, "z", std::numeric_limits<int>::min(), maxObstaclesHeight_);
-				}
-				if(cloud->size())
+			ROS_ERROR("Recieved empty point cloud!");
+			if(groundPub_.getNumSubscribers())
 			{
-					rtabmap::util3d::segmentObstaclesFromGround<pcl::PointXYZ>(cloud,
-							ground, obstacles, normalEstimationRadius_, groundNormalAngle_, minClusterSize_);
-				}
+				sensor_msgs::PointCloud2 rosCloud;
+				pcl::toROSMsg(*originalCloud, rosCloud);
+				rosCloud.header.stamp = cloudMsg->header.stamp;
+				rosCloud.header.frame_id = frameId_;
+
+				//publish the message
+				groundPub_.publish(rosCloud);
 			}
 
-			pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
-			if(groundPub_.getNumSubscribers() && ground.get() && ground->size())
+			if(obstaclesPub_.getNumSubscribers())
 			{
-				pcl::copyPointCloud(*cloud, *ground, *groundCloud);
+				sensor_msgs::PointCloud2 rosCloud;
+				pcl::toROSMsg(*originalCloud, rosCloud);
+				rosCloud.header.stamp = cloudMsg->header.stamp;
+				rosCloud.header.frame_id = frameId_;
+
+				//publish the message
+				obstaclesPub_.publish(rosCloud);
 			}
+			return;
+		}
+
+		//Common variables for all strategies
+		pcl::PointCloud<pcl::PointXYZ>::Ptr hypotheticalGroundCloud(new pcl::PointCloud<pcl::PointXYZ>);
 		pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
-			if(obstaclesPub_.getNumSubscribers() && obstacles.get() && obstacles->size())
+		pcl::IndicesPtr ground, obstacles;
+		pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
+
+		ros::Time lasttime = ros::Time::now();
+
+		originalCloud = rtabmap::util3d::transformPointCloud(originalCloud, localTransform);
+		hypotheticalGroundCloud  = rtabmap::util3d::passThrough(originalCloud, "z", std::numeric_limits<int>::min(), maxFloorHeight_);
+		obstaclesCloud = rtabmap::util3d::passThrough(originalCloud, "z", maxFloorHeight_, maxObstaclesHeight_);
+
+		if (simpleSegmentation_) {
+		    // If the option simple segmentation has been set to true,
+		    // the floor is just the hypothetical ground cloud, simply
+		    // cut off based on z
+		    groundCloud = hypotheticalGroundCloud;
+		}
+
+		else if (!optimizeForCloseObject_) {
+			// This is the default strategy
+			// The cloud is divided in two based on reported Z and the position of the camera.
+			// One is the hypothetical ground cloud and the other one is the obstacles pointcloud.
+			// The algorithm then extracts (and removes) from the hypothetical ground cloud
+			// the detected obstacles, and adds them to the obstacles pointcloud
+
+			rtabmap::util3d::segmentObstaclesFromGround<pcl::PointXYZ>(hypotheticalGroundCloud,
+					ground, obstacles, normalEstimationRadius_, groundNormalAngle_, minClusterSize_);
+
+			if(ground.get() && ground->size())
 			{
-				pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud);
+				pcl::copyPointCloud(*hypotheticalGroundCloud, *ground, *groundCloud);
+			}
+
+			if(obstacles.get() && obstacles->size())
+			{
+				pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesFloorCloud(new pcl::PointCloud<pcl::PointXYZ>);
+				pcl::copyPointCloud(*hypotheticalGroundCloud, *obstacles, *obstaclesFloorCloud);
+				*obstaclesCloud += *obstaclesFloorCloud;
+			}
+
+		}
+
+		else {
+			// in this case optimizeForCloseObject_ is true:
+			// we divide the floor point cloud into two subsections, one for all potential floor points up to 1m
+			// one for potential floor points further away than 1m.
+			// For the points at closer range, we use a smaller normal estimation radius and ground normal angle,
+			// which allows to detect smaller objects, without increasing the number of false positive.
+			// For all other points, we use a bigger normal estimation radius (* 3.) and tolerance for the
+			// grond normal angle (* 2.).
+
+			pcl::PointCloud<pcl::PointXYZ>::Ptr hypotheticalGroundCloud_near = rtabmap::util3d::passThrough(hypotheticalGroundCloud, "x", std::numeric_limits<int>::min(), 1.);
+			pcl::PointCloud<pcl::PointXYZ>::Ptr hypotheticalGroundCloud_far = rtabmap::util3d::passThrough(hypotheticalGroundCloud, "x", 1., std::numeric_limits<int>::max());
+
+			obstaclesCloud = rtabmap::util3d::passThrough(obstaclesCloud, "x",  0.8, std::numeric_limits<int>::max());
+
+			// Part 1: segment floor and obstacles near the robot
+			rtabmap::util3d::segmentObstaclesFromGround<pcl::PointXYZ>(hypotheticalGroundCloud_near,
+								ground, obstacles, normalEstimationRadius_, groundNormalAngle_, minClusterSize_);
+
+			if(ground.get() && ground->size())
+			{
+				pcl::copyPointCloud(*hypotheticalGroundCloud_near, *ground, *groundCloud);
+			}
+
+
+			if(obstacles.get() && obstacles->size())
+			{
+				pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesFloorCloud_near(new pcl::PointCloud<pcl::PointXYZ>);
+				pcl::copyPointCloud(*hypotheticalGroundCloud_near, *obstacles, *obstaclesFloorCloud_near);
+				*obstaclesCloud += *obstaclesFloorCloud_near;
+			}
+
+			// Part 2: segment floor and obstacles far from the robot
+			rtabmap::util3d::segmentObstaclesFromGround<pcl::PointXYZ>(hypotheticalGroundCloud_far,
+											ground, obstacles, 3.*normalEstimationRadius_, 2.*groundNormalAngle_, minClusterSize_);
+
+			if(ground.get() && ground->size())
+			{
+				pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud2 (new pcl::PointCloud<pcl::PointXYZ>);
+				pcl::copyPointCloud(*hypotheticalGroundCloud_far, *ground, *groundCloud2);
+				*groundCloud += *groundCloud2;
+			}
+
+
+			if(obstacles.get() && obstacles->size())
+			{
+				pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesFloorCloud_far(new pcl::PointCloud<pcl::PointXYZ>);
+				pcl::copyPointCloud(*hypotheticalGroundCloud_far, *obstacles, *obstaclesFloorCloud_far);
+				*obstaclesCloud += *obstaclesFloorCloud_far;
+			}
+
 		}
 
 		if(groundPub_.getNumSubscribers())
@@ -175,7 +285,12 @@ private:
 			//publish the message
 			obstaclesPub_.publish(rosCloud);
 		}
-		}
+
+		ros::Time curtime = ros::Time::now();
+
+		ros::Duration process_duration = curtime - lasttime;
+		ros::Duration between_frames = curtime - this->_lastFrameTime;
+		this->_lastFrameTime = curtime;
 	}
 
 private:
@@ -184,7 +299,10 @@ private:
 	double groundNormalAngle_;
 	int minClusterSize_;
 	double maxObstaclesHeight_;
+	double maxFloorHeight_;
 	bool waitForTransform_;
+	bool simpleSegmentation_;
+	bool optimizeForCloseObject_;
 
 	tf::TransformListener tfListener_;
 
@@ -192,6 +310,7 @@ private:
 	ros::Publisher obstaclesPub_;
 
 	ros::Subscriber cloudSub_;
+	ros::Time _lastFrameTime;
 };
 
 PLUGINLIB_EXPORT_CLASS(rtabmap_ros::ObstaclesDetection, nodelet::Nodelet);

src/nodelets/point_cloud_aggregator.cpp

@@ -0,0 +1,88 @@
+
+#include <ros/ros.h>
+#include <pluginlib/class_list_macros.h>
+#include <nodelet/nodelet.h>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+
+#include <tf/transform_listener.h>
+
+#include <sensor_msgs/PointCloud2.h>
+
+#include <image_transport/image_transport.h>
+#include <image_transport/subscriber_filter.h>
+
+#include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/subscriber.h>
+#include <message_filters/sync_policies/approximate_time.h>
+
+#include <rtabmap_ros/MsgConversion.h>
+
+namespace rtabmap_ros
+{
+
+class PointCloudAggregator : public nodelet::Nodelet
+{
+public:
+	PointCloudAggregator() : sync(NULL)
+	{}
+
+	virtual ~PointCloudAggregator()
+	{
+	    if (sync!=NULL) delete sync;
+	}
+
+private:
+	void clouds_callback(const sensor_msgs::PointCloud2ConstPtr & cloudMsg_1,
+	                     const sensor_msgs::PointCloud2ConstPtr & cloudMsg_2,
+	                     const sensor_msgs::PointCloud2ConstPtr & cloudMsg_3)
+	{
+		if(cloudPub_.getNumSubscribers())
+		{
+			pcl::fromROSMsg(*cloudMsg_1, cloud1);
+			pcl::fromROSMsg(*cloudMsg_2, cloud2);
+			pcl::fromROSMsg(*cloudMsg_3, cloud3);
+		    pcl::PointCloud<pcl::PointXYZ> totalCloud;
+		    totalCloud = cloud1 + cloud2;
+		    totalCloud += cloud3;
+		    sensor_msgs::PointCloud2 rosCloud;
+		    pcl::toROSMsg(totalCloud, rosCloud);
+		    rosCloud.header.stamp = cloudMsg_1->header.stamp;
+		    rosCloud.header.frame_id = cloudMsg_1->header.frame_id;
+		    cloudPub_.publish(rosCloud);
+		}
+	}
+
+    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::PointCloud2, sensor_msgs::PointCloud2> MySyncPolicy;
+	virtual void onInit()
+	{
+		ros::NodeHandle & nh = getNodeHandle();
+		ros::NodeHandle & pnh = getPrivateNodeHandle();
+
+		int queueSize = 5;
+		pnh.param("queue_size", queueSize, queueSize);
+
+		cloudSub_1_.subscribe(nh, "cloud1", 1);
+		cloudSub_2_.subscribe(nh, "cloud2", 1);
+		cloudSub_3_.subscribe(nh, "cloud3", 1);
+
+        sync = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_);
+        sync->registerCallback(boost::bind(&rtabmap_ros::PointCloudAggregator::clouds_callback, this, _1, _2, _3));
+
+		cloudPub_ = nh.advertise<sensor_msgs::PointCloud2>("combined_cloud", 1);
+	}
+
+    message_filters::Synchronizer<MySyncPolicy>* sync;
+	message_filters::Subscriber<sensor_msgs::PointCloud2> cloudSub_1_;
+	message_filters::Subscriber<sensor_msgs::PointCloud2> cloudSub_2_;
+	message_filters::Subscriber<sensor_msgs::PointCloud2> cloudSub_3_;
+	pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2, cloud3;
+
+	ros::Publisher cloudPub_;
+};
+
+PLUGINLIB_EXPORT_CLASS(rtabmap_ros::PointCloudAggregator, nodelet::Nodelet);
+}
+

src/nodelets/point_cloud_xyz.cpp

@@ -69,7 +69,10 @@ public:
 		approxSyncDepth_(0),
 		approxSyncDisparity_(0),
 		exactSyncDepth_(0),
-		exactSyncDisparity_(0)
+		exactSyncDisparity_(0),
+		cut_right_(0),
+		cut_left_(0),
+		create_close_obstacle_if_depth_is_missing_(false)
 	{}
 
 	virtual ~PointCloudXYZ()
@@ -99,6 +102,10 @@ private:
 		pnh.param("decimation", decimation_, decimation_);
 		pnh.param("noise_filter_radius", noiseFilterRadius_, noiseFilterRadius_);
 		pnh.param("noise_filter_min_neighbors", noiseFilterMinNeighbors_, noiseFilterMinNeighbors_);
+		pnh.param("cut_left", cut_left_, cut_left_);
+		pnh.param("cut_right", cut_right_, cut_right_);
+		pnh.param("special_filter_close_object", create_close_obstacle_if_depth_is_missing_, create_close_obstacle_if_depth_is_missing_);
+
 		ROS_INFO("Approximate time sync = %s", approxSync?"true":"false");
 
 		if(approxSync)
@@ -147,6 +154,47 @@ private:
 		if(cloudPub_.getNumSubscribers())
 		{
 			cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(depth);
+			cv::Mat image=imageDepthPtr->image;
+			int rows = image.rows;
+			int cols = image.cols;
+
+			//Cut left and cut right options to mask the image.
+			//If cut_left (resp. cut_right)  is set to a positive value, we set the first (resp. last) columns
+			//of the depth image to 0, meaning that no depth reading has been received.
+			//Number of columns to be masked is equal to cut_left (resp. cut_right value)
+			if (cut_left_>0){
+				cv::Mat pRoi = image(cv::Rect(0, 0, cut_left_, rows));
+				pRoi.setTo(cv::Scalar(0.));
+			}
+			if (cut_right_<0){
+				cv::Mat pRoi = image(cv::Rect(cols-cut_right_, 0, cut_right_, rows));
+				pRoi.setTo(cv::Scalar(0.));
+			}
+
+			//This option enables a filter for close object.
+			//Fist, we do a median blur on the image to get rid of potential noise
+			//Second, we set all false reading that are likely due to an object sitting in front of the camera
+			// to a short distance estimation (here, 40cm).
+			//This hence make the assumption that the depth camera is looking forward and sees the floor on
+			// the bottom rows of the depth image
+			//This option is highly experimental and should be used with extreme care.
+			if (create_close_obstacle_if_depth_is_missing_){
+				cv::Mat pRoi = image(cv::Rect(int(0.05*(float(cols))),int(0.05*(float(rows))),int(0.9*(float(cols))),int(0.9*float(rows))));
+				cv::medianBlur(pRoi, pRoi, 3);
+
+				//Do filter of close objects
+				//If the depth is registered, there is usually a black frame around the depth image
+				//Hence, the ROI stops before the expected "frame"
+				pRoi = image(cv::Rect(int(cols/10),int(0.8*(float(rows))),int(0.8*(float(cols))),int(0.15*float(rows))));
+				cv::Mat blurredImage=pRoi.clone();
+				cv::GaussianBlur(pRoi, blurredImage, cv::Size(5, 5), 0, 0);
+				for(int y = 0; y < blurredImage.cols; y++)
+					for(int x = 0; x < blurredImage.rows; x++){
+						if (blurredImage.at<unsigned short>(x,y) == 0){
+							pRoi.at<unsigned short>(x,y) = 400;
+						}
+					}
+			}
 
 			image_geometry::PinholeCameraModel model;
 			model.fromCameraInfo(*cameraInfo);
@@ -157,13 +205,12 @@ private:
 
 			pcl::PointCloud<pcl::PointXYZ>::Ptr pclCloud;
 			pclCloud = rtabmap::util3d::cloudFromDepth(
-					imageDepthPtr->image,
+					image,
 					cx,
 					cy,
 					fx,
 					fy,
 					decimation_);
-
 			processAndPublish(pclCloud, depth->header);
 		}
 	}
@@ -245,6 +292,9 @@ private:
 	int decimation_;
 	double noiseFilterRadius_;
 	int noiseFilterMinNeighbors_;
+	int cut_left_;
+	int cut_right_;
+	bool create_close_obstacle_if_depth_is_missing_;
 
 	ros::Publisher cloudPub_;