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,145 +4792,217 @@ bool Rtabmap::addLink(const Link & link)
 		UERROR("Link's transform is null!");
 		return false;
 	}
-	if(_memory->getSignature(link.from()) == 0)
+	if(_memory->isIncremental())
 	{
-		UERROR("Link's \"from id\" %d is not in working memory", link.from());
+		if(_memory->getSignature(link.from()) == 0)
-		return false;
-	}
-	if(_memory->getSignature(link.to()) == 0)
-	{
-		UERROR("Link's \"to id\" %d is not in working memory", link.to());
-		return false;
-	}
-
-	std::map<int, Transform> poses;
-	std::multimap<int, Link> links;
-	this->getGraph(poses, links, true, true);
-
-	if(poses.find(link.from()) == poses.end())
-	{
-		UERROR("Link's \"from id\" %d is not in the graph", link.from());
-		return false;
-	}
-	if(poses.find(link.to()) == poses.end())
-	{
-		UERROR("Link's \"to id\" %d is not in the graph", link.to());
-		return false;
-	}
-	int from = link.from();
-	int to = link.to();
-
-	if(_optimizationMaxError > 0.0f)
-	{
-		//optimize the graph to see if the new constraint is globally valid
-		std::multimap<int, Link> linksIn = links;
-		linksIn.insert(std::make_pair(link.from(), link));
-		const Link * maxLinearLink = 0;
-		const Link * maxAngularLink = 0;
-		float maxLinearError = 0.0f;
-		float maxAngularError = 0.0f;
-		float maxLinearErrorRatio = 0.0f;
-		float maxAngularErrorRatio = 0.0f;
-		std::map<int, Transform> optimizedPoses;
-		UASSERT_MSG(poses.find(from) != poses.end(), uFormat("id=%d poses=%d links=%d", from, (int)poses.size(), (int)links.size()).c_str());
-		UASSERT_MSG(poses.find(to) != poses.end(), uFormat("id=%d poses=%d links=%d", to, (int)poses.size(), (int)links.size()).c_str());
-		_graphOptimizer->getConnectedGraph(from, poses, linksIn, optimizedPoses, links);
-		UASSERT_MSG(optimizedPoses.find(from) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", from, (int)optimizedPoses.size(), (int)links.size()).c_str());
-		UASSERT_MSG(optimizedPoses.find(to) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", to, (int)optimizedPoses.size(), (int)links.size()).c_str());
-		UASSERT(graph::findLink(links, from, to) != links.end());
-		int fromId = _optimizeFromGraphEnd?poses.rbegin()->first:poses.begin()->first;
-		optimizedPoses = _graphOptimizer->optimize(fromId, optimizedPoses, links);
-		std::string msg;
-		if(optimizedPoses.size())
 		{
-			graph::computeMaxGraphErrors(
+			UERROR("Link's \"from id\" %d is not in working memory", link.from());
-					optimizedPoses,
+			return false;
-					links,
+		}
-					maxLinearErrorRatio,
+		if(_memory->getSignature(link.to()) == 0)
-					maxAngularErrorRatio,
+		{
-					maxLinearError,
+			UERROR("Link's \"to id\" %d is not in working memory", link.to());
-					maxAngularError,
+			return false;
-					&maxLinearLink,
+		}
-					&maxAngularLink);
+
-			if(maxLinearLink)
+		std::map<int, Transform> poses;
+		std::multimap<int, Link> links;
+		this->getGraph(poses, links, true, false);
+
+		if(_memory->isIncremental())
+		{
+			if(poses.find(link.from()) == poses.end())
 			{
-				UINFO("Max optimization linear error = %f m (link %d->%d)", maxLinearError, maxLinearLink->from(), maxLinearLink->to());
+				UERROR("Link's \"from id\" %d is not in the graph", link.from());
-				if(maxLinearErrorRatio > _optimizationMaxError)
+				return false;
+			}
+			if(poses.find(link.to()) == poses.end())
+			{
+				UERROR("Link's \"to id\" %d is not in the graph", link.to());
+				return false;
+			}
+		}
+
+		int from = link.from();
+		int to = link.to();
+
+		if(_optimizationMaxError > 0.0f)
+		{
+			//optimize the graph to see if the new constraint is globally valid
+			std::multimap<int, Link> linksIn = links;
+			linksIn.insert(std::make_pair(link.from(), link));
+			const Link * maxLinearLink = 0;
+			const Link * maxAngularLink = 0;
+			float maxLinearError = 0.0f;
+			float maxAngularError = 0.0f;
+			float maxLinearErrorRatio = 0.0f;
+			float maxAngularErrorRatio = 0.0f;
+			std::map<int, Transform> optimizedPoses;
+			UASSERT_MSG(poses.find(from) != poses.end(), uFormat("id=%d poses=%d links=%d", from, (int)poses.size(), (int)links.size()).c_str());
+			UASSERT_MSG(poses.find(to) != poses.end(), uFormat("id=%d poses=%d links=%d", to, (int)poses.size(), (int)links.size()).c_str());
+			_graphOptimizer->getConnectedGraph(from, poses, linksIn, optimizedPoses, links);
+			UASSERT_MSG(optimizedPoses.find(from) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", from, (int)optimizedPoses.size(), (int)links.size()).c_str());
+			UASSERT_MSG(optimizedPoses.find(to) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", to, (int)optimizedPoses.size(), (int)links.size()).c_str());
+			UASSERT(graph::findLink(links, from, to) != links.end());
+			int fromId = _optimizeFromGraphEnd?poses.rbegin()->first:poses.begin()->first;
+			optimizedPoses = _graphOptimizer->optimize(fromId, optimizedPoses, links);
+			std::string msg;
+			if(optimizedPoses.size())
+			{
+				graph::computeMaxGraphErrors(
+						optimizedPoses,
+						links,
+						maxLinearErrorRatio,
+						maxAngularErrorRatio,
+						maxLinearError,
+						maxAngularError,
+						&maxLinearLink,
+						&maxAngularLink);
+				if(maxLinearLink)
 				{
-					msg = uFormat("Rejecting edge %d->%d because "
+					UINFO("Max optimization linear error = %f m (link %d->%d)", maxLinearError, maxLinearLink->from(), maxLinearLink->to());
-							  "graph error is too large after optimization (%f m for edge %d->%d with ratio %f > std=%f m). "
+					if(maxLinearErrorRatio > _optimizationMaxError)
-							  "\"%s\" is %f.",
+					{
-							  from,
+						msg = uFormat("Rejecting edge %d->%d because "
-							  to,
+								  "graph error is too large after optimization (%f m for edge %d->%d with ratio %f > std=%f m). "
-							  maxLinearError,
+								  "\"%s\" is %f.",
-							  maxLinearLink->from(),
+								  from,
-							  maxLinearLink->to(),
+								  to,
-							  maxLinearErrorRatio,
+								  maxLinearError,
-							  sqrt(maxLinearLink->transVariance()),
+								  maxLinearLink->from(),
-							  Parameters::kRGBDOptimizeMaxError().c_str(),
+								  maxLinearLink->to(),
-							  _optimizationMaxError);
+								  maxLinearErrorRatio,
+								  sqrt(maxLinearLink->transVariance()),
+								  Parameters::kRGBDOptimizeMaxError().c_str(),
+								  _optimizationMaxError);
+					}
+				}
+				else if(maxAngularLink)
+				{
+					UINFO("Max optimization angular error = %f deg (link %d->%d)", maxAngularError*180.0f/M_PI, maxAngularLink->from(), maxAngularLink->to());
+					if(maxAngularErrorRatio > _optimizationMaxError)
+					{
+						msg = uFormat("Rejecting edge %d->%d because "
+								  "graph error is too large after optimization (%f deg for edge %d->%d with ratio %f > std=%f deg). "
+								  "\"%s\" is %f m.",
+								  from,
+								  to,
+								  maxAngularError*180.0f/M_PI,
+								  maxAngularLink->from(),
+								  maxAngularLink->to(),
+								  maxAngularErrorRatio,
+								  sqrt(maxAngularLink->rotVariance()),
+								  Parameters::kRGBDOptimizeMaxError().c_str(),
+								  _optimizationMaxError);
+					}
 				}
 			}
-			else if(maxAngularLink)
+			else
 			{
-				UINFO("Max optimization angular error = %f deg (link %d->%d)", maxAngularError*180.0f/M_PI, maxAngularLink->from(), maxAngularLink->to());
+				msg = uFormat("Rejecting edge %d->%d because graph optimization has failed!",
-				if(maxAngularErrorRatio > _optimizationMaxError)
+						  from,
-				{
+						  to);
-					msg = uFormat("Rejecting edge %d->%d because "
+			}
-							  "graph error is too large after optimization (%f deg for edge %d->%d with ratio %f > std=%f deg). "
+			if(!msg.empty())
-							  "\"%s\" is %f m.",
+			{
-							  from,
+				UERROR("%s", msg.c_str());
-							  to,
+				return false;
-							  maxAngularError*180.0f/M_PI,
-							  maxAngularLink->from(),
-							  maxAngularLink->to(),
-							  maxAngularErrorRatio,
-							  sqrt(maxAngularLink->rotVariance()),
-							  Parameters::kRGBDOptimizeMaxError().c_str(),
-							  _optimizationMaxError);
-				}
 			}
 		}
 		else
 		{
-			msg = uFormat("Rejecting edge %d->%d because graph optimization has failed!",
+			int fromId = _optimizeFromGraphEnd?poses.rbegin()->first:poses.begin()->first;
-					  from,
+			poses = _graphOptimizer->optimize(fromId, poses, links, 0);
-					  to);
+			if(poses.empty())
-		}
-		if(!msg.empty())
-		{
-			UERROR("%s", msg.c_str());
-			return false;
-		}
-	}
-	else
-	{
-		int fromId = _optimizeFromGraphEnd?poses.rbegin()->first:poses.begin()->first;
-		poses = _graphOptimizer->optimize(fromId, poses, links, 0);
-		if(poses.empty())
-		{
-			UERROR("Rejecting edge %d->%d because graph optimization has failed!", from, to);
-			return false;
-		}
-	}
-
-	if(_memory->addLink(link, false))
-	{
-		// Update optimized poses
-		for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
-		{
-			std::map<int, Transform>::iterator jter = poses.find(iter->first);
-			if(jter != poses.end())
 			{
-				iter->second = jter->second;
+				UERROR("Rejecting edge %d->%d because graph optimization has failed!", from, to);
+				return false;
 			}
 		}
-		std::map<int, Transform> tmp;
-		// Update also the links if some have been added in WM
-		_memory->getMetricConstraints(uKeysSet(_optimizedPoses), tmp, _constraints, false);
-		// This will force rtabmap_ros to regenerate the global occupancy grid if there was one
-		_memory->save2DMap(cv::Mat(), 0, 0, 0);
 
+		if(_memory->addLink(link, false))
+		{
+			// Update optimized poses
+			for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
+			{
+				std::map<int, Transform>::iterator jter = poses.find(iter->first);
+				if(jter != poses.end())
+				{
+					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);
+			// This will force rtabmap_ros to regenerate the global occupancy grid if there was one
+			_memory->save2DMap(cv::Mat(), 0, 0, 0);
+
+			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());
 					}