Merge branch 'master' of github.com:introlab/rtabmap_ros into devel

CMakeLists.txt

@@ -135,6 +135,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

README.md

@@ -11,6 +11,10 @@ For the RTAB-Map libraries and standalone application, visit the [RTAB-Map's hom
 
 ### ROS distribution 
 RTAB-Map is released as binaries in the ROS distribution.
+ * Jade
+  ```
+$ sudo apt-get install ros-jade-rtabmap-ros
+```
  * Indigo
   ```
 $ sudo apt-get install ros-indigo-rtabmap-ros
@@ -21,13 +25,13 @@ $ sudo apt-get install ros-hydro-rtabmap-ros
 ```
 
 ### Build from source
-This section shows how to install RTAB-Map ros-pkg on **ROS Hydro/Indigo** (Catkin build). RTAB-Map works only with the PCL 1.7, which is the default version installed with ROS Hydro/Indigo (**Fuerte and Groovy are not supported**).
- * **Note for ROS Indigo**: If you want SURF/SIFT, you have to build OpenCV from source to have access to *nonfree* module. Install it in `/usr/local` (default) and the rtabmap library should link with it instead of the one installed in ROS.
+This section shows how to install RTAB-Map ros-pkg on **ROS Hydro/Indigo/Jade** (Catkin build). RTAB-Map works only with the PCL 1.7, which is the default version installed with ROS Hydro/Indigo/Jade (**Fuerte and Groovy are not supported**).
+ * **Note for ROS Indigo/Jade**: If you want SURF/SIFT, you have to build OpenCV from source to have access to *nonfree* module. Install it in `/usr/local` (default) and the rtabmap library should link with it instead of the one installed in ROS.
 
  * The next instructions assume that you have setup your ROS workspace using this [tutorial](http://wiki.ros.org/catkin/Tutorials/create_a_workspace). The workspace path is `~/catkin_ws` and your `~/.bashrc` contains:
  
   ```bash
-source /opt/ros/hydro/setup.bash
+source /opt/ros/[hydro|indigo|jade]/setup.bash
 source ~/catkin_ws/devel/setup.bash
 ```
 

launch/tests/test_obstacles_detection.launch

@@ -0,0 +1,30 @@
+<launch>
+
+<!-- Use stereo_outdoorA.bag for testing -->
+<arg name="optimize_for_close_objects" default="false" />
+
+<include file="$(find rtabmap_ros)/launch/demo/demo_stereo_outdoor.launch"/>
+
+<group ns="/stereo_camera" >
+    <node pkg="nodelet" type="nodelet" name="disparity2cloud" args="load rtabmap_ros/point_cloud_xyz stereo_nodelet">
+		<remap from="disparity/image"       to="disparity"/>
+		<remap from="disparity/camera_info" to="right/camera_info_throttle"/>
+		<remap from="cloud"                 to="cloudXYZ"/>
+		
+		<param name="voxel_size" type="double" value="0.05"/>
+		<param name="decimation" type="int" value="4"/>
+		<param name="max_depth" type="double" value="4"/>
+	</node>
+	
+	<node pkg="nodelet" type="nodelet" name="obstacles_detection" args="load rtabmap_ros/obstacles_detection stereo_nodelet">
+	    <remap from="cloud" to="cloudXYZ"/>
+
+		<param name="frame_id" type="string" value="base_footprint"/>		
+		<param name="wait_for_transform" type="bool" value="true"/>
+		<param name="min_cluster_size" type="int" value="20"/>
+		<param name="max_obstacles_height" type="double" value="0.0"/>
+		<param name="optimize_for_close_objects" type="bool" value="$(arg optimize_for_close_objects)"/>
+    </node>
+</group>
+
+</launch>

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,9 @@ public:
 		normalEstimationRadius_(0.05),
 		groundNormalAngle_(M_PI_4),
 		minClusterSize_(20),
-		maxObstaclesHeight_(0),
-		waitForTransform_(false)
+		maxObstaclesHeight_(0.0), // if<=0.0 -> disabled
+		waitForTransform_(false),
+		optimizeForCloseObjects_(false)
 	{}
 
 	virtual ~ObstaclesDetection()
@@ -91,6 +92,7 @@ private:
 		pnh.param("min_cluster_size", minClusterSize_, minClusterSize_);
 		pnh.param("max_obstacles_height", maxObstaclesHeight_, maxObstaclesHeight_);
 		pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
+		pnh.param("optimize_for_close_objects", optimizeForCloseObjects_, optimizeForCloseObjects_);
 
 		cloudSub_ = nh.subscribe("cloud", 1, &ObstaclesDetection::callback, this);
 
@@ -102,8 +104,14 @@ private:
 
 	void callback(const sensor_msgs::PointCloud2ConstPtr & cloudMsg)
 	{
-		if(groundPub_.getNumSubscribers() || obstaclesPub_.getNumSubscribers())
+		ros::Time time = ros::Time::now();
+
+		if (groundPub_.getNumSubscribers() == 0 && obstaclesPub_.getNumSubscribers() == 0)
 		{
+			// no one wants the results
+			return;
+		}
+
 		rtabmap::Transform localTransform;
 		try
 		{
@@ -111,7 +119,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 +129,108 @@ 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);
 
+		//Common variables for all strategies
+		pcl::IndicesPtr ground, obstacles;
+		pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
+		pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
+
+		if(originalCloud->size())
+		{
+			originalCloud = rtabmap::util3d::transformPointCloud(originalCloud, localTransform);
 			if(maxObstaclesHeight_ > 0)
 			{
-					cloud = rtabmap::util3d::passThrough(cloud, "z", std::numeric_limits<int>::min(), maxObstaclesHeight_);
-				}
-				if(cloud->size())
-				{
-					rtabmap::util3d::segmentObstaclesFromGround<pcl::PointXYZ>(cloud,
-							ground, obstacles, normalEstimationRadius_, groundNormalAngle_, minClusterSize_);
-				}
+				originalCloud = rtabmap::util3d::passThrough(originalCloud, "z", std::numeric_limits<int>::min(), maxObstaclesHeight_);
 			}
 
-			pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
+			if(originalCloud->size())
+			{
+				if(!optimizeForCloseObjects_)
+				{
+					// This is the default strategy
+					rtabmap::util3d::segmentObstaclesFromGround<pcl::PointXYZ>(
+							originalCloud,
+							ground,
+							obstacles,
+							normalEstimationRadius_,
+							groundNormalAngle_,
+							minClusterSize_);
+
 					if(groundPub_.getNumSubscribers() && ground.get() && ground->size())
 					{
-				pcl::copyPointCloud(*cloud, *ground, *groundCloud);
+						pcl::copyPointCloud(*originalCloud, *ground, *groundCloud);
 					}
-			pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
+
 					if(obstaclesPub_.getNumSubscribers() && obstacles.get() && obstacles->size())
 					{
-				pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud);
+						pcl::copyPointCloud(*originalCloud, *obstacles, *obstaclesCloud);
+					}
+				}
+				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 originalCloud_near = rtabmap::util3d::passThrough(originalCloud, "x", std::numeric_limits<int>::min(), 1.);
+					pcl::PointCloud<pcl::PointXYZ>::Ptr originalCloud_far = rtabmap::util3d::passThrough(originalCloud, "x", 1., std::numeric_limits<int>::max());
+
+					// Part 1: segment floor and obstacles near the robot
+					rtabmap::util3d::segmentObstaclesFromGround<pcl::PointXYZ>(
+							originalCloud_near,
+							ground,
+							obstacles,
+							normalEstimationRadius_,
+							groundNormalAngle_,
+							minClusterSize_);
+
+					if(groundPub_.getNumSubscribers() && ground.get() && ground->size())
+					{
+						pcl::copyPointCloud(*originalCloud_near, *ground, *groundCloud);
+						ground->clear();
+					}
+
+					if(obstaclesPub_.getNumSubscribers() && obstacles.get() && obstacles->size())
+					{
+						pcl::copyPointCloud(*originalCloud_near, *obstacles, *obstaclesCloud);
+						obstacles->clear();
+					}
+
+					// Part 2: segment floor and obstacles far from the robot
+					rtabmap::util3d::segmentObstaclesFromGround<pcl::PointXYZ>(
+							originalCloud_far,
+							ground,
+							obstacles,
+							3.*normalEstimationRadius_,
+							2.*groundNormalAngle_,
+							minClusterSize_);
+
+					if(groundPub_.getNumSubscribers() && ground.get() && ground->size())
+					{
+						pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud2 (new pcl::PointCloud<pcl::PointXYZ>);
+						pcl::copyPointCloud(*originalCloud_far, *ground, *groundCloud2);
+						*groundCloud += *groundCloud2;
+					}
+
+
+					if(obstaclesPub_.getNumSubscribers() && obstacles.get() && obstacles->size())
+					{
+						pcl::PointCloud<pcl::PointXYZ>::Ptr obstacles2(new pcl::PointCloud<pcl::PointXYZ>);
+						pcl::copyPointCloud(*originalCloud_far, *obstacles, *obstacles2);
+						*obstaclesCloud += *obstacles2;
+					}
+				}
+			}
 		}
 
 		if(groundPub_.getNumSubscribers())
@@ -175,7 +254,8 @@ private:
 			//publish the message
 			obstaclesPub_.publish(rosCloud);
 		}
-		}
+
+		ROS_INFO("Obstacles segmentation time = %f s", (ros::Time::now() - time).toSec());
 	}
 
 private:
@@ -185,6 +265,7 @@ private:
 	int minClusterSize_;
 	double maxObstaclesHeight_;
 	bool waitForTransform_;
+	bool optimizeForCloseObjects_;
 
 	tf::TransformListener tfListener_;
 

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

@@ -66,6 +66,9 @@ public:
 		decimation_(1),
 		noiseFilterRadius_(0.0),
 		noiseFilterMinNeighbors_(5),
+		cut_left_(0),
+		cut_right_(0),
+		create_close_obstacle_if_depth_is_missing_(false),
 		approxSyncDepth_(0),
 		approxSyncDisparity_(0),
 		exactSyncDepth_(0),
@@ -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_;