Rtabmap::addLink() supporting localization mode. MainWindow: fixed flickering camera frustum.

corelib/include/rtabmap/core/Rtabmap.h

@@ -313,6 +313,7 @@ private:
 	bool _currentSessionHasGPS;
 	std::map<int, Transform> _odomCachePoses;       // used in localization mode to reject loop closures
 	std::multimap<int, Link> _odomCacheConstraints; // used in localization mode to reject loop closures
+	std::map<int, Transform> _odomCacheAddLink; // used in localization mode when adding external link
 
 	// Planning stuff
 	int _pathStatus;

corelib/src/RegistrationVis.cpp

@@ -1881,7 +1881,7 @@ Transform RegistrationVis::computeTransformationImpl(
 	}
 	else if(toSignature.sensorData().isValid())
 	{
-		UWARN("Missing correspondences for registration (%d->%d). fromWords = %d fromImageEmpty=%d toWords = %d toImageEmpty=%d",
+		msg = uFormat("Missing correspondences for registration (%d->%d). fromWords = %d fromImageEmpty=%d toWords = %d toImageEmpty=%d",
 				fromSignature.id(), toSignature.id(),
 				(int)fromSignature.getWords().size(), fromSignature.sensorData().imageRaw().empty()?1:0,
 				(int)toSignature.getWords().size(), toSignature.sensorData().imageRaw().empty()?1:0);

corelib/src/Rtabmap.cpp

@@ -1373,6 +1373,12 @@ bool Rtabmap::process(
 		_lastLocalizationPose = newPose; // keep in cache the latest corrected pose
 		if(!_memory->isIncremental())
 		{
+			_odomCacheAddLink.insert(std::make_pair(signature->id(), signature->getPose()));
+			while(!_odomCacheAddLink.empty() && (int)_odomCacheAddLink.size() > _maxOdomCacheSize+1)
+			{
+				_odomCacheAddLink.erase(_odomCacheAddLink.begin());
+			}
+
 			if(_optimizationMaxError <= 0.0f || _maxOdomCacheSize <= 0)
 			{
 				_odomCachePoses.clear();
@@ -2907,7 +2913,7 @@ bool Rtabmap::process(
 				lastProximitySpaceClosureId = 0;
 				rejectedGlobalLoopClosure = true;
 			}
-			else if(_memory->isIncremental() && // FIXME: not tested in localization mode, so do it only in mapping mode
+			else if(_memory->isIncremental() &&
 			  _optimizationMaxError > 0.0f &&
 			  loopClosureLinksAdded.size() &&
 			  optimizationIterations > 0 &&
@@ -4786,6 +4792,8 @@ bool Rtabmap::addLink(const Link & link)
 		UERROR("Link's transform is null!");
 		return false;
 	}
+	if(_memory->isIncremental())
+	{
 		if(_memory->getSignature(link.from()) == 0)
 		{
 			UERROR("Link's \"from id\" %d is not in working memory", link.from());
@@ -4799,8 +4807,10 @@ bool Rtabmap::addLink(const Link & link)
 
 		std::map<int, Transform> poses;
 		std::multimap<int, Link> links;
-	this->getGraph(poses, links, true, true);
+		this->getGraph(poses, links, true, false);
 
+		if(_memory->isIncremental())
+		{
 			if(poses.find(link.from()) == poses.end())
 			{
 				UERROR("Link's \"from id\" %d is not in the graph", link.from());
@@ -4811,6 +4821,8 @@ bool Rtabmap::addLink(const Link & link)
 				UERROR("Link's \"to id\" %d is not in the graph", link.to());
 				return false;
 			}
+		}
+
 		int from = link.from();
 		int to = link.to();
 
@@ -4919,6 +4931,11 @@ bool Rtabmap::addLink(const Link & link)
 					iter->second = jter->second;
 				}
 			}
+			if(!_optimizeFromGraphEnd)
+			{
+				_mapCorrection = _optimizedPoses.rbegin()->second * _memory->getSignature(_optimizedPoses.rbegin()->first)->getPose().inverse();
+			}
+
 			std::map<int, Transform> tmp;
 			// Update also the links if some have been added in WM
 			_memory->getMetricConstraints(uKeysSet(_optimizedPoses), tmp, _constraints, false);
@@ -4927,6 +4944,67 @@ bool Rtabmap::addLink(const Link & link)
 
 			return true;
 		}
+	}
+	else // localization mode
+	{
+		int oldestId = link.from()>link.to()?link.to():link.from();
+		int newestId = link.from()<link.to()?link.to():link.from();
+
+		// Note that graph verification is not implemented here
+		if(_memory->getSignature(oldestId) == 0)
+		{
+			UERROR("Link's id %d is not in working memory", oldestId);
+			return false;
+		}
+		if(_optimizedPoses.find(oldestId) == _optimizedPoses.end())
+		{
+			UERROR("Link's id %d is not in the optimized graph", oldestId);
+			return false;
+		}
+		if(_optimizeFromGraphEnd)
+		{
+			UERROR("Adding link with %s=true in localization mode is not supported.", Parameters::kRGBDOptimizeFromGraphEnd().c_str());
+			return false;
+		}
+		if(_odomCacheAddLink.find(newestId) == _odomCacheAddLink.end())
+		{
+			if(!_odomCacheAddLink.empty())
+			{
+				UERROR("Link's id %d is not in the odometry cache (oldest=%d, newest=%d, %s=%d)",
+						newestId,
+						_odomCacheAddLink.begin()->first,
+						_odomCacheAddLink.rbegin()->first,
+						Parameters::kRGBDMaxOdomCacheSize().c_str(),
+						_maxOdomCacheSize);
+			}
+			else
+			{
+				UERROR("Link's id %d is not in the odometry cache (%s=%d).",
+						newestId,
+						Parameters::kRGBDMaxOdomCacheSize().c_str(),
+						_maxOdomCacheSize);
+			}
+			return false;
+		}
+		Transform odomPose = _odomCacheAddLink.find(newestId)->second;
+		if(oldestId == link.from())
+		{
+			_lastLocalizationPose = _optimizedPoses.at(link.from()) * link.transform();
+		}
+		else
+		{
+			_lastLocalizationPose = _optimizedPoses.at(link.to()) * link.transform().inverse();
+		}
+		UERROR("Set _lastLocalizationPose=%s", _lastLocalizationPose.prettyPrint().c_str());
+		if(_graphOptimizer->isSlam2d())
+		{
+			// transform constraint to 2D
+			_lastLocalizationPose = _lastLocalizationPose.to3DoF();
+		}
+		_mapCorrection = _lastLocalizationPose * odomPose.inverse();
+		_lastLocalizationNodeId = oldestId;
+		return true;
+	}
 	return false;
 }
 

guilib/src/MainWindow.cpp

@@ -1340,11 +1340,11 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 	{
 		_odometryReceived = true;
 		// update camera position
-		if(odom.data().cameraModels().size() && !odom.data().cameraModels()[0].localTransform().isNull())
+		if(odom.data().cameraModels().size() && odom.data().cameraModels()[0].isValidForProjection())
 		{
 			_cloudViewer->updateCameraFrustums(_odometryCorrection*odom.pose(), odom.data().cameraModels());
 		}
-		else if(!odom.data().stereoCameraModel().localTransform().isNull())
+		else if(odom.data().stereoCameraModel().isValidForProjection())
 		{
 			_cloudViewer->updateCameraFrustum(_odometryCorrection*odom.pose(), odom.data().stereoCameraModel());
 		}
@@ -2063,11 +2063,11 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 			{
 				if(poses.rbegin()->first == stat.getLastSignatureData().id())
 				{
-					if(stat.getLastSignatureData().sensorData().cameraModels().size() && !stat.getLastSignatureData().sensorData().cameraModels()[0].localTransform().isNull())
+					if(stat.getLastSignatureData().sensorData().cameraModels().size() && stat.getLastSignatureData().sensorData().cameraModels()[0].isValidForProjection())
 					{
 						_cloudViewer->updateCameraFrustums(poses.rbegin()->second, stat.getLastSignatureData().sensorData().cameraModels());
 					}
-					else if(!stat.getLastSignatureData().sensorData().stereoCameraModel().localTransform().isNull())
+					else if(stat.getLastSignatureData().sensorData().stereoCameraModel().isValidForProjection())
 					{
 						_cloudViewer->updateCameraFrustum(poses.rbegin()->second, stat.getLastSignatureData().sensorData().stereoCameraModel());
 					}