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.

.project

@@ -23,7 +23,7 @@
 				</dictionary>
 				<dictionary>
 					<key>org.eclipse.cdt.make.core.buildArguments</key>
-					<value>VERBOSE=true -j4</value>
+					<value>VERBOSE=true -j2</value>
 				</dictionary>
 				<dictionary>
 					<key>org.eclipse.cdt.make.core.buildCommand</key>

include/rtabmap_ros/CoreWrapper.h

@@ -198,8 +198,6 @@ private:
 	double genScanMaxDepth_;
 	double genScanMinDepth_;
 	int scanCloudMaxPoints_;
-	int scanCloudNormalK_;
-	float scanCloudNormalRadius_;
 
 	rtabmap::Transform mapToOdom_;
 	boost::mutex mapToOdomMutex_;

include/rtabmap_ros/MsgConversion.h

@@ -205,9 +205,7 @@ bool convertScanMsg(
 		cv::Mat & scan,
 		rtabmap::Transform & scanLocalTransform,
 		tf::TransformListener & listener,
-		double waitForTransform,
+		double waitForTransform);
-		int scanCloudNormalK = 0,
-		float scanCloudNormalRadius = 0.0f);
 
 bool convertScan3dMsg(
 		const sensor_msgs::PointCloud2ConstPtr & scan3dMsg,
@@ -217,9 +215,7 @@ bool convertScan3dMsg(
 		cv::Mat & scan,
 		rtabmap::Transform & scanLocalTransform,
 		tf::TransformListener & listener,
-		double waitForTransform,
+		double waitForTransform);
-		int scanCloudNormalK = 0,
-		float scanCloudNormalRadius = 0.0f);
 
 }
 

include/rtabmap_ros/OdometryROS.h

@@ -97,6 +97,8 @@ private:
 	std::string groundTruthFrameId_;
 	std::string groundTruthBaseFrameId_;
 	std::string guessFrameId_;
+	double guessMinTranslation_;
+	double guessMinRotation_;
 	bool publishTf_;
 	bool waitForTransform_;
 	double waitForTransformDuration_;

launch/data_recorder.launch

@@ -4,12 +4,16 @@
   <arg name="subscribe_odometry"   default="false"/>
   <arg name="subscribe_depth"      default="true"/>
   <arg name="subscribe_stereo"     default="false"/>
+  <arg name="subscribe_rgbd"       default="false"/>
   <arg name="subscribe_scan"       default="false"/>
+  <arg name="subscribe_scan_cloud" default="false"/>
   <arg     if="$(arg subscribe_stereo)" name="approx_sync"  default="false"/>
   <arg unless="$(arg subscribe_stereo)" name="approx_sync"  default="true"/>
   
   <arg name="frame_id"      default="camera_link"/>
   <arg name="odom_frame_id" default=""/>        <!-- use topic when not set, otherwise use TF if set -->
+  <arg name="ground_truth_frame_id"   default=""/>     <!-- e.g., "world" -->
+  <arg name="ground_truth_base_frame_id" default=""/>  <!-- e.g., "tracker", a fake frame matching the frame "frame_id" (but on different TF tree) -->
   
   <arg name="output_path"   default="output.db"/>
   <arg name="record_in_RAM" default="false"/>
@@ -23,8 +27,10 @@
   <arg name="left_info_topic"  default="camera/left/camera_info"/>
   <arg name="right_topic"      default="camera/right/image_rect"/>
   <arg name="right_info_topic" default="camera/right/camera_info"/>
+  <arg name="rgbd_topic"       default="camera/rgbd_image" />
   <arg name="odom_topic"       default="odom"/>
   <arg name="scan_topic"       default="scan"/>
+  <arg name="scan_cloud_topic" default="scan_cloud"/>
   
   <arg name="rgb_image_transport"  default="raw"/>
   <arg name="depth_image_transport"  default="raw"/>
@@ -50,9 +56,12 @@
 	<param name="DbSqlite3/InMemory"  type="string" value="$(arg record_in_RAM)"/>
 	<param name="database_path"       type="string" value="$(arg output_path)"/>
 	<param name="frame_id"            type="string" value="$(arg frame_id)"/>
