rtabmap: scan_cloud_normal_k and scan_normal_radius removed. odometry: added guess_min_translation, guess_min_rotation and scan_voxel_size parameters. data_recorder.launch: updated to support rgbd_image and scan_cloud inputs.

src/CoreWrapper.cpp

@@ -102,8 +102,6 @@ CoreWrapper::CoreWrapper() :
 		genScanMaxDepth_(4.0),
 		genScanMinDepth_(0.0),
 		scanCloudMaxPoints_(0),
-		scanCloudNormalK_(0),
-		scanCloudNormalRadius_(0.0f),
 		mapToOdom_(rtabmap::Transform::getIdentity()),
 		transformThread_(0),
 		tfThreadRunning_(false),
@@ -162,14 +160,16 @@ void CoreWrapper::onInit()
 	pnh.param("gen_scan_max_depth",  genScanMaxDepth_, genScanMaxDepth_);
 	pnh.param("gen_scan_min_depth",  genScanMinDepth_, genScanMinDepth_);
 	pnh.param("scan_cloud_max_points",  scanCloudMaxPoints_, scanCloudMaxPoints_);
-	pnh.param("scan_normal_k", scanCloudNormalK_, scanCloudNormalK_);
-	if(pnh.hasParam("scan_cloud_normal_k") && !pnh.hasParam("scan_normal_k"))
+	if(pnh.hasParam("scan_cloud_normal_k"))
 	{
-		ROS_WARN("rtabmap: Parameter \"scan_cloud_normal_k\" has been renamed to \"scan_normal_k\". "
-				"The value is still used. Use \"scan_normal_k\" to avoid this warning.");
-		pnh.param("scan_cloud_normal_k", scanCloudNormalK_, scanCloudNormalK_);
+		ROS_WARN("rtabmap: Parameter \"scan_cloud_normal_k\" has been removed. RTAB-Map's parameter \"%s\" should be used instead. "
+				"The value is copied. Use \"%s\" to avoid this warning.",
+				Parameters::kMemLaserScanNormalK().c_str(),
+				Parameters::kMemLaserScanNormalK().c_str());
+		double value;
+		pnh.getParam("scan_cloud_normal_k", value);
+		uInsert(parameters_, ParametersPair(Parameters::kMemLaserScanNormalK(), uNumber2Str(value)));
 	}
-	pnh.param("scan_normal_radius", scanCloudNormalRadius_, scanCloudNormalRadius_);
 	pnh.param("stereo_to_depth", stereoToDepth_, stereoToDepth_);
 	pnh.param("odom_sensor_sync", odomSensorSync_, odomSensorSync_);
 	if(pnh.hasParam("flip_scan"))
@@ -996,18 +996,7 @@ void CoreWrapper::commonDepthCallbackImpl(
 				genScanMaxDepth_,
 				genScanMinDepth_);
 		genMaxScanPts += depth.cols;
