Odometry: 2D scans + normals support

corelib/include/rtabmap/core/Odometry.h

@@ -68,6 +68,7 @@ public:
 	bool isInfoDataFilled() const {return _fillInfoData;}
 	const Transform & previousVelocityTransform() const {return previousVelocityTransform_;}
 	double previousStamp() const {return previousStamp_;}
+	unsigned int framesProcessed() const {return framesProcessed_;}
 
 private:
 	virtual Transform computeTransform(SensorData & data, const Transform & guess = Transform(), OdometryInfo * info = 0) = 0;
@@ -98,6 +99,7 @@ private:
 	Transform previousVelocityTransform_;
 	Transform previousGroundTruthPose_;
 	float distanceTravelled_;
+	unsigned int framesProcessed_;
 
 	std::vector<ParticleFilter *> particleFilters_;
 	cv::KalmanFilter kalmanFilter_;

corelib/src/Odometry.cpp

@@ -102,7 +102,8 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 		_pose(Transform::getIdentity()),
 		_resetCurrentCount(0),
 		previousStamp_(0),
-		distanceTravelled_(0)
+		distanceTravelled_(0),
+		framesProcessed_(0)
 {
 	Parameters::parse(parameters, Parameters::kOdomResetCountdown(), _resetCountdown);
 
@@ -168,6 +169,7 @@ void Odometry::reset(const Transform & initialPose)
 	_resetCurrentCount = 0;
 	previousStamp_ = 0;
 	distanceTravelled_ = 0;
+	framesProcessed_ = 0;
 	if(_force3DoF || particleFilters_.size())
 	{
 		float x,y,z, roll,pitch,yaw;
@@ -544,6 +546,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 			distanceTravelled_ += t.getNorm();
 			info->distanceTravelled = distanceTravelled_;
 		}
+		++framesProcessed_;
 
 		return _pose *= t; // update
 	}

corelib/src/OdometryF2F.cpp

@@ -83,6 +83,7 @@ Transform OdometryF2F::computeTransform(
 		return output;
 	}
 
+	bool addKeyFrame = false;
 	RegistrationInfo regInfo;
 
 	UASSERT(!this->getPose().isNull());
