rtabmap 0.15.3 now required. icp_odometry: added scan_downsampling_step parameter. OdometryROS: fixed too old tf stamps when guess and minimum motion are set.

CMakeLists.txt

@@ -18,7 +18,7 @@ find_package(rviz)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
-find_package(RTABMap 0.14.0 REQUIRED)
+find_package(RTABMap 0.15.3 REQUIRED)
 
 find_package(OpenCV REQUIRED)
 

include/rtabmap_ros/OdometryROS.h

@@ -127,6 +127,8 @@ private:
 	bool stereoParams_;
 	bool visParams_;
 	bool icpParams_;
+	rtabmap::Transform guess_;
+	double guessStamp_;
 };
 
 }

src/OdometryROS.cpp

@@ -77,7 +77,8 @@ OdometryROS::OdometryROS(bool stereoParams, bool visParams, bool icpParams) :
 	resetCurrentCount_(0),
 	stereoParams_(stereoParams),
 	visParams_(visParams),
-	icpParams_(icpParams)
+	icpParams_(icpParams),
+	guessStamp_(0.0)
 {
 
 }
@@ -397,20 +398,25 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 		}
 	}
 
-	Transform guess;
 	Transform guessCurrentPose;
 	if(!guessFrameId_.empty())
 	{
-		Transform previousPose = this->getTransform(guessFrameId_, frameId_, odometry_->previousStamp()>0.0?ros::Time(odometry_->previousStamp()):stamp);
+		Transform previousPose = this->getTransform(guessFrameId_, frameId_, guessStamp_>0.0?ros::Time(guessStamp_):stamp);
 		guessCurrentPose = this->getTransform(guessFrameId_, frameId_, stamp);
 		if(!previousPose.isNull() && !guessCurrentPose.isNull())
 		{
-			guess = previousPose.inverse() * guessCurrentPose;
+			if(guess_.isNull())
-
+			{
-			if(odometry_->previousStamp()>0.0 && (guessMinTranslation_ > 0.0 || guessMinRotation_ > 0.0))
+				guess_ = previousPose.inverse() * guessCurrentPose;
+			}
+			else
+			{
+				guess_ = guess_ * previousPose.inverse() * guessCurrentPose;
+			}
+			if(guessStamp_>0.0 && (guessMinTranslation_ > 0.0 || guessMinRotation_ > 0.0))
 			{
 				float x,y,z,roll,pitch,yaw;
-				guess.getTranslationAndEulerAngles(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_))
 				{
@@ -421,13 +427,15 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 						correctionMsg.child_frame_id = guessFrameId_;
 						correctionMsg.header.frame_id = odomFrameId_;
 						correctionMsg.header.stamp = stamp;
-						Transform correction = odometry_->getPose() * guess * guessCurrentPose.inverse();
+						Transform correction = odometry_->getPose() * guess_ * guessCurrentPose.inverse();
 						rtabmap_ros::transformToGeometryMsg(correction, correctionMsg.transform);
 						tfBroadcaster_.sendTransform(correctionMsg);
 					}
+					guessStamp_ = stamp.toSec();
 					return;
 				}
 			}