-		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::laserScan2dFromPointCloud(*pclScanNormal);
-		}
-		else
-		{
-			scan = rtabmap::util3d::laserScan2dFromPointCloud(*scanCloud2d);
-		}
+		scan = rtabmap::util3d::laserScan2dFromPointCloud(*scanCloud2d);
 	}
 	else if(scan2dMsg.get() != 0)
 	{
@@ -1019,9 +1008,7 @@ void CoreWrapper::commonDepthCallbackImpl(
 				scan,
 				scanLocalTransform,
 				tfListener_,
-				waitForTransform_?waitForTransformDuration_:0,
-				scanCloudNormalK_,
-				scanCloudNormalRadius_))
+				waitForTransform_?waitForTransformDuration_:0))
 		{
 			NODELET_ERROR("Could not convert laser scan msg! Aborting rtabmap update...");
 			return;
@@ -1050,9 +1037,7 @@ void CoreWrapper::commonDepthCallbackImpl(
 				scan,
 				scanLocalTransform,
 				tfListener_,
-				waitForTransform_?waitForTransformDuration_:0,
-				scanCloudNormalK_,
-				scanCloudNormalRadius_))
+				waitForTransform_?waitForTransformDuration_:0))
 		{
 			NODELET_ERROR("Could not convert 3d laser scan msg! Aborting rtabmap update...");
 			return;
@@ -1262,9 +1247,7 @@ void CoreWrapper::commonStereoCallback(
 				scan,
 				scanLocalTransform,
 				tfListener_,
-				waitForTransform_?waitForTransformDuration_:0,
-				scanCloudNormalK_,
-				scanCloudNormalRadius_))
+				waitForTransform_?waitForTransformDuration_:0))
 		{
 			NODELET_ERROR("Could not convert laser scan msg! Aborting rtabmap update...");
 			return;
@@ -1293,9 +1276,7 @@ void CoreWrapper::commonStereoCallback(
 				scan,
 				scanLocalTransform,
 				tfListener_,
-				waitForTransform_?waitForTransformDuration_:0,
-				scanCloudNormalK_,
-				scanCloudNormalRadius_))
+				waitForTransform_?waitForTransformDuration_:0))
 		{
 			NODELET_ERROR("Could not convert 3d laser scan msg! Aborting rtabmap update...");
 			return;

src/MsgConversion.cpp

@@ -1360,9 +1360,7 @@ bool convertScanMsg(
 		cv::Mat & scan,
 		rtabmap::Transform & scanLocalTransform,
 		tf::TransformListener & listener,
-		double waitForTransform,
-		int scanCloudNormalK,
-		float scanCloudNormalRadius)
+		double waitForTransform)
 {
 	// make sure the frame of the laser is updated too
 	rtabmap::Transform tmpT = getTransform(
@@ -1393,6 +1391,7 @@ bool convertScanMsg(
 	projection.transformLaserScanToPointCloud(odomFrameId.empty()?frameId:odomFrameId, *scan2dMsg, scanOut, listener);
 	pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
 	pcl::fromROSMsg(scanOut, *pclScan);
+	pclScan->is_dense = true;
 
 	//transform back in laser frame
 	rtabmap::Transform laserToOdom = getTransform(
@@ -1428,19 +1427,7 @@ bool convertScanMsg(
 		}
 	}
 
-	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::laserScan2dFromPointCloud(*pclScanNormal, laserToOdom); // put back in laser frame
-	}
-	else
-	{
-		scan = rtabmap::util3d::laserScan2dFromPointCloud(*pclScan, laserToOdom); // put back in laser frame
-	}
-
+	scan = rtabmap::util3d::laserScan2dFromPointCloud(*pclScan, laserToOdom); // put back in laser frame
 
 	return true;
 }
@@ -1453,9 +1440,7 @@ bool convertScan3dMsg(
 		cv::Mat & scan,
 		rtabmap::Transform & scanLocalTransform,
 		tf::TransformListener & listener,
-		double waitForTransform,
-		int scanCloudNormalK,
-		float scanCloudNormalRadius)
+		double waitForTransform)
 {
 	bool containNormals = false;
 	bool containColors = false;
@@ -1533,18 +1518,7 @@ bool convertScan3dMsg(
 				pclScan = rtabmap::util3d::removeNaNFromPointCloud(pclScan);
 			}
 
-			if(scanCloudNormalK > 0 || scanCloudNormalRadius>0.0f)
-			{
-				//compute normals
-				pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(pclScan, scanCloudNormalK, scanCloudNormalRadius);
-				pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
-				pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
-				scan = rtabmap::util3d::laserScanFromPointCloud(*rtabmap::util3d::removeNaNNormalsFromPointCloud(pclScanNormal));
-			}
-			else
-			{
-				scan = rtabmap::util3d::laserScanFromPointCloud(*pclScan);
-			}
+			scan = rtabmap::util3d::laserScanFromPointCloud(*pclScan);
 		}
 		else
 		{
@@ -1555,18 +1529,7 @@ bool convertScan3dMsg(
 				pclScan = rtabmap::util3d::removeNaNFromPointCloud(pclScan);
 			}
 
-			if(scanCloudNormalK > 0 || scanCloudNormalRadius>0.0f)
-			{
-				//compute normals
-				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(*rtabmap::util3d::removeNaNNormalsFromPointCloud(pclScanNormal));
-			}
-			else
-			{
-				scan = rtabmap::util3d::laserScanFromPointCloud(*pclScan);
-			}
+			scan = rtabmap::util3d::laserScanFromPointCloud(*pclScan);
 		}
 	}
 	return true;

src/OdometryROS.cpp

@@ -49,6 +49,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/utilite/UStl.h"
 #include "rtabmap/utilite/UFile.h"