@@ -99,8 +100,8 @@ Transform OdometryF2F::computeTransform(
 		output = registrationPipeline_->computeTransformationMod(
 				tmpRefFrame,
 				newFrame,
-				// special case for ICP-only odom, set guess to identity if we just started
+				// special case for ICP-only odom, set guess to identity if we just started or reset
-				!guess.isNull()?motionSinceLastKeyFrame*guess:!registrationPipeline_->isImageRequired()&&this->getPose().isIdentity()?Transform::getIdentity():Transform(),
+				!guess.isNull()?motionSinceLastKeyFrame*guess:!registrationPipeline_->isImageRequired()&&this->framesProcessed()<2?motionSinceLastKeyFrame:Transform(),
 				&regInfo);
 
 		if(output.isNull() && !guess.isNull() && registrationPipeline_->isImageRequired())
@@ -185,7 +186,7 @@ Transform OdometryF2F::computeTransform(
 		{
 			UDEBUG("Update key frame");
 			int features = newFrame.getWordsDescriptors().size();
-			if(features == 0)
+			if(registrationPipeline_->isImageRequired() && features == 0)
 			{
 				newFrame = Signature(data);
 				// this will generate features only for the first frame or if optical flow was used (no 3d words)
@@ -209,6 +210,8 @@ Transform OdometryF2F::computeTransform(
 
 				//reset motion
 				lastKeyFramePose_.setNull();
+
+				addKeyFrame = true;
 			}
 			else
 			{
@@ -249,6 +252,7 @@ Transform OdometryF2F::computeTransform(
 		info->icpInliersRatio = regInfo.icpInliersRatio;
 		info->matches = regInfo.matches;
 		info->features = newFrame.sensorData().keypoints().size();
+		info->keyFrameAdded = addKeyFrame;
 	}
 
 	UINFO("Odom update time = %fs lost=%s inliers=%d, ref frame corners=%d, transform accepted=%s",

corelib/src/OdometryF2M.cpp

@@ -186,9 +186,10 @@ Transform OdometryF2M::computeTransform(
 			Transform transform = regPipeline_->computeTransformationMod(
 					tmpMap,
 					*lastFrame_,
-					// special case for ICP-only odom, set guess to identity if we just started
+					// special case for ICP-only odom, set guess to identity if we just started or reset
-					!guess.isNull()?this->getPose()*guess:!regPipeline_->isImageRequired()&&this->getPose().isIdentity()?Transform::getIdentity():Transform(),
+					!guess.isNull()?this->getPose()*guess:!regPipeline_->isImageRequired()&&this->framesProcessed()<2?this->getPose():Transform(),
 					&regInfo);
+
 			if(transform.isNull() && !guess.isNull() && regPipeline_->isImageRequired())
 			{
 				tmpMap = *map_;
@@ -591,7 +592,7 @@ Transform OdometryF2M::computeTransform(
 
 					if(lastFrame_->sensorData().laserScanRaw().cols)
 					{
-						pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(mapScan);
+						pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(mapScan, tmpMap.sensorData().laserScanInfo().localTransform());
 						pcl::PointCloud<pcl::PointNormal>::Ptr frameCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose * lastFrame_->sensorData().laserScanInfo().localTransform());
 
 						pcl::IndicesPtr frameCloudNormalsIndices(new std::vector<int>);
@@ -623,17 +624,6 @@ Transform OdometryF2M::computeTransform(
 									newPoints,
 									scanMaximumMapSize_);
 
-							if(newPoints < 20)
-							{
-								UWARN("The number of new scan points added to local odometry "
-									  "map is low (%d), you may want to decrease the parameter \"%s\" "
-									  "(current value=%f and ICP inliers ratio is %f)",
-										newPoints,
-										Parameters::kOdomScanKeyFrameThr().c_str(),
-										scanKeyFrameThr_,
-										regInfo.icpInliersRatio);
-							}
-
 							if(scansBuffer_.size() > 1 &&
 								int(mapCloudNormals->size() + newPoints) > scanMaximumMapSize_)
 							{
@@ -691,7 +681,16 @@ Transform OdometryF2M::computeTransform(
 									*mapCloudNormals += *scansBuffer_.back().first;
 								}
 							}
-							mapScan = util3d::laserScanFromPointCloud(*mapCloudNormals);
+							if(mapScan.channels() == 2 || mapScan.channels() == 5)
+							{
+								Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(),0,0,0,0);
+								mapScan = util3d::laserScan2dFromPointCloud(*mapCloudNormals, mapViewpoint);
+							}
+							else
+							{
+								Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(), -newFramePose.z(),0,0,0);
+								mapScan = util3d::laserScanFromPointCloud(*mapCloudNormals, mapViewpoint);
+							}
 							modified=true;
 						}
 					}
@@ -702,7 +701,18 @@ Transform OdometryF2M::computeTransform(
 				{
 					*map_ = tmpMap;
 
-					map_->sensorData().setLaserScanRaw(mapScan, LaserScanInfo(0, 0));
+					if(mapScan.channels() == 2 || mapScan.channels() == 5)
+					{
+
+						map_->sensorData().setLaserScanRaw(mapScan,
+									LaserScanInfo(0, 0.0f, Transform(newFramePose.x(), newFramePose.y(), lastFrame_->sensorData().laserScanInfo().localTransform().z(),0,0,0)));
+					}
+					else
+					{
+						map_->sensorData().setLaserScanRaw(mapScan,
+								LaserScanInfo(0, 0.0f, newFramePose.translation()));
+					}
+
 					map_->setWords(mapWords);
 					map_->setWords3(mapPoints);
 				 	map_->setWordsDescriptors(mapDescriptors);
@@ -717,7 +727,7 @@ Transform OdometryF2M::computeTransform(
 				if(this->isInfoDataFilled())
 				{
 					info->localMap = uMultimapToMap(tmpMap.getWords3());
-					info->localScanMap = tmpMap.sensorData().laserScanRaw();
+					info->localScanMap = util3d::transformLaserScan(tmpMap.sensorData().laserScanRaw(), tmpMap.sensorData().laserScanInfo().localTransform());
 				}
 			}
 		}
@@ -831,7 +841,18 @@ Transform OdometryF2M::computeTransform(
 					frameValid = true;
 					pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose * lastFrame_->sensorData().laserScanInfo().localTransform());
 					scansBuffer_.push_back(std::make_pair(mapCloudNormals, pcl::IndicesPtr(new std::vector<int>)));
-					map_->sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*mapCloudNormals), LaserScanInfo(0,0));
+					if(lastFrame_->sensorData().laserScanRaw().channels() == 2 || lastFrame_->sensorData().laserScanRaw().channels() == 5)
+					{
+						Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(),0,0,0,0);
+						map_->sensorData().setLaserScanRaw(util3d::laserScan2dFromPointCloud(*mapCloudNormals, mapViewpoint),
+								LaserScanInfo(0, 0.0f, Transform(newFramePose.x(), newFramePose.y(), lastFrame_->sensorData().laserScanInfo().localTransform().z(),0,0,0)));
+					}
+					else
+					{
+						Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(), -newFramePose.z(),0,0,0);
+						map_->sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*mapCloudNormals, mapViewpoint),
+								LaserScanInfo(0, 0.0f, newFramePose.translation()));
+					}
 					addKeyFrame = true;
 				}
 				else
