0.13.3: scan2d normal support

src/CoreWrapper.cpp

@@ -36,7 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <std_msgs/Bool.h>
 #include <sensor_msgs/image_encodings.h>
 #include <cv_bridge/cv_bridge.h>
-#include <pcl/io/pcd_io.h>
+#include <pcl/io/io.h>
 
 #include <visualization_msgs/MarkerArray.h>
 
@@ -50,6 +50,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/util2d.h>
 #include <rtabmap/core/util3d.h>
 #include <rtabmap/core/util3d_transforms.h>
+#include <rtabmap/core/util3d_surface.h>
 #include <rtabmap/core/Memory.h>
 #include <rtabmap/core/OdometryEvent.h>
 #include <rtabmap/core/Version.h>
@@ -102,6 +103,7 @@ CoreWrapper::CoreWrapper() :
 		genScanMinDepth_(0.0),
 		scanCloudMaxPoints_(0),
 		scanCloudNormalK_(0),
+		scanCloudNormalRadius_(0.0f),
 		mapToOdom_(rtabmap::Transform::getIdentity()),
 		transformThread_(0),
 		tfThreadRunning_(false),
@@ -161,6 +163,7 @@ void CoreWrapper::onInit()
 	pnh.param("gen_scan_min_depth",  genScanMinDepth_, genScanMinDepth_);
 	pnh.param("scan_cloud_max_points",  scanCloudMaxPoints_, scanCloudMaxPoints_);
 	pnh.param("scan_cloud_normal_k", scanCloudNormalK_, scanCloudNormalK_);
+	pnh.param("scan_cloud_normal_radius", scanCloudNormalRadius_, scanCloudNormalRadius_);
 	pnh.param("stereo_to_depth", stereoToDepth_, stereoToDepth_);
 	pnh.param("odom_sensor_sync", odomSensorSync_, odomSensorSync_);
 	if(pnh.hasParam("flip_scan"))