+#include "rtabmap/utilite/UMath.h"
 
 #define BAD_COVARIANCE 9999
 
@@ -65,6 +66,8 @@ OdometryROS::OdometryROS(bool stereoParams, bool visParams, bool icpParams) :
 	groundTruthFrameId_(""),
 	groundTruthBaseFrameId_(""),
 	guessFrameId_(""),
+	guessMinTranslation_(0.0),
+	guessMinRotation_(0.0),
 	publishTf_(true),
 	waitForTransform_(true),
 	waitForTransformDuration_(0.1), // 100 ms
@@ -139,6 +142,8 @@ void OdometryROS::onInit()
 		}
 	}
 	pnh.param("guess_frame_id", guessFrameId_, guessFrameId_); // odometry guess frame
+	pnh.param("guess_min_translation", guessMinTranslation_, guessMinTranslation_);
+	pnh.param("guess_min_rotation", guessMinRotation_, guessMinRotation_);
 
 	if(publishTf_ && !guessFrameId_.empty() && guessFrameId_.compare(odomFrameId_) == 0)
 	{
@@ -147,6 +152,19 @@ void OdometryROS::onInit()
 				"are the same frame (value=\"%s\"). \"guess_frame_id\" is disabled.", odomFrameId_.c_str());
 		guessFrameId_.clear();
 	}
+	NODELET_INFO("Odometry: frame_id               = %s", frameId_.c_str());
+	NODELET_INFO("Odometry: odom_frame_id          = %s", odomFrameId_.c_str());
+	NODELET_INFO("Odometry: publish_tf             = %s", publishTf_?"true":"false");
+	NODELET_INFO("Odometry: wait_for_transform     = %s", waitForTransform_?"true":"false");
+	NODELET_INFO("Odometry: wait_for_transform_duration  = %f", waitForTransformDuration_);
+	NODELET_INFO("Odometry: initial_pose           = %s", initialPose.prettyPrint().c_str());
+	NODELET_INFO("Odometry: ground_truth_frame_id  = %s", groundTruthFrameId_.c_str());
+	NODELET_INFO("Odometry: ground_truth_base_frame_id = %s", groundTruthBaseFrameId_.c_str());
+	NODELET_INFO("Odometry: config_path            = %s", configPath.c_str());
+	NODELET_INFO("Odometry: publish_null_when_lost = %s", publishNullWhenLost_?"true":"false");
+	NODELET_INFO("Odometry: guess_frame_id         = %s", guessFrameId_.c_str());
+	NODELET_INFO("Odometry: guess_min_translation  = %f", guessMinTranslation_);
+	NODELET_INFO("Odometry: guess_min_rotation     = %f", guessMinRotation_);
 
 	configPath = uReplaceChar(configPath, '~', UDirectory::homeDir());
 	if(configPath.size() && configPath.at(0) != '/')