+			guessStamp_ = stamp.toSec();
 		}
 		else
 		{
@@ -440,7 +448,8 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 	ros::WallTime time = ros::WallTime::now();
 	rtabmap::OdometryInfo info;
 	SensorData dataCpy = data;
-	rtabmap::Transform pose = odometry_->process(dataCpy, guess, &info);
+	rtabmap::Transform pose = odometry_->process(dataCpy, guess_, &info);
+	guess_.setNull();
 	if(!pose.isNull())
 	{
 		resetCurrentCount_ = resetCountdown_;
@@ -682,6 +691,9 @@ bool OdometryROS::reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
 {
 	NODELET_INFO( "visual_odometry: reset odom!");
 	odometry_->reset();
+	guess_.setNull();
+	guessStamp_ = 0.0;
+	resetCurrentCount_ = resetCountdown_;
 	this->flushCallbacks();
 	return true;
 }
@@ -691,6 +703,9 @@ bool OdometryROS::resetToPose(rtabmap_ros::ResetPose::Request& req, rtabmap_ros:
 	Transform pose(req.x, req.y, req.z, req.roll, req.pitch, req.yaw);
 	NODELET_INFO( "visual_odometry: reset odom to pose %s!", pose.prettyPrint().c_str());
 	odometry_->reset(pose);
+	guess_.setNull();
+	guessStamp_ = 0.0;
+	resetCurrentCount_ = resetCountdown_;
 	this->flushCallbacks();
 	return true;
 }

src/nodelets/icp_odometry.cpp

@@ -58,6 +58,7 @@ public:
 	ICPOdometry() :
 		OdometryROS(false, false, true),
 		scanCloudMaxPoints_(0),
+		scanDownsamplingStep_(1),
 		scanVoxelSize_(0.0),
 		scanNormalK_(0),
 		scanNormalRadius_(0.0)
@@ -76,6 +77,7 @@ private:
 		ros::NodeHandle & pnh = getPrivateNodeHandle();
 
 		pnh.param("scan_cloud_max_points",  scanCloudMaxPoints_, scanCloudMaxPoints_);
+		pnh.param("scan_downsampling_step", scanDownsamplingStep_, scanDownsamplingStep_);
 		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"))
@@ -86,10 +88,11 @@ private:
 		}
 		pnh.param("scan_normal_radius", scanNormalRadius_, scanNormalRadius_);
 
-		NODELET_INFO("IcpOdometry: scan_cloud_max_points = %d", scanCloudMaxPoints_);
+		NODELET_INFO("IcpOdometry: scan_cloud_max_points  = %d", scanCloudMaxPoints_);
-		NODELET_INFO("IcpOdometry: scan_voxel_size       = %f", scanVoxelSize_);
+		NODELET_INFO("IcpOdometry: scan_downsampling_step = %d", scanDownsamplingStep_);
-		NODELET_INFO("IcpOdometry: scan_normal_k         = %d", scanNormalK_);
+		NODELET_INFO("IcpOdometry: scan_voxel_size        = %f", scanVoxelSize_);
-		NODELET_INFO("IcpOdometry: scan_normal_radius    = %f", scanNormalRadius_);
+		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);
@@ -106,6 +109,24 @@ private:
 		uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "1"));
 
 		ros::NodeHandle & pnh = getPrivateNodeHandle();
+		iter = parameters.find(Parameters::kIcpDownsamplingStep());
+		if(iter != parameters.end())
+		{
+			int value = uStr2Int(iter->second);
+			if(value > 1)
+			{
+				if(!pnh.hasParam("scan_downsampling_step"))
+				{
+					ROS_WARN("IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_downsampling_step\" for convenience. \"%s\" is set to 0.", iter->second.c_str(), iter->first.c_str(), iter->first.c_str());
+					scanDownsamplingStep_ = value;
+					iter->second = "1";
+				}
+				else
+				{
+					ROS_WARN("IcpOdometry: Both parameter \"%s\" and ros parameter \"scan_downsampling_step\" are set.", iter->first.c_str());
+				}
+			}
+		}
 		iter = parameters.find(Parameters::kIcpVoxelSize());
 		if(iter != parameters.end())
 		{
@@ -176,6 +197,11 @@ private:
 		int maxLaserScans = (int)scanMsg->ranges.size();
 		if(pclScan->size())
 		{
+			if(scanDownsamplingStep_ > 1)
+			{
+				pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
+				maxLaserScans /= scanDownsamplingStep_;
+			}
 			if(scanVoxelSize_ > 0.0f)
 			{
 				float pointsBeforeFiltering = (float)pclScan->size();
@@ -245,6 +271,11 @@ private:
 		{
 			pcl::PointCloud<pcl::PointNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointNormal>);
 			pcl::fromROSMsg(*cloudMsg, *pclScan);
+			if(pclScan->size() && scanDownsamplingStep_ > 1)
+			{
+				pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
+				maxLaserScans /= scanDownsamplingStep_;
+			}
 			if(!pclScan->is_dense)
 			{
 				pclScan = util3d::removeNaNNormalsFromPointCloud(pclScan);
@@ -255,6 +286,11 @@ private:
 		{
 			pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
 			pcl::fromROSMsg(*cloudMsg, *pclScan);
+			if(pclScan->size() && scanDownsamplingStep_ > 1)
+			{
+				pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
+				maxLaserScans /= scanDownsamplingStep_;
+			}
 			if(!pclScan->is_dense)
 			{
 				pclScan = util3d::removeNaNFromPointCloud(pclScan);
@@ -306,6 +342,7 @@ private:
 	ros::Subscriber scan_sub_;
 	ros::Subscriber cloud_sub_;
 	int scanCloudMaxPoints_;
+	int scanDownsamplingStep_;
 	double scanVoxelSize_;
 	int scanNormalK_;
 	double scanNormalRadius_;