@@ -854,7 +875,7 @@ Transform OdometryF2M::computeTransform(
 				if(this->isInfoDataFilled())
 				{
 					info->localMap = uMultimapToMap(map_->getWords3());
-					info->localScanMap = map_->sensorData().laserScanRaw();
+					info->localScanMap = util3d::transformLaserScan(map_->sensorData().laserScanRaw(), map_->sensorData().laserScanInfo().localTransform());
 				}
 			}
 		}

corelib/src/RegistrationIcp.cpp

@@ -505,18 +505,24 @@ Transform RegistrationIcp::computeTransformationImpl(
 				pcl::PointCloud<pcl::PointXYZ>::Ptr toCloudFiltered = toCloud;
 				if(_voxelSize > 0.0f)
 				{
-					int pointsBeforeFiltering = fromCloudFiltered->size();
+					float pointsBeforeFiltering = (float)fromCloudFiltered->size();
 					fromCloudFiltered = util3d::voxelize(fromCloudFiltered, _voxelSize);
-					maxLaserScansFrom = maxLaserScansFrom * fromCloudFiltered->size() / pointsBeforeFiltering;
+					float ratioFrom = float(fromCloudFiltered->size()) / pointsBeforeFiltering;
+					maxLaserScansFrom = int(float(maxLaserScansFrom) * ratioFrom);
 
-					pointsBeforeFiltering = toCloudFiltered->size();
+					pointsBeforeFiltering = (float)toCloudFiltered->size();
 					toCloudFiltered = util3d::voxelize(toCloudFiltered, _voxelSize);
-					maxLaserScansTo = maxLaserScansTo * toCloudFiltered->size() / pointsBeforeFiltering;
+					float ratioTo = float(toCloudFiltered->size()) / pointsBeforeFiltering;
+					maxLaserScansTo = int(float(maxLaserScansTo) * ratioTo);
 
-					UDEBUG("Voxel filtering time (voxel=%f m, ratioFrom=%f ratioTo=%f) = %f s",
+					UDEBUG("Voxel filtering time (voxel=%f m, ratioFrom=%f->%d/%d ratioTo=%f->%d/%d) = %f s",
 							_voxelSize,
-							float(fromCloudFiltered->size()) / float(pointsBeforeFiltering),
+							ratioFrom,
-							float(toCloudFiltered->size()) / float(pointsBeforeFiltering),
+							(int)fromCloudFiltered->size(),
+							maxLaserScansFrom,
+							ratioTo,
+							(int)toCloudFiltered->size(),
+							maxLaserScansTo,
 							timer.ticks());
 				}
 
@@ -531,11 +537,22 @@ Transform RegistrationIcp::computeTransformationImpl(
 
 					if(fromScan.channels() == 2 || fromScan.channels() == 5)
 					{
-						normals = util3d::computeFastOrganizedNormals2D(
+						if(_voxelSize > 0.0f)
-								fromCloudFiltered,
+						{
-								_pointToPlaneK,
+							normals = util3d::computeNormals2D(
-								_pointToPlaneRadius,
+									fromCloudFiltered,
-								viewpointFrom);
+									_pointToPlaneK,
+									_pointToPlaneRadius,
+									viewpointFrom);
+						}
+						else
+						{
+							normals = util3d::computeFastOrganizedNormals2D(
+									fromCloudFiltered,
+									_pointToPlaneK,
+									_pointToPlaneRadius,
+									viewpointFrom);
+						}
 					}
 					else
 					{
@@ -546,11 +563,22 @@ Transform RegistrationIcp::computeTransformationImpl(
 
 					if(toScan.channels() == 2 || toScan.channels() == 5)
 					{
-						normals = util3d::computeFastOrganizedNormals2D(
+						if(_voxelSize > 0.0f)
-								toCloudFiltered,
+						{
-								_pointToPlaneK,
+							normals = util3d::computeNormals2D(
-								_pointToPlaneRadius,
+									toCloudFiltered,
-								viewpointTo);
+									_pointToPlaneK,
+									_pointToPlaneRadius,
+									viewpointTo);
+						}
+						else
+						{
+							normals = util3d::computeFastOrganizedNormals2D(
+									toCloudFiltered,
+									_pointToPlaneK,
+									_pointToPlaneRadius,
+									viewpointTo);
+						}
 					}
 					else
 					{
@@ -564,7 +592,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 					fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals);
 
 					// update output scans
-					if(fromScan.channels() == 2 || toScan.channels() == 5)
+					if(fromScan.channels() == 2 || fromScan.channels() == 5)
 					{
 						fromSignature.sensorData().setLaserScanRaw(util3d::laserScan2dFromPointCloud(*fromCloudNormals, fromLocalTransform.inverse()), LaserScanInfo(maxLaserScansFrom, fromSignature.sensorData().laserScanInfo().maxRange(), fromLocalTransform));
 					}
@@ -654,8 +682,22 @@ Transform RegistrationIcp::computeTransformationImpl(
 					if(_voxelSize > 0.0f)
 					{
 						// update output scans
-						fromSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*fromCloudFiltered, fromLocalTransform.inverse()), LaserScanInfo(maxLaserScansFrom, fromSignature.sensorData().laserScanInfo().maxRange(), fromLocalTransform));
+						if(fromScan.channels() == 2 || fromScan.channels() == 5)
-						toSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*toCloudFiltered, (guess*toLocalTransform).inverse()), LaserScanInfo(maxLaserScansTo, toSignature.sensorData().laserScanInfo().maxRange(), toLocalTransform));
+						{
+							fromSignature.sensorData().setLaserScanRaw(util3d::laserScan2dFromPointCloud(*fromCloudFiltered, fromLocalTransform.inverse()), LaserScanInfo(maxLaserScansFrom, fromSignature.sensorData().laserScanInfo().maxRange(), fromLocalTransform));
+						}
+						else
+						{
+							fromSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*fromCloudFiltered, fromLocalTransform.inverse()), LaserScanInfo(maxLaserScansFrom, fromSignature.sensorData().laserScanInfo().maxRange(), fromLocalTransform));
+						}
+						if(toScan.channels() == 2 || toScan.channels() == 5)
+						{
+							toSignature.sensorData().setLaserScanRaw(util3d::laserScan2dFromPointCloud(*toCloudFiltered, (guess*toLocalTransform).inverse()), LaserScanInfo(maxLaserScansTo, toSignature.sensorData().laserScanInfo().maxRange(), toLocalTransform));
+						}
+						else
+						{
+							toSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*toCloudFiltered, (guess*toLocalTransform).inverse()), LaserScanInfo(maxLaserScansTo, toSignature.sensorData().laserScanInfo().maxRange(), toLocalTransform));
+						}
 					}
 
 #ifdef RTABMAP_POINTMATCHER