@@ -977,17 +980,28 @@ void CoreWrapper::commonDepthCallbackImpl(
 
 	cv::Mat scan;
 	Transform scanLocalTransform = Transform::getIdentity();
-	pcl::PointCloud<pcl::PointXYZ> scanCloud2d;
 	bool genMaxScanPts = 0;
 	if(scan2dMsg.get() == 0 && scan3dMsg.get() == 0 && !depth.empty() && genScan_)
 	{
-		scanCloud2d = util3d::laserScanFromDepthImages(
+		pcl::PointCloud<pcl::PointXYZ>::Ptr scanCloud2d(new pcl::PointCloud<pcl::PointXYZ>);
+		*scanCloud2d = util3d::laserScanFromDepthImages(
 				depth,
 				cameraModels,
 				genScanMaxDepth_,
 				genScanMinDepth_);
 		genMaxScanPts += depth.cols;
-		scan = util3d::laserScan2dFromPointCloud(scanCloud2d);
+		if(scanCloudNormalK_ > 0 || scanCloudNormalRadius_>0.0f)
+		{
+			//compute normals
+			pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeFastOrganizedNormals2D(scanCloud2d, scanCloudNormalK_, scanCloudNormalRadius_);
+			pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
+			pcl::concatenateFields(*scanCloud2d, *normals, *pclScanNormal);
+			scan = rtabmap::util3d::laserScanFromPointCloud(*pclScanNormal);
+		}
+		else
+		{
+			scan = rtabmap::util3d::laserScan2dFromPointCloud(*scanCloud2d);
+		}
 	}
 	else if(scan2dMsg.get() != 0)
 	{
@@ -999,7 +1013,9 @@ void CoreWrapper::commonDepthCallbackImpl(
 				scan,
 				scanLocalTransform,
 				tfListener_,
-				waitForTransform_?waitForTransformDuration_:0))
+				waitForTransform_?waitForTransformDuration_:0,
+				scanCloudNormalK_,
+				scanCloudNormalRadius_))
 		{
 			NODELET_ERROR("Could not convert laser scan msg! Aborting rtabmap update...");
 			return;
@@ -1025,11 +1041,12 @@ void CoreWrapper::commonDepthCallbackImpl(
 				frameId_,
 				odomSensorSync_?odomFrameId:"",
 				lastPoseStamp_,
-				scanCloudNormalK_,
 				scan,
 				scanLocalTransform,
 				tfListener_,
-				waitForTransform_?waitForTransformDuration_:0))
+				waitForTransform_?waitForTransformDuration_:0,
+				scanCloudNormalK_,
+				scanCloudNormalRadius_))
 		{
 			NODELET_ERROR("Could not convert 3d laser scan msg! Aborting rtabmap update...");
 			return;
@@ -1058,8 +1075,6 @@ void CoreWrapper::commonDepthCallbackImpl(
 		userData_ = cv::Mat();
 	}
 
-
-
 	SensorData data(scan,
 			LaserScanInfo(
 					scan2dMsg.get() != 0?(int)scan2dMsg->ranges.size():(genScan_?genMaxScanPts:scan3dMsg.get() != 0?scanCloudMaxPoints_:0),
@@ -1241,7 +1256,9 @@ void CoreWrapper::commonStereoCallback(
 				scan,
 				scanLocalTransform,
 				tfListener_,
-				waitForTransform_?waitForTransformDuration_:0))
+				waitForTransform_?waitForTransformDuration_:0,
+				scanCloudNormalK_,
+				scanCloudNormalRadius_))
 		{
 			NODELET_ERROR("Could not convert laser scan msg! Aborting rtabmap update...");
 			return;
@@ -1267,11 +1284,12 @@ void CoreWrapper::commonStereoCallback(
 				frameId_,
 				odomSensorSync_?odomFrameId:"",
 				lastPoseStamp_,
-				scanCloudNormalK_,
 				scan,
 				scanLocalTransform,
 				tfListener_,
-				waitForTransform_?waitForTransformDuration_:0))
+				waitForTransform_?waitForTransformDuration_:0,
+				scanCloudNormalK_,
+				scanCloudNormalRadius_))
 		{
 			NODELET_ERROR("Could not convert 3d laser scan msg! Aborting rtabmap update...");
 			return;

src/GuiWrapper.cpp

@@ -542,7 +542,6 @@ void GuiWrapper::commonDepthCallback(
 					frameId_,
 					odomSensorSync_?odomHeader.frame_id:"",
 					odomHeader.stamp,
-					0,
 					scan,
 					scanLocalTransform,
 					tfListener_,
@@ -698,7 +697,6 @@ void GuiWrapper::commonStereoCallback(
 					frameId_,
 					odomSensorSync_?odomHeader.frame_id:"",
 					odomHeader.stamp,
-					0,
 					scan,
 					scanLocalTransform,
 					tfListener_,

src/MsgConversion.cpp

@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <zlib.h>
 #include <ros/ros.h>
 #include <rtabmap/core/util3d.h>
+#include <rtabmap/core/util3d_filtering.h>
 #include <rtabmap/core/Compression.h>
 #include <rtabmap/utilite/UStl.h>
 #include <rtabmap/utilite/ULogger.h>
@@ -1353,7 +1354,9 @@ bool convertScanMsg(
 		cv::Mat & scan,
 		rtabmap::Transform & scanLocalTransform,
 		tf::TransformListener & listener,
-		double waitForTransform)
+		double waitForTransform,
+		int scanCloudNormalK,
+		float scanCloudNormalRadius)
 {
 	// make sure the frame of the laser is updated too
 	rtabmap::Transform tmpT = getTransform(
@@ -1418,7 +1421,21 @@ bool convertScanMsg(
 			scanLocalTransform = sensorT * scanLocalTransform;
 		}
 	}
-	scan = rtabmap::util3d::laserScan2dFromPointCloud(*pclScan, laserToOdom); // put back in laser frame
+
+	if(scanCloudNormalK > 0 || scanCloudNormalRadius>0.0f)
+	{
+		//compute normals
+		pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeFastOrganizedNormals2D(pclScan, scanCloudNormalK, scanCloudNormalRadius);
+		pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
+		pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
+		scan = rtabmap::util3d::laserScanFromPointCloud(*rtabmap::util3d::removeNaNNormalsFromPointCloud(pclScanNormal), laserToOdom); // put back in laser frame
+	}
+	else
+	{
+		scan = rtabmap::util3d::laserScan2dFromPointCloud(*pclScan, laserToOdom); // put back in laser frame
+	}
+
+
 	return true;
 }
 
@@ -1427,11 +1444,12 @@ bool convertScan3dMsg(
 		const std::string & frameId,
 		const std::string & odomFrameId,
 		const ros::Time & odomStamp,
-		int scanCloudNormalK,
 		cv::Mat & scan,
 		rtabmap::Transform & scanLocalTransform,
 		tf::TransformListener & listener,
-		double waitForTransform)
+		double waitForTransform,
+		int scanCloudNormalK,
+		float scanCloudNormalRadius)
 {
 	bool containNormals = false;
 	for(unsigned int i=0; i<scan3dMsg->fields.size(); ++i)
@@ -1483,13 +1501,13 @@ bool convertScan3dMsg(
 		pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
 		pcl::fromROSMsg(*scan3dMsg, *pclScan);
 
-		if(scanCloudNormalK > 0)
+		if(scanCloudNormalK > 0 || scanCloudNormalRadius>0.0f)
 		{
 			//compute normals
-			pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(pclScan, scanCloudNormalK);
+			pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(pclScan, scanCloudNormalK, scanCloudNormalRadius);
 			pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
 			pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
-			scan = rtabmap::util3d::laserScanFromPointCloud(*pclScanNormal);
+			scan = rtabmap::util3d::laserScanFromPointCloud(*rtabmap::util3d::removeNaNNormalsFromPointCloud(pclScanNormal));
 		}
 		else
 		{

src/nodelets/icp_odometry.cpp

@@ -57,7 +57,8 @@ public:
 	ICPOdometry() :
 		OdometryROS(false, false, true),
 		scanCloudMaxPoints_(0),
-		scanCloudNormalK_(0)
+		scanCloudNormalK_(0),
+		scanCloudNormalRadius_(0.0f)
 	{
 	}
 
@@ -74,6 +75,7 @@ private:
 
 		pnh.param("scan_cloud_max_points",  scanCloudMaxPoints_, scanCloudMaxPoints_);
 		pnh.param("scan_cloud_normal_k", scanCloudNormalK_, scanCloudNormalK_);
+		pnh.param("scan_cloud_normal_k", scanCloudNormalRadius_, scanCloudNormalRadius_);
 
 		scan_sub_ = nh.subscribe("scan", 1, &ICPOdometry::callbackScan, this);
 		cloud_sub_ = nh.subscribe("scan_cloud", 1, &ICPOdometry::callbackCloud, this);
@@ -109,7 +111,19 @@ private:
 		pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
 		pcl::fromROSMsg(scanOut, *pclScan);
 
-		cv::Mat scan = util3d::laserScan2dFromPointCloud(*pclScan);
+		cv::Mat scan;
+		if(scanCloudNormalK_ > 0 || scanCloudNormalRadius_>0.0f)
+		{
+			//compute normals
+			pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeFastOrganizedNormals2D(pclScan, scanCloudNormalK_, scanCloudNormalRadius_);
+			pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
+			pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
+			scan = util3d::laserScanFromPointCloud(*pclScanNormal);
+		}
+		else
+		{
+			scan = util3d::laserScan2dFromPointCloud(*pclScan);
+		}
 
 		rtabmap::SensorData data(
 				scan,
@@ -154,10 +168,10 @@ private:
 			pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
 			pcl::fromROSMsg(*cloudMsg, *pclScan);
 
-			if(scanCloudNormalK_ > 0)
+			if(scanCloudNormalK_ > 0 || scanCloudNormalRadius_>0.0f)
 			{
 				//compute normals
-				pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(pclScan, scanCloudNormalK_);
+				pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(pclScan, scanCloudNormalK_, scanCloudNormalRadius_);
 				pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
 				pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
 				scan = util3d::laserScanFromPointCloud(*pclScanNormal);
@@ -191,6 +205,7 @@ private:
 	ros::Subscriber cloud_sub_;
 	int scanCloudMaxPoints_;
 	int scanCloudNormalK_;
+	float scanCloudNormalRadius_;
 };
 
 PLUGINLIB_EXPORT_CLASS(rtabmap_ros::ICPOdometry, nodelet::Nodelet);

src/nodelets/point_cloud_xyz.cpp

@@ -53,9 +53,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <cv_bridge/cv_bridge.h>
 #include <opencv2/highgui/highgui.hpp>
 
+#include "rtabmap/core/util2d.h"
 #include "rtabmap/core/util3d.h"
 #include "rtabmap/core/util3d_filtering.h"
-#include "rtabmap/core/Features2d.h"
+#include "rtabmap/core/util3d_surface.h"
 #include "rtabmap/utilite/UConversion.h"
 #include "rtabmap/utilite/UStl.h"
 
@@ -72,6 +73,8 @@ public:
 		decimation_(1),
 		noiseFilterRadius_(0.0),
 		noiseFilterMinNeighbors_(5),
+		normalK_(0),
+		normalRadius_(0.0f),
 		approxSyncDepth_(0),
 		approxSyncDisparity_(0),
 		exactSyncDepth_(0),
@@ -107,6 +110,8 @@ private:
 		pnh.param("decimation", decimation_, decimation_);
 		pnh.param("noise_filter_radius", noiseFilterRadius_, noiseFilterRadius_);
 		pnh.param("noise_filter_min_neighbors", noiseFilterMinNeighbors_, noiseFilterMinNeighbors_);
+		pnh.param("normal_k", normalK_, normalK_);
+		pnh.param("normal_radius", normalRadius_, normalRadius_);
 		pnh.param("roi_ratios", roiStr, roiStr);
 
 		// Deprecated
@@ -208,7 +213,7 @@ private:
 			ros::WallTime time = ros::WallTime::now();
 
 			cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(depth);
-			cv::Rect roi = rtabmap::Feature2D::computeRoi(imageDepthPtr->image, roiRatios_);
+			cv::Rect roi = rtabmap::util2d::computeRoi(imageDepthPtr->image, roiRatios_);
 
 			image_geometry::PinholeCameraModel model;
 			model.fromCameraInfo(*cameraInfo);
@@ -259,7 +264,7 @@ private:
 		{
 			ros::WallTime time = ros::WallTime::now();
 
-			cv::Rect roi = rtabmap::Feature2D::computeRoi(disparity, roiRatios_);
+			cv::Rect roi = rtabmap::util2d::computeRoi(disparity, roiRatios_);
 
 			pcl::PointCloud<pcl::PointXYZ>::Ptr pclCloud;
 			rtabmap::CameraModel leftModel = rtabmap_ros::cameraModelFromROS(*cameraInfo);
@@ -303,7 +308,18 @@ private:
 		}
 
 		sensor_msgs::PointCloud2 rosCloud;
-		pcl::toROSMsg(*pclCloud, rosCloud);
+		if(pclCloud->size() && (normalK_ > 0 || noiseFilterRadius_ > 0.0f))
+		{
+			//compute normals
+			pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(pclCloud, normalK_, normalRadius_);
+			pcl::PointCloud<pcl::PointNormal>::Ptr pclCloudNormal(new pcl::PointCloud<pcl::PointNormal>);
+			pcl::concatenateFields(*pclCloud, *normals, *pclCloudNormal);
+			pcl::toROSMsg(*pclCloudNormal, rosCloud);
+		}
+		else
+		{
+			pcl::toROSMsg(*pclCloud, rosCloud);
+		}
 		rosCloud.header.stamp = header.stamp;
 		rosCloud.header.frame_id = header.frame_id;
 
@@ -319,6 +335,8 @@ private:
 	int decimation_;
 	double noiseFilterRadius_;
 	int noiseFilterMinNeighbors_;
+	int normalK_;
+	float normalRadius_;
 	std::vector<float> roiRatios_;
 
 	ros::Publisher cloudPub_;

src/nodelets/point_cloud_xyzrgb.cpp

@@ -40,6 +40,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <sensor_msgs/image_encodings.h>
 #include <sensor_msgs/CameraInfo.h>
 
+#include <stereo_msgs/DisparityImage.h>
+
 #include <image_transport/image_transport.h>
 #include <image_transport/subscriber_filter.h>
 
@@ -53,9 +55,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <cv_bridge/cv_bridge.h>
 #include <opencv2/highgui/highgui.hpp>
 
+#include "rtabmap/core/util2d.h"
 #include "rtabmap/core/util3d.h"
 #include "rtabmap/core/util3d_filtering.h"
-#include "rtabmap/core/Features2d.h"
+#include "rtabmap/core/util3d_surface.h"
 #include "rtabmap/utilite/UConversion.h"
 #include "rtabmap/utilite/UStl.h"
 
@@ -72,6 +75,8 @@ public:
 		decimation_(1),
 		noiseFilterRadius_(0.0),
 		noiseFilterMinNeighbors_(5),
+		normalK_(0),
+		normalRadius_(0.0f),
 		approxSyncDepth_(0),
 		approxSyncStereo_(0),
 		exactSyncDepth_(0),
@@ -107,6 +112,8 @@ private:
 		pnh.param("decimation", decimation_, decimation_);
 		pnh.param("noise_filter_radius", noiseFilterRadius_, noiseFilterRadius_);
 		pnh.param("noise_filter_min_neighbors", noiseFilterMinNeighbors_, noiseFilterMinNeighbors_);
+		pnh.param("normal_k", normalK_, normalK_);
+		pnh.param("normal_radius", normalRadius_, normalRadius_);
 		pnh.param("roi_ratios", roiStr, roiStr);
 
 		//parse roi (region of interest)
@@ -142,6 +149,36 @@ private:
 			}
 		}
 
+		// StereoBM parameters
+		stereoBMParameters_ = rtabmap::Parameters::getDefaultParameters("StereoBM");
+		for(rtabmap::ParametersMap::iterator iter=stereoBMParameters_.begin(); iter!=stereoBMParameters_.end(); ++iter)
+		{
+			std::string vStr;
+			bool vBool;
+			int vInt;
+			double vDouble;
+			if(pnh.getParam(iter->first, vStr))
+			{
+				NODELET_INFO("point_cloud_xyzrgb: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str());
+				iter->second = vStr;
+			}
+			else if(pnh.getParam(iter->first, vBool))
+			{
+				NODELET_INFO("point_cloud_xyzrgb: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), uBool2Str(vBool).c_str());
+				iter->second = uBool2Str(vBool);
+			}
+			else if(pnh.getParam(iter->first, vDouble))
+			{
+				NODELET_INFO("point_cloud_xyzrgb: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble).c_str());
+				iter->second = uNumber2Str(vDouble);
+			}
+			else if(pnh.getParam(iter->first, vInt))
+			{
+				NODELET_INFO("point_cloud_xyzrgb: Setting parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vInt).c_str());
+				iter->second = uNumber2Str(vInt);
+			}
+		}
+
 		NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false");
 
 		cloudPub_ = nh.advertise<sensor_msgs::PointCloud2>("cloud", 1);
@@ -152,6 +189,9 @@ private:
 			approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
 			approxSyncDepth_->registerCallback(boost::bind(&PointCloudXYZRGB::depthCallback, this, _1, _2, _3));
 
+			approxSyncDisparity_ = new message_filters::Synchronizer<MyApproxSyncDisparityPolicy>(MyApproxSyncDisparityPolicy(queueSize), imageLeft_, imageDisparitySub_, cameraInfoLeft_);
+			approxSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZRGB::disparityCallback, this, _1, _2, _3));
+
 			approxSyncStereo_ = new message_filters::Synchronizer<MyApproxSyncStereoPolicy>(MyApproxSyncStereoPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
 			approxSyncStereo_->registerCallback(boost::bind(&PointCloudXYZRGB::stereoCallback, this, _1, _2, _3, _4));
 		}
@@ -160,6 +200,9 @@ private:
 			exactSyncDepth_ = new message_filters::Synchronizer<MyExactSyncDepthPolicy>(MyExactSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
 			exactSyncDepth_->registerCallback(boost::bind(&PointCloudXYZRGB::depthCallback, this, _1, _2, _3));
 
+			exactSyncDisparity_ = new message_filters::Synchronizer<MyExactSyncDisparityPolicy>(MyExactSyncDisparityPolicy(queueSize), imageLeft_, imageDisparitySub_, cameraInfoLeft_);
+			exactSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZRGB::disparityCallback, this, _1, _2, _3));
+
 			exactSyncStereo_ = new message_filters::Synchronizer<MyExactSyncStereoPolicy>(MyExactSyncStereoPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
 			exactSyncStereo_->registerCallback(boost::bind(&PointCloudXYZRGB::stereoCallback, this, _1, _2, _3, _4));
 		}
@@ -177,7 +220,6 @@ private:
 		imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
 		cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
 
-
 		ros::NodeHandle left_nh(nh, "left");
 		ros::NodeHandle right_nh(nh, "right");
 		ros::NodeHandle left_pnh(pnh, "left");
@@ -187,6 +229,8 @@ private:
 		image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh);
 		image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh);
 
+		imageDisparitySub_.subscribe(nh, "disparity", 1);
+
 		imageLeft_.subscribe(left_it, left_nh.resolveName("image"), 1, hintsLeft);
 		imageRight_.subscribe(right_it, right_nh.resolveName("image"), 1, hintsRight);
 		cameraInfoLeft_.subscribe(left_nh, "camera_info", 1);
@@ -242,7 +286,7 @@ private:
 			ROS_ASSERT(imageDepthPtr->image.rows == imagePtr->image.rows);
 
 			pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
-			cv::Rect roi = rtabmap::Feature2D::computeRoi(imageDepthPtr->image, roiRatios_);
+			cv::Rect roi = rtabmap::util2d::computeRoi(imageDepthPtr->image, roiRatios_);
 
 			rtabmap::CameraModel m(
 					model.fx(),
@@ -266,6 +310,68 @@ private:
 		}
 	}
 
+	void disparityCallback(
+			const sensor_msgs::ImageConstPtr& image,
+			const stereo_msgs::DisparityImageConstPtr& imageDisparity,
+			const sensor_msgs::CameraInfoConstPtr& cameraInfo)
+	{
+		cv_bridge::CvImageConstPtr imagePtr;
+		if(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0)
+		{
+			imagePtr = cv_bridge::toCvShare(image);
+		}
+		else if(image->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
+				image->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
+		{
+			imagePtr = cv_bridge::toCvShare(image, "mono8");
+		}
+		else
+		{
+			imagePtr = cv_bridge::toCvShare(image, "bgr8");
+		}
+
+		if(imageDisparity->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) !=0 &&
+		   imageDisparity->image.encoding.compare(sensor_msgs::image_encodings::TYPE_16SC1) !=0)
+		{
+			NODELET_ERROR("Input type must be disparity=32FC1 or 16SC1");
+			return;
+		}
+
+		cv::Mat disparity;
+		if(imageDisparity->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0)
+		{
+			disparity = cv::Mat(imageDisparity->image.height, imageDisparity->image.width, CV_32FC1, const_cast<uchar*>(imageDisparity->image.data.data()));
+		}
+		else
+		{
+			disparity = cv::Mat(imageDisparity->image.height, imageDisparity->image.width, CV_16SC1, const_cast<uchar*>(imageDisparity->image.data.data()));
+		}
+
+		if(cloudPub_.getNumSubscribers())
+		{
+			ros::WallTime time = ros::WallTime::now();
+
+			cv::Rect roi = rtabmap::util2d::computeRoi(disparity, roiRatios_);
+
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
+			rtabmap::CameraModel leftModel = rtabmap_ros::cameraModelFromROS(*cameraInfo);
+			rtabmap::StereoCameraModel stereoModel(imageDisparity->f, imageDisparity->f, leftModel.cx()-roiRatios_[0]*double(disparity.cols), leftModel.cy()-roiRatios_[2]*double(disparity.rows), imageDisparity->T);
+			pcl::IndicesPtr indices(new std::vector<int>);
+			pclCloud = rtabmap::util3d::cloudFromDisparityRGB(
+					cv::Mat(imagePtr->image, roi),
+					cv::Mat(disparity, roi),
+					stereoModel,
+					decimation_,
+					maxDepth_,
+					minDepth_,
+					indices.get());
+
+			processAndPublish(pclCloud, indices, imageDisparity->header);
+
+			NODELET_DEBUG("point_cloud_xyzrgb from disparity time = %f s", (ros::WallTime::now() - time).toSec());
+		}
+	}
+
 	void stereoCallback(const sensor_msgs::ImageConstPtr& imageLeft,
 			const sensor_msgs::ImageConstPtr& imageRight,
 			const sensor_msgs::CameraInfoConstPtr& camInfoLeft,
@@ -314,7 +420,8 @@ private:
 					decimation_,
 					maxDepth_,
 					minDepth_,
-					indices.get());
+					indices.get(),
+					stereoBMParameters_);
 
 			processAndPublish(pclCloud, indices, imageLeft->header);
 