-	<param name="subscribe_depth"     type="bool"   value="$(arg subscribe_depth)"/>
+	<param name="ground_truth_frame_id"       type="string" value="$(arg ground_truth_frame_id)"/>
+    <param name="ground_truth_base_frame_id"  type="string" value="$(arg ground_truth_base_frame_id)"/>
 	<param name="subscribe_scan"      type="bool"   value="$(arg subscribe_scan)"/>
+	<param name="subscribe_scan_cloud" type="bool"   value="$(arg subscribe_scan_cloud)"/>
 	<param name="subscribe_stereo"    type="bool"   value="$(arg subscribe_stereo)"/>
+	<param name="subscribe_rgbd"      type="bool"   value="$(arg subscribe_rgbd)"/>
 	<param name="queue_size"          type="int"    value="$(arg queue_size)"/>
 	<param name="approx_sync"         type="bool"   value="$(arg approx_sync)"/>
 	
@@ -72,7 +81,9 @@
   	<remap from="left/camera_info"  to="$(arg left_info_topic)"/>
   	<remap from="right/image_rect"  to="$(arg right_topic)"/>
   	<remap from="right/camera_info" to="$(arg right_info_topic)"/>
+  	<remap from="rgbd_image"        to="$(arg rgbd_topic)"/>
   	<remap from="scan"              to="$(arg scan_topic)"/>
+  	<remap from="scan_cloud"        to="$(arg scan_cloud_topic)"/>
   	<remap from="odom"              to="$(arg odom_topic)"/>
   </node>
 </launch>

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"))
-	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\". "
+		ROS_WARN("rtabmap: Parameter \"scan_cloud_normal_k\" has been removed. RTAB-Map's parameter \"%s\" should be used instead. "
-				"The value is still used. Use \"scan_normal_k\" to avoid this warning.");
+				"The value is copied. Use \"%s\" to avoid this warning.",
-		pnh.param("scan_cloud_normal_k", scanCloudNormalK_, scanCloudNormalK_);
+				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)
+		scan = rtabmap::util3d::laserScan2dFromPointCloud(*scanCloud2d);
-		{
-			//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);
-		}
 	}
 	else if(scan2dMsg.get() != 0)
 	{
@@ -1019,9 +1008,7 @@ 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;
@@ -1050,9 +1037,7 @@ void CoreWrapper::commonDepthCallbackImpl(
 				scan,
 				scanLocalTransform,
 				tfListener_,
-				waitForTransform_?waitForTransformDuration_:0,
+				waitForTransform_?waitForTransformDuration_:0))
-				scanCloudNormalK_,
-				scanCloudNormalRadius_))
 		{
 			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,
+				waitForTransform_?waitForTransformDuration_:0))
-				scanCloudNormalK_,
-				scanCloudNormalRadius_))
 		{
 			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,
+				waitForTransform_?waitForTransformDuration_:0))
-				scanCloudNormalK_,
-				scanCloudNormalRadius_))
 		{
 			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,
+		double waitForTransform)
-		int scanCloudNormalK,
-		float scanCloudNormalRadius)
 {
 	// 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)
+	scan = rtabmap::util3d::laserScan2dFromPointCloud(*pclScan, laserToOdom); // put back in laser frame
-	{
-		//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
-	}
-
 
 	return true;
 }
@@ -1453,9 +1440,7 @@ bool convertScan3dMsg(
 		cv::Mat & scan,
 		rtabmap::Transform & scanLocalTransform,
 		tf::TransformListener & listener,
-		double waitForTransform,
+		double waitForTransform)
-		int scanCloudNormalK,
-		float scanCloudNormalRadius)
 {
 	bool containNormals = false;
 	bool containColors = false;
@@ -1533,18 +1518,7 @@ bool convertScan3dMsg(
 				pclScan = rtabmap::util3d::removeNaNFromPointCloud(pclScan);
 			}
 
-			if(scanCloudNormalK > 0 || scanCloudNormalRadius>0.0f)
+			scan = rtabmap::util3d::laserScanFromPointCloud(*pclScan);
-			{
-				//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);
-			}
 		}
 		else
 		{
@@ -1555,18 +1529,7 @@ bool convertScan3dMsg(
 				pclScan = rtabmap::util3d::removeNaNFromPointCloud(pclScan);
 			}
 
-			if(scanCloudNormalK > 0 || scanCloudNormalRadius>0.0f)
+			scan = rtabmap::util3d::laserScanFromPointCloud(*pclScan);
-			{
-				//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);
-			}
 		}
 	}
 	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),