corelib/src/util3d_registration.cpp

@@ -244,8 +244,8 @@ void computeVarianceAndCorrespondences(
 	correspondencesOut = 0;
 	pcl::registration::CorrespondenceEstimation<pcl::PointNormal, pcl::PointNormal>::Ptr est;
 	est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointNormal, pcl::PointNormal>);
-	est->setInputTarget(cloudB);
+	est->setInputTarget(cloudA->size()>cloudB->size()?cloudA:cloudB);
-	est->setInputSource(cloudA);
+	est->setInputSource(cloudA->size()>cloudB->size()?cloudB:cloudA);
 	pcl::Correspondences correspondences;
 	est->determineCorrespondences(correspondences, maxCorrespondenceDistance);
 
@@ -277,8 +277,8 @@ void computeVarianceAndCorrespondences(
 	correspondencesOut = 0;
 	pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>::Ptr est;
 	est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>);
-	est->setInputTarget(cloudB);
+	est->setInputTarget(cloudA->size()>cloudB->size()?cloudA:cloudB);
-	est->setInputSource(cloudA);
+	est->setInputSource(cloudA->size()>cloudB->size()?cloudB:cloudA);
 	pcl::Correspondences correspondences;
 	est->determineCorrespondences(correspondences, maxCorrespondenceDistance);
 

guilib/src/DatabaseViewer.cpp

@@ -2469,7 +2469,8 @@ void DatabaseViewer::update(int value,
 				labelPose->setText(QString("%1xyz=(%2,%3,%4)\nrpy=(%5,%6,%7)").arg(odomPose.isIdentity()?"* ":"").arg(x).arg(y).arg(z).arg(roll).arg(pitch).arg(yaw));
 				if(s!=0.0)
 				{
-					stamp->setText(QDateTime::fromMSecsSinceEpoch(s*1000.0).toString("dd.MM.yyyy hh:mm:ss.zzz"));
+					stamp->setText(QString::number(s, 'f'));
+					stamp->setToolTip(QDateTime::fromMSecsSinceEpoch(s*1000.0).toString("dd.MM.yyyy hh:mm:ss.zzz"));
 				}
 				if(data.cameraModels().size() || data.stereoCameraModel().isValidForProjection())
 				{