@@ -346,7 +453,18 @@ private:
 		}
 
 		sensor_msgs::PointCloud2 rosCloud;
-		pcl::toROSMsg(*pclCloud, rosCloud);
+		if(pclCloud->size() && (normalK_ > 0 || noiseFilterRadius_ > 0.0f))
+		{
+			//compute normals
+			pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(pclCloud, normalK_, normalRadius_);
+			pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pclCloudNormal(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+			pcl::concatenateFields(*pclCloud, *normals, *pclCloudNormal);
+			pcl::toROSMsg(*pclCloudNormal, rosCloud);
+		}
+		else
+		{
+			pcl::toROSMsg(*pclCloud, rosCloud);
+		}
 		rosCloud.header.stamp = header.stamp;
 		rosCloud.header.frame_id = header.frame_id;
 
@@ -362,7 +480,10 @@ private:
 	int decimation_;
 	double noiseFilterRadius_;
 	int noiseFilterMinNeighbors_;
+	int normalK_;
+	float normalRadius_;
 	std::vector<float> roiRatios_;
+	rtabmap::ParametersMap stereoBMParameters_;
 
 	ros::Publisher cloudPub_;
 
@@ -370,6 +491,8 @@ private:
 	image_transport::SubscriberFilter imageDepthSub_;
 	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
 
+	message_filters::Subscriber<stereo_msgs::DisparityImage> imageDisparitySub_;
+
 	image_transport::SubscriberFilter imageLeft_;
 	image_transport::SubscriberFilter imageRight_;
 	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoLeft_;
@@ -378,12 +501,18 @@ private:
 	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyApproxSyncDepthPolicy;
 	message_filters::Synchronizer<MyApproxSyncDepthPolicy> * approxSyncDepth_;
 
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, stereo_msgs::DisparityImage, sensor_msgs::CameraInfo> MyApproxSyncDisparityPolicy;
+	message_filters::Synchronizer<MyApproxSyncDisparityPolicy> * approxSyncDisparity_;
+
 	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyApproxSyncStereoPolicy;
 	message_filters::Synchronizer<MyApproxSyncStereoPolicy> * approxSyncStereo_;
 
 	typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncDepthPolicy;
 	message_filters::Synchronizer<MyExactSyncDepthPolicy> * exactSyncDepth_;
 
+	typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, stereo_msgs::DisparityImage, sensor_msgs::CameraInfo> MyExactSyncDisparityPolicy;
+	message_filters::Synchronizer<MyExactSyncDisparityPolicy> * exactSyncDisparity_;
+
 	typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyExactSyncStereoPolicy;
 	message_filters::Synchronizer<MyExactSyncStereoPolicy> * exactSyncStereo_;
 };