@@ -388,6 +406,28 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 		if(!previousPose.isNull() && !guessCurrentPose.isNull())
 		{
 			guess = previousPose.inverse() * guessCurrentPose;
+
+			if(odometry_->previousStamp()>0.0 && (guessMinTranslation_ > 0.0 || guessMinRotation_ > 0.0))
+			{
+				float x,y,z,roll,pitch,yaw;
+				guess.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
+				if((guessMinTranslation_ <= 0.0 || uMax3(fabs(x), fabs(y), fabs(z)) < guessMinTranslation_) &&
+				   (guessMinRotation_ <= 0.0 || uMax3(fabs(roll), fabs(pitch), fabs(yaw)) < guessMinRotation_))
+				{
+					// Ignore odometry update, we didn't move enough
+					if(publishTf_)
+					{
+						geometry_msgs::TransformStamped correctionMsg;
+						correctionMsg.child_frame_id = guessFrameId_;
+						correctionMsg.header.frame_id = odomFrameId_;
+						correctionMsg.header.stamp = stamp;
+						Transform correction = odometry_->getPose() * guess * guessCurrentPose.inverse();
+						rtabmap_ros::transformToGeometryMsg(correction, correctionMsg.transform);
+						tfBroadcaster_.sendTransform(correctionMsg);
+					}
+					return;
+				}
+			}
 		}
 		else
 		{

src/nodelets/icp_odometry.cpp

@@ -58,8 +58,9 @@ public:
 	ICPOdometry() :
 		OdometryROS(false, false, true),
 		scanCloudMaxPoints_(0),
-		scanCloudNormalK_(0),
-		scanCloudNormalRadius_(0.0f)
+		scanVoxelSize_(0.0f),
+		scanNormalK_(0),
+		scanNormalRadius_(0.0f)
 	{
 	}
 
@@ -75,14 +76,20 @@ private:
 		ros::NodeHandle & pnh = getPrivateNodeHandle();
 
 		pnh.param("scan_cloud_max_points",  scanCloudMaxPoints_, scanCloudMaxPoints_);
-		pnh.param("scan_normal_k", scanCloudNormalK_, scanCloudNormalK_);
+		pnh.param("scan_voxel_size", scanVoxelSize_, scanVoxelSize_);
+		pnh.param("scan_normal_k", scanNormalK_, scanNormalK_);
 		if(pnh.hasParam("scan_cloud_normal_k") && !pnh.hasParam("scan_normal_k"))
 		{
 			ROS_WARN("rtabmap: Parameter \"scan_cloud_normal_k\" has been renamed to \"scan_normal_k\". "
 					"The value is still used. Use \"scan_normal_k\" to avoid this warning.");
-			pnh.param("scan_cloud_normal_k", scanCloudNormalK_, scanCloudNormalK_);
+			pnh.param("scan_cloud_normal_k", scanNormalK_, scanNormalK_);
 		}
-		pnh.param("scan_normal_radius", scanCloudNormalRadius_, scanCloudNormalRadius_);
+		pnh.param("scan_normal_radius", scanNormalRadius_, scanNormalRadius_);
+
+		NODELET_INFO("IcpOdometry: scan_cloud_max_points = %d", scanCloudMaxPoints_);
+		NODELET_INFO("IcpOdometry: scan_voxel_size       = %f", scanVoxelSize_);
+		NODELET_INFO("IcpOdometry: scan_normal_k         = %d", scanNormalK_);
+		NODELET_INFO("IcpOdometry: scan_normal_radius    = %f", scanNormalRadius_);
 
 		scan_sub_ = nh.subscribe("scan", 1, &ICPOdometry::callbackScan, this);
 		cloud_sub_ = nh.subscribe("scan_cloud", 1, &ICPOdometry::callbackCloud, this);
@@ -117,24 +124,44 @@ private:
 		projection.transformLaserScanToPointCloud(scanMsg->header.frame_id, *scanMsg, scanOut, this->tfListener());
 		pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
 		pcl::fromROSMsg(scanOut, *pclScan);
+		pclScan->is_dense = true;
 
 		cv::Mat scan;
-		if(scanCloudNormalK_ > 0 || scanCloudNormalRadius_>0.0f)
+		int maxLaserScans = (int)scanMsg->ranges.size();
+		if(pclScan->size())
 		{
-			//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::laserScan2dFromPointCloud(*pclScanNormal);
-		}
-		else
-		{
-			scan = util3d::laserScan2dFromPointCloud(*pclScan);
+			if(scanVoxelSize_ > 0.0f)
+			{
+				float pointsBeforeFiltering = (float)pclScan->size();
+				pclScan = util3d::voxelize(pclScan, scanVoxelSize_);
+				float ratio = float(pclScan->size()) / pointsBeforeFiltering;
+				maxLaserScans = int(float(maxLaserScans) * ratio);
+			}
+			if(scanNormalK_ > 0 || scanNormalRadius_>0.0f)
+			{
+				//compute normals
+				pcl::PointCloud<pcl::Normal>::Ptr normals;
+				if(scanVoxelSize_ > 0.0f)
+				{
+					normals = util3d::computeNormals2D(pclScan, scanNormalK_, scanNormalRadius_);
+				}
+				else
+				{
+					normals = util3d::computeFastOrganizedNormals2D(pclScan, scanNormalK_, scanNormalRadius_);
+				}
+				pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
+				pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
+				scan = util3d::laserScan2dFromPointCloud(*pclScanNormal);
+			}
+			else
+			{
+				scan = util3d::laserScan2dFromPointCloud(*pclScan);
+			}
 		}
 
 		rtabmap::SensorData data(
 				scan,
-				LaserScanInfo((int)scanMsg->ranges.size(), scanMsg->range_max, localScanTransform),
+				LaserScanInfo(maxLaserScans, scanMsg->range_max, localScanTransform),
 				cv::Mat(),
 				cv::Mat(),
 				CameraModel(),
@@ -148,12 +175,15 @@ private:
 	{
 		cv::Mat scan;
 		bool containNormals = false;
-		for(unsigned int i=0; i<cloudMsg->fields.size(); ++i)
+		if(scanVoxelSize_ == 0.0f)
 		{
-			if(cloudMsg->fields[i].name.compare("normal_x") == 0)
+			for(unsigned int i=0; i<cloudMsg->fields.size(); ++i)
 			{
-				containNormals = true;
-				break;
+				if(cloudMsg->fields[i].name.compare("normal_x") == 0)
+				{
+					containNormals = true;
+					break;
+				}
 			}
 		}
 
@@ -164,6 +194,7 @@ private:
 			return;
 		}
 
+		int maxLaserScans = scanCloudMaxPoints_;
 		if(containNormals)
 		{
 			pcl::PointCloud<pcl::PointNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointNormal>);
@@ -183,23 +214,33 @@ private:
 				pclScan = util3d::removeNaNFromPointCloud(pclScan);
 			}
 
-			if(scanCloudNormalK_ > 0 || scanCloudNormalRadius_>0.0f)
+			if(pclScan->size())
 			{
-				//compute normals
-				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);
-			}
-			else
-			{
-				scan = util3d::laserScanFromPointCloud(*pclScan);
+				if(scanVoxelSize_ > 0.0f)
+				{
+					float pointsBeforeFiltering = (float)pclScan->size();
+					pclScan = util3d::voxelize(pclScan, scanVoxelSize_);
+					float ratio = float(pclScan->size()) / pointsBeforeFiltering;
+					maxLaserScans = int(float(maxLaserScans) * ratio);
+				}
+				if(scanNormalK_ > 0 || scanNormalRadius_>0.0f)
+				{
+					//compute normals
+					pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(pclScan, scanNormalK_, scanNormalRadius_);
+					pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
+					pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
+					scan = util3d::laserScanFromPointCloud(*pclScanNormal);
+				}
+				else
+				{
+					scan = util3d::laserScanFromPointCloud(*pclScan);
+				}
 			}
 		}
 
 		rtabmap::SensorData data(
 				scan,
-				LaserScanInfo(scanCloudMaxPoints_, 0, localScanTransform),
+				LaserScanInfo(maxLaserScans, 0, localScanTransform),
 				cv::Mat(),
 				cv::Mat(),
 				CameraModel(),
@@ -219,8 +260,9 @@ private:
 	ros::Subscriber scan_sub_;
 	ros::Subscriber cloud_sub_;
 	int scanCloudMaxPoints_;
-	int scanCloudNormalK_;
-	float scanCloudNormalRadius_;
+	float scanVoxelSize_;
+	int scanNormalK_;
+	float scanNormalRadius_;
 };
 
 PLUGINLIB_EXPORT_CLASS(rtabmap_ros::ICPOdometry, nodelet::Nodelet);

src/nodelets/rgbd_odometry.cpp

@@ -117,6 +117,11 @@ private:
 			NODELET_FATAL("Only 2 cameras maximum supported yet.");
 		}
 
+		NODELET_INFO("RGBDOdometry: approx_sync    = %s", approxSync?"true":"false");
+		NODELET_INFO("RGBDOdometry: queue_size     = %d", queueSize_);
+		NODELET_INFO("RGBDOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false");
+		NODELET_INFO("RGBDOdometry: rgbd_cameras   = %d", rgbdCameras);
+
 		std::string subscribedTopicsMsg;
 		if(subscribeRGBD)
 		{

src/nodelets/rgbdicp_odometry.cpp

@@ -74,8 +74,9 @@ public:
 		exactCloudSync_(0),
 		queueSize_(5),
 		scanCloudMaxPoints_(0),
-		scanCloudNormalK_(0),
-		scanCloudNormalRadius_(0.0f)
+		scanVoxelSize_(0.0f),
+		scanNormalK_(0),
+		scanNormalRadius_(0.0f)
 	{
 	}
 
@@ -112,14 +113,23 @@ private:
 		pnh.param("queue_size", queueSize_, queueSize_);
 		pnh.param("subscribe_scan_cloud", subscribeScanCloud, subscribeScanCloud);
 		pnh.param("scan_cloud_max_points",  scanCloudMaxPoints_, scanCloudMaxPoints_);
-		pnh.param("scan_normal_k", scanCloudNormalK_, scanCloudNormalK_);
+		pnh.param("scan_voxel_size", scanVoxelSize_, scanVoxelSize_);
+		pnh.param("scan_normal_k", scanNormalK_, scanNormalK_);
 		if(pnh.hasParam("scan_cloud_normal_k") && !pnh.hasParam("scan_normal_k"))
 		{
 			ROS_WARN("rtabmap: Parameter \"scan_cloud_normal_k\" has been renamed to \"scan_normal_k\". "
 					"The value is still used. Use \"scan_normal_k\" to avoid this warning.");
-			pnh.param("scan_cloud_normal_k", scanCloudNormalK_, scanCloudNormalK_);
+			pnh.param("scan_cloud_normal_k", scanNormalK_, scanNormalK_);
 		}
-		pnh.param("scan_normal_radius", scanCloudNormalRadius_, scanCloudNormalRadius_);
+		pnh.param("scan_normal_radius", scanNormalRadius_, scanNormalRadius_);
+
+		NODELET_INFO("RGBDIcpOdometry: approx_sync           = %s", approxSync?"true":"false");
+		NODELET_INFO("RGBDIcpOdometry: queue_size            = %d", queueSize_);
+		NODELET_INFO("RGBDIcpOdometry: subscribe_scan_cloud  = %s", subscribeScanCloud?"true":"false");
+		NODELET_INFO("RGBDIcpOdometry: scan_cloud_max_points = %d", scanCloudMaxPoints_);
+		NODELET_INFO("RGBDIcpOdometry: scan_voxel_size       = %f", scanVoxelSize_);
+		NODELET_INFO("RGBDIcpOdometry: scan_normal_k         = %d", scanNormalK_);
+		NODELET_INFO("RGBDIcpOdometry: scan_normal_radius    = %f", scanNormalRadius_);
 
 		ros::NodeHandle rgb_nh(nh, "rgb");
 		ros::NodeHandle depth_nh(nh, "depth");
@@ -193,16 +203,6 @@ private:
 		uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "2"));
 	}
 
-	void callback(
-			const sensor_msgs::ImageConstPtr& image,
-			const sensor_msgs::ImageConstPtr& depth,
-			const sensor_msgs::CameraInfoConstPtr& cameraInfo)
-	{
-		sensor_msgs::LaserScanConstPtr scanMsg;
-		sensor_msgs::PointCloud2ConstPtr cloudMsg;
-		callbackCommon(image, depth, cameraInfo, scanMsg, cloudMsg);
-	}
-
 	void callbackScan(
 			const sensor_msgs::ImageConstPtr& image,
 			const sensor_msgs::ImageConstPtr& depth,
@@ -282,6 +282,7 @@ private:
 
 				cv::Mat scan;
 				Transform localScanTransform = Transform::getIdentity();
+				int maxLaserScans = 0;
 				if(scanMsg.get() != 0)
 				{
 					// make sure the frame of the laser is updated too
@@ -300,29 +301,52 @@ private:
 					projection.transformLaserScanToPointCloud(scanMsg->header.frame_id, *scanMsg, scanOut, this->tfListener());
 					pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
 					pcl::fromROSMsg(scanOut, *pclScan);
+					pclScan->is_dense = true;
 
-					if(scanCloudNormalK_ > 0 || scanCloudNormalRadius_>0.0f)
+					maxLaserScans = (int)scanMsg->ranges.size();
+					if(pclScan->size())
 					{
-						//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::laserScan2dFromPointCloud(*pclScanNormal);
-					}
-					else
-					{
-						scan = util3d::laserScan2dFromPointCloud(*pclScan);
+						if(scanVoxelSize_ > 0.0f)
+						{
+							float pointsBeforeFiltering = (float)pclScan->size();
+							pclScan = util3d::voxelize(pclScan, scanVoxelSize_);
+							float ratio = float(pclScan->size()) / pointsBeforeFiltering;
+							maxLaserScans = int(float(maxLaserScans) * ratio);
+						}
+						if(scanNormalK_ > 0 || scanNormalRadius_>0.0f)
+						{
+							//compute normals
+							pcl::PointCloud<pcl::Normal>::Ptr normals;
+							if(scanVoxelSize_ > 0.0f)
+							{
+								normals = util3d::computeNormals2D(pclScan, scanNormalK_, scanNormalRadius_);
+							}
+							else
+							{
+								normals = util3d::computeFastOrganizedNormals2D(pclScan, scanNormalK_, scanNormalRadius_);
+							}
+							pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
+							pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
+							scan = util3d::laserScan2dFromPointCloud(*pclScanNormal);
+						}
+						else
+						{
+							scan = util3d::laserScan2dFromPointCloud(*pclScan);
+						}
 					}
 				}
 				else if(cloudMsg.get() != 0)
 				{
 					bool containNormals = false;
-					for(unsigned int i=0; i<cloudMsg->fields.size(); ++i)
+					if(scanVoxelSize_ == 0.0f)
 					{
-						if(cloudMsg->fields[i].name.compare("normal_x") == 0)
+						for(unsigned int i=0; i<cloudMsg->fields.size(); ++i)
 						{
-							containNormals = true;
-							break;
+							if(cloudMsg->fields[i].name.compare("normal_x") == 0)
+							{
+								containNormals = true;
+								break;
+							}
 						}
 					}
 					localScanTransform = getTransform(this->frameId(), cloudMsg->header.frame_id, cloudMsg->header.stamp);
@@ -332,6 +356,7 @@ private:
 						return;
 					}
 
+					maxLaserScans = scanCloudMaxPoints_;
 					if(containNormals)
 					{
 						pcl::PointCloud<pcl::PointNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointNormal>);
@@ -351,17 +376,27 @@ private:
 							pclScan = util3d::removeNaNFromPointCloud(pclScan);
 						}
 