+		scanVoxelSize_(0.0f),
-		scanCloudNormalRadius_(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
+			if(scanVoxelSize_ > 0.0f)
-			pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeFastOrganizedNormals2D(pclScan, scanCloudNormalK_, scanCloudNormalRadius_);
+			{
-			pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
+				float pointsBeforeFiltering = (float)pclScan->size();
-			pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
+				pclScan = util3d::voxelize(pclScan, scanVoxelSize_);
-			scan = util3d::laserScan2dFromPointCloud(*pclScanNormal);
+				float ratio = float(pclScan->size()) / pointsBeforeFiltering;
-		}
+				maxLaserScans = int(float(maxLaserScans) * ratio);
-		else
+			}
-		{
+			if(scanNormalK_ > 0 || scanNormalRadius_>0.0f)
-			scan = util3d::laserScan2dFromPointCloud(*pclScan);
+			{
+				//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;
+				if(cloudMsg->fields[i].name.compare("normal_x") == 0)
-				break;
+				{
+					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
+				if(scanVoxelSize_ > 0.0f)
-				pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(pclScan, scanCloudNormalK_, scanCloudNormalRadius_);
+				{
-				pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
+					float pointsBeforeFiltering = (float)pclScan->size();
-				pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
+					pclScan = util3d::voxelize(pclScan, scanVoxelSize_);
-				scan = util3d::laserScanFromPointCloud(*pclScanNormal);
+					float ratio = float(pclScan->size()) / pointsBeforeFiltering;
-			}
+					maxLaserScans = int(float(maxLaserScans) * ratio);
-			else
+				}
-			{
+				if(scanNormalK_ > 0 || scanNormalRadius_>0.0f)
-				scan = util3d::laserScanFromPointCloud(*pclScan);
+				{
+					//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 scanVoxelSize_;
-	float scanCloudNormalRadius_;
+	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),
+		scanVoxelSize_(0.0f),
-		scanCloudNormalRadius_(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
+						if(scanVoxelSize_ > 0.0f)
-						pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeFastOrganizedNormals2D(pclScan, scanCloudNormalK_, scanCloudNormalRadius_);
+						{
-						pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
+							float pointsBeforeFiltering = (float)pclScan->size();
-						pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
+							pclScan = util3d::voxelize(pclScan, scanVoxelSize_);
-						scan = util3d::laserScan2dFromPointCloud(*pclScanNormal);
+							float ratio = float(pclScan->size()) / pointsBeforeFiltering;
-					}
+							maxLaserScans = int(float(maxLaserScans) * ratio);
-					else
+						}
-					{
+						if(scanNormalK_ > 0 || scanNormalRadius_>0.0f)
-						scan = util3d::laserScan2dFromPointCloud(*pclScan);
+						{
+							//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;
+							if(cloudMsg->fields[i].name.compare("normal_x") == 0)
-							break;
+							{
+								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
+							if(scanVoxelSize_ > 0.0f)
-							pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(pclScan, scanCloudNormalK_, scanCloudNormalRadius_);
+							{
-							pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
+								float pointsBeforeFiltering = (float)pclScan->size();
-							pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
+								pclScan = util3d::voxelize(pclScan, scanVoxelSize_);
-							scan = util3d::laserScanFromPointCloud(*pclScanNormal);
+								float ratio = float(pclScan->size()) / pointsBeforeFiltering;
-						}
+								maxLaserScans = int(float(maxLaserScans) * ratio);
-						else
+							}
-						{
+							if(scanNormalK_ > 0 || scanNormalRadius_>0.0f)
-							scan = util3d::laserScanFromPointCloud(*pclScan);
+							{
+								//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 scanVoxelSize_;
-	float scanCloudNormalRadius_;
+	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");