src/nodelets/rgbdicp_odometry.cpp

@@ -73,7 +73,8 @@ public:
 		exactCloudSync_(0),
 		queueSize_(5),
 		scanCloudMaxPoints_(0),
-		scanCloudNormalK_(0)
+		scanCloudNormalK_(0),
+		scanCloudNormalRadius_(0.0f)
 	{
 	}
 
@@ -111,6 +112,7 @@ private:
 		pnh.param("subscribe_scan_cloud", subscribeScanCloud, subscribeScanCloud);
 		pnh.param("scan_cloud_max_points",  scanCloudMaxPoints_, scanCloudMaxPoints_);
 		pnh.param("scan_cloud_normal_k", scanCloudNormalK_, scanCloudNormalK_);
+		pnh.param("scan_cloud_normal_radius", scanCloudNormalRadius_, scanCloudNormalRadius_);
 
 		ros::NodeHandle rgb_nh(nh, "rgb");
 		ros::NodeHandle depth_nh(nh, "depth");
@@ -292,7 +294,18 @@ private:
 					pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
 					pcl::fromROSMsg(scanOut, *pclScan);
 
-					scan = util3d::laserScan2dFromPointCloud(*pclScan);
+					if(scanCloudNormalK_ > 0 || scanCloudNormalRadius_>0.0f)
+					{
+						//compute normals
+						pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeFastOrganizedNormals2D(pclScan, scanCloudNormalK_, scanCloudNormalRadius_);
+						pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
+						pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
+						scan = util3d::laserScanFromPointCloud(*pclScanNormal);
+					}
+					else
+					{
+						scan = util3d::laserScan2dFromPointCloud(*pclScan);
+					}
 				}
 				else if(cloudMsg.get() != 0)
 				{
@@ -323,10 +336,10 @@ private:
 						pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
 						pcl::fromROSMsg(*cloudMsg, *pclScan);
 
-						if(scanCloudNormalK_ > 0)
+						if(scanCloudNormalK_ > 0 || scanCloudNormalRadius_>0.0f)
 						{
 							//compute normals
-							pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(pclScan, scanCloudNormalK_);
+							pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(pclScan, scanCloudNormalK_, scanCloudNormalRadius_);
 							pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
 							pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
 							scan = util3d::laserScanFromPointCloud(*pclScanNormal);
@@ -402,6 +415,7 @@ private:
 	int queueSize_;
 	int scanCloudMaxPoints_;
 	int scanCloudNormalK_;
+	float scanCloudNormalRadius_;
 };
 
 PLUGINLIB_EXPORT_CLASS(rtabmap_ros::RGBDICPOdometry, nodelet::Nodelet);