-						if(scanCloudNormalK_ > 0 || scanCloudNormalRadius_>0.0f)
+						if(pclScan->size())
 						{
-							//compute normals
-							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);
-						}
-						else
-						{
-							scan = util3d::laserScanFromPointCloud(*pclScan);
+							if(scanVoxelSize_ > 0.0f)
+							{
+								float pointsBeforeFiltering = (float)pclScan->size();
+								pclScan = util3d::voxelize(pclScan, scanVoxelSize_);
+								float ratio = float(pclScan->size()) / pointsBeforeFiltering;
+								maxLaserScans = int(float(maxLaserScans) * ratio);
+							}
+							if(scanNormalK_ > 0 || scanNormalRadius_>0.0f)
+							{
+								//compute normals
+								pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(pclScan, scanNormalK_, scanNormalRadius_);
+								pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
+								pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
+								scan = util3d::laserScanFromPointCloud(*pclScanNormal);
+							}
+							else
+							{
+								scan = util3d::laserScanFromPointCloud(*pclScan);
+							}
 						}
 					}
 				}
@@ -369,7 +404,7 @@ private:
 				rtabmap::SensorData data(
 						scan,
 						LaserScanInfo(
-								scanMsg.get() != 0?(int)scanMsg->ranges.size():cloudMsg.get() != 0?scanCloudMaxPoints_:0,
+								scanMsg.get() != 0 || cloudMsg.get() != 0?maxLaserScans:0,
 								scanMsg.get() != 0?scanMsg->range_max:0,
 								localScanTransform),
 						ptrImage->image,
@@ -429,8 +464,9 @@ private:
 	message_filters::Synchronizer<MyExactCloudSyncPolicy> * exactCloudSync_;
 	int queueSize_;
 	int scanCloudMaxPoints_;
-	int scanCloudNormalK_;
-	float scanCloudNormalRadius_;
+	float scanVoxelSize_;
+	int scanNormalK_;
+	float scanNormalRadius_;
 };
 
 PLUGINLIB_EXPORT_CLASS(rtabmap_ros::RGBDICPOdometry, nodelet::Nodelet);

src/nodelets/stereo_odometry.cpp

@@ -88,6 +88,9 @@ private:
 		pnh.param("approx_sync", approxSync, approxSync);
 		pnh.param("queue_size", queueSize_, queueSize_);
 
+		NODELET_INFO("StereoOdometry: approx_sync = %s", approxSync?"true":"false");
+		NODELET_INFO("StereoOdometry: queue_size = %d", queueSize_);
+
 		ros::NodeHandle left_nh(nh, "left");
 		ros::NodeHandle right_nh(nh, "right");
 		ros::NodeHandle left_pnh(pnh, "left");