Rtabmap: added addLink() function

corelib/include/rtabmap/core/Rtabmap.h

@@ -174,6 +174,7 @@ public:
 			bool interSession = true,
 			const ProgressState * state = 0);
 	int refineLinks();
+	bool addLink(const Link & link);
 	cv::Mat getInformation(const cv::Mat & covariance) const;
 
 	int getPathStatus() const {return _pathStatus;} // -1=failed 0=idle/executing 1=success

corelib/src/Rtabmap.cpp

@@ -4770,6 +4770,168 @@ int Rtabmap::refineLinks()
 	return (int)linksRefined.size();
 }
 
+bool Rtabmap::addLink(const Link & link)
+{
+	const Transform & t = link.transform();
+	if(!_rgbdSlamMode)
+	{
+		UERROR("Adding new link can be done only in RGBD-SLAM mode.");
+		return false;
+	}
+	if(!_memory)
+	{
+		UERROR("Memory is not initialized.");
+		return false;
+	}
+	if(t.isNull())
+	{
+		UERROR("Link's transform is null!");
+		return false;
+	}
+	if(_memory->getSignature(link.from()) == 0)
+	{
+		UERROR("Link's \"from id\" %d is not in working memory", link.from());
+		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(
+					optimizedPoses,
+					links,
+					maxLinearErrorRatio,
+					maxAngularErrorRatio,
+					maxLinearError,
+					maxAngularError,
+					&maxLinearLink,
+					&maxAngularLink);
+			if(maxLinearLink)
+			{
+				UINFO("Max optimization linear error = %f m (link %d->%d)", maxLinearError, maxLinearLink->from(), maxLinearLink->to());
+				if(maxLinearErrorRatio > _optimizationMaxError)
+				{
+					msg = uFormat("Rejecting edge %d->%d because "
+							  "graph error is too large after optimization (%f m for edge %d->%d with ratio %f > std=%f m). "
+							  "\"%s\" is %f.",
+							  from,
+							  to,
+							  maxLinearError,
+							  maxLinearLink->from(),
+							  maxLinearLink->to(),
+							  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
+		{
+			msg = uFormat("Rejecting edge %d->%d because graph optimization has failed!",
+					  from,
+					  to);
+		}
+		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;
+			}
+		}
+		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;
+	}
+	return false;
+}
+
 cv::Mat Rtabmap::getInformation(const cv::Mat & covariance) const
 {
 	cv::Mat information = covariance.inv();