Prioritize neighbor links in Optimizer::getConnectedGraph() (#1610)

* Prioritize neighbor links in Optimizer::getConnectedGraph() to avoid odometry jumps.

* Fixed landmark order

* report: added search for first valid id

* Fixed non-neighbor comparison logic

* RGBD/LocalizationPriorError can now be 0 (disabled) to avoid using priors to fix the graph (https://github.com/introlab/rtabmap_ros/issues/1371)

* amend previous commit

corelib/include/rtabmap/core/Parameters.h

@@ -399,7 +399,7 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(RGBD, LoopCovLimited,               bool, false,  "Limit covariance of non-neighbor links to minimum covariance of neighbor links. In other words, if covariance of a loop closure link is smaller than the minimum covariance of odometry links, its covariance is set to minimum covariance of odometry links.");
     RTABMAP_PARAM(RGBD, MaxOdomCacheSize,             int,  10,      uFormat("Maximum odometry cache size. Used only in localization mode (when %s=false). This is used to get smoother localizations and to verify localization transforms (when %s!=0) to make sure we don't teleport to a location very similar to one we previously localized on. Set 0 to disable caching.", kMemIncrementalMemory().c_str(), kRGBDOptimizeMaxError().c_str()));
     RTABMAP_PARAM(RGBD, LocalizationSmoothing,        bool, true,    uFormat("Adjust localization constraints based on optimized odometry cache poses (when %s>0).",  kRGBDMaxOdomCacheSize().c_str()));
-    RTABMAP_PARAM(RGBD, LocalizationPriorError,       double, 0.001, uFormat("The corresponding variance (error x error) set to priors of the map's poses during localization (when %s>0).",  kRGBDMaxOdomCacheSize().c_str()));
+    RTABMAP_PARAM(RGBD, LocalizationPriorError,       double, 0.001, uFormat("The corresponding variance (error x error) set to priors of the map's poses during localization (when %s>0). Set to 0 to only fix the first map's pose linked in odometry cache (i.e., other map's poses will be allowed to move during the optimization).",  kRGBDMaxOdomCacheSize().c_str()));
     RTABMAP_PARAM(RGBD, LocalizationSecondTryWithoutProximityLinks,  bool, true, uFormat("When localization is rejected by graph optimization validation, try a second time without proximity links if landmark or loop closure links are also present in odometry cache (see %s). If it succeeds, the proximity links are removed. This assumes that global loop closure and landmark links are more accurate than proximity links.", kRGBDMaxOdomCacheSize().c_str()));
 
     // Local/Proximity loop closure detection

corelib/src/Optimizer.cpp

@@ -185,6 +185,48 @@ Optimizer * Optimizer::create(Optimizer::Type type, const ParametersMap & parame
 	return optimizer;
 }
 
+class LinkIdKey
+{
+public:
+LinkIdKey(int id, Link::Type type) :
+		id_(id),
+		type_(type) {}
+	bool operator<(const LinkIdKey & k) const
+	{
+		// landmark, sort by smallest to largest landmark id, after normal links
+		if(id_ < 0 && k.id_ < 0)
+		{
+			return id_ > k.id_;
+		}
+		else if(id_ < 0) {
+			return false;
+		}
+		else if(k.id_ < 0) {
+			return true;
+		}
+
+		if(type_ == Link::kNeighbor && k.type_ != Link::kNeighbor)
+		{
+			return true;
+		}
+		else if(type_ != Link::kNeighbor && k.type_ == Link::kNeighbor)
+		{
+			return false;
+		}
+		else if(type_ == Link::kNeighborMerged && k.type_ != Link::kNeighbor && k.type_ != Link::kNeighborMerged)
+		{
+			return true;
+		}
+		else
+		{
+			// normal link, sort by smallest to largest id
+			return id_ < k.id_;
+		}
+	}
+	int id_;
+	Link::Type type_;
+};
+
 void Optimizer::getConnectedGraph(
 		int fromId,
 		const std::map<int, Transform> & posesIn,
@@ -199,8 +241,8 @@ void Optimizer::getConnectedGraph(
 	posesOut.clear();
 	linksOut.clear();
 
-	std::set<int> nextPoses;
+	std::map<LinkIdKey, Transform> nextPoses;
-	nextPoses.insert(fromId);
+	nextPoses.insert(std::make_pair(LinkIdKey(fromId, Link::kUndef), posesIn.find(fromId)->second));
 	std::multimap<int, std::pair<int, Link::Type> > biLinks;
 	for(std::multimap<int, Link>::const_iterator iter=linksIn.begin(); iter!=linksIn.end(); ++iter)
 	{
@@ -216,20 +258,25 @@ void Optimizer::getConnectedGraph(
 
 	while(nextPoses.size())
 	{
-		int currentId = *nextPoses.rbegin(); // fill up all nodes before landmarks
+		// Fill up all nodes before landmarks
-		nextPoses.erase(*nextPoses.rbegin());
+		// For nodes, fill up all neightbor nodes before loop closure ones
+		int currentId = nextPoses.begin()->first.id_; 
+		Transform currentPose = nextPoses.begin()->second;
+		nextPoses.erase(nextPoses.begin());
 
-		if(posesOut.empty())
+		if(posesOut.find(currentId) != posesOut.end()) {
+			// Already added from priority list
+			continue;
+		}
+
+		posesOut.insert(std::make_pair(currentId, currentPose));
+
+		// add prior links
+		for(std::multimap<int, Link>::const_iterator pter=linksIn.find(currentId); pter!=linksIn.end() && pter->first==currentId; ++pter)
 		{
-			posesOut.insert(std::make_pair(currentId, posesIn.find(currentId)->second));
+			if(pter->second.from() == pter->second.to() && (!priorsIgnored() || pter->second.type() != Link::kPosePrior))
-
-			// add prior links
-			for(std::multimap<int, Link>::const_iterator pter=linksIn.find(currentId); pter!=linksIn.end() && pter->first==currentId; ++pter)
 			{
-				if(pter->second.from() == pter->second.to() && (!priorsIgnored() || pter->second.type() != Link::kPosePrior))
+				linksOut.insert(*pter);
-				{
-					linksOut.insert(*pter);
-				}
 			}
 		}
 
@@ -240,52 +287,42 @@ void Optimizer::getConnectedGraph(
 			if(posesIn.find(toId) != posesIn.end() && (!landmarksIgnored() || toId>0))
 			{
 				std::multimap<int, Link>::const_iterator kter = graph::findLink(linksIn, currentId, toId, true, type);
-				if(nextPoses.find(toId) == nextPoses.end())
+				UASSERT(kter!=linksIn.end());
+				if(!uContains(posesOut, toId))
 				{
-					if(!uContains(posesOut, toId))
+					const Transform & poseToIn = posesIn.at(toId);
+					Transform t = kter->second.from()==currentId?kter->second.transform():kter->second.transform().inverse();
+					Transform pose;
+					if(isSlam2d() && kter->second.type() == Link::kLandmark && toId>0 && (poseToIn.is3DoF() || poseToIn.is4DoF()))
 					{
-						const Transform & poseToIn = posesIn.at(toId);
+						if(poseToIn.is3DoF())
-						Transform t = kter->second.from()==currentId?kter->second.transform():kter->second.transform().inverse();
-						if(isSlam2d() && kter->second.type() == Link::kLandmark && toId>0 && (poseToIn.is3DoF() || poseToIn.is4DoF()))
 						{
-							if(poseToIn.is3DoF())
+							pose = (posesOut.at(currentId) * t).to3DoF();
-							{
-								posesOut.insert(std::make_pair(toId, (posesOut.at(currentId) * t).to3DoF()));
-							}
-							else
-							{
-								posesOut.insert(std::make_pair(toId, (posesOut.at(currentId) * t).to4DoF()));
-							}
 						}
 						else
 						{
-							posesOut.insert(std::make_pair(toId, posesOut.at(currentId)* t));
+							pose = (posesOut.at(currentId) * t).to4DoF();
 						}
-
+					}
-						// add prior links
+					else
-						for(std::multimap<int, Link>::const_iterator pter=linksIn.find(toId); pter!=linksIn.end() && pter->first==toId; ++pter)
+					{
-						{
+						pose = posesOut.at(currentId)* t;
-							if(pter->second.from() == pter->second.to() && (!priorsIgnored() || pter->second.type() != Link::kPosePrior))
-							{
-								linksOut.insert(*pter);
-							}
-						}
-
-						nextPoses.insert(toId);
 					}
 
-					// only add unique links
+					nextPoses.insert(std::make_pair(LinkIdKey(toId, type), pose));
-					if(graph::findLink(linksOut, currentId, toId, true, kter->second.type()) == linksOut.end())
+				}
+
+				// only add unique links
+				if(graph::findLink(linksOut, currentId, toId, true, kter->second.type()) == linksOut.end())
+				{
+					if(kter->second.to() < 0)
 					{
-						if(kter->second.to() < 0)
+						// For landmarks, make sure fromId is the landmark
-						{
+						linksOut.insert(std::make_pair(kter->second.to(), kter->second.inverse()));
-							// For landmarks, make sure fromId is the landmark
+					}
-							linksOut.insert(std::make_pair(kter->second.to(), kter->second.inverse()));
+					else
-						}
+					{
-						else
+						linksOut.insert(*kter);
-						{
-							linksOut.insert(*kter);
-						}
 					}
 				}
 			}

corelib/src/Rtabmap.cpp

@@ -631,8 +631,8 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kRGBDLocalizationSmoothing(), _localizationSmoothing);
 	double localizationPriorError = Parameters::defaultRGBDLocalizationPriorError();
 	Parameters::parse(parameters, Parameters::kRGBDLocalizationPriorError(), localizationPriorError);
-	UASSERT(localizationPriorError>0.0);
+	UASSERT(localizationPriorError>=0.0);
-	_localizationPriorInf = 1.0/(localizationPriorError*localizationPriorError);
+	_localizationPriorInf = localizationPriorError>0?1.0/(localizationPriorError*localizationPriorError):0.0;
 	Parameters::parse(parameters, Parameters::kRGBDLocalizationSecondTryWithoutProximityLinks(), _localizationSecondTryWithoutProximityLinks);
 	Parameters::parse(parameters, Parameters::kRGBDProximityGlobalScanMap(), _createGlobalScanMap);
 
@@ -3258,25 +3258,39 @@ bool Rtabmap::process(
 				{
 					constraints.insert(std::make_pair(iter->second.from(), iter->second));
 				}
+
 				cv::Mat priorInfMat = cv::Mat::eye(6,6, CV_64FC1)*_localizationPriorInf;
+				std::list<int> addedPriors;
 				for(std::multimap<int, Link>::iterator iter=constraints.begin(); iter!=constraints.end(); ++iter)
 				{
 					std::map<int, Transform>::iterator iterPose = _optimizedPoses.find(iter->second.to());
 					if(iterPose != _optimizedPoses.end() && poses.find(iterPose->first) == poses.end())
 					{
 						poses.insert(*iterPose);
-						// make the poses in the map fixed
+						if(_localizationPriorInf > 0)
-						constraints.insert(std::make_pair(iterPose->first, Link(iterPose->first, iterPose->first, Link::kPosePrior, iterPose->second, priorInfMat)));
+						{
-						UDEBUG("Constraint %d->%d: %s (type=%s, var=%f)", iterPose->first, iterPose->first, iterPose->second.prettyPrint().c_str(), Link::typeName(Link::kPosePrior).c_str(), 1./_localizationPriorInf);
+							// make the poses in the map fixed
+							constraints.insert(std::make_pair(iterPose->first, Link(iterPose->first, iterPose->first, Link::kPosePrior, iterPose->second, priorInfMat)));
+							UDEBUG("Constraint %d->%d: %s (type=%s, var=%f)", iterPose->first, iterPose->first, iterPose->second.prettyPrint().c_str(), Link::typeName(Link::kPosePrior).c_str(), 1./_localizationPriorInf);
+							addedPriors.push_back(iterPose->first);
+						}
 					}
 					UDEBUG("Constraint %d->%d: %s (type=%s, var = %f %f)", iter->second.from(), iter->second.to(), iter->second.transform().prettyPrint().c_str(), iter->second.typeName().c_str(), iter->second.transVariance(), iter->second.rotVariance());
 				}
+				if(addedPriors.size() == 1) {
+					// When there is only one map node, remove the prior to use fixed constraint in g2o (https://github.com/introlab/rtabmap_ros/issues/1371)
+					UDEBUG("Currently localizing on a single map node, removing prior on %d", addedPriors.front());
+					constraints.erase(graph::findLink(constraints, addedPriors.front(), addedPriors.front(), false, Link::kPosePrior));
+				}
 
 				std::map<int, Transform> posesOut;
 				std::multimap<int, Link> edgeConstraintsOut;
 				bool priorsIgnored = _graphOptimizer->priorsIgnored();
-				UDEBUG("priorsIgnored was %s", priorsIgnored?"true":"false");
+				if(_localizationPriorInf > 0)
-				_graphOptimizer->setPriorsIgnored(false); //temporary set false to use priors above to fix nodes of the map
+				{
+					UDEBUG("priorsIgnored was %s", priorsIgnored?"true":"false");
+					_graphOptimizer->setPriorsIgnored(false); //temporary set false to use priors above to fix nodes of the map
+				}
 				// If slam2d: get connected graph while keeping original roll,pitch,z values.
 				_graphOptimizer->getConnectedGraph(signature->id(), poses, constraints, posesOut, edgeConstraintsOut);
 				if(ULogger::level() == ULogger::kDebug)

corelib/src/optimizer/OptimizerG2O.cpp

@@ -1004,8 +1004,8 @@ std::map<int, Transform> OptimizerG2O::optimize(
 						g2o::EdgeSE3 * e = new g2o::EdgeSE3();
 						g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
 						g2o::VertexSE3* v2 = (g2o::VertexSE3*)optimizer.vertex(id2);
-						UASSERT(v1 != 0);
+						UASSERT_MSG(v1 != 0, uFormat("v1=%d v2=%d", id1, id2).c_str());
-						UASSERT(v2 != 0);
+						UASSERT_MSG(v2 != 0, uFormat("v1=%d v2=%d", id1, id2).c_str());
 						e->setVertex(0, v1);
 						e->setVertex(1, v2);
 						e->setMeasurement(constraint);

guilib/src/ui/preferencesDialog.ui

@@ -12888,7 +12888,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        <item row="21" column="1">
                         <widget class="QLabel" name="label_space2_19">
                          <property name="text">
-                          <string>Localization prior error. Used only in localization mode. The corresponding variance (error x error) set to priors of the map's poses during localization.</string>
+                          <string>Localization prior error. Used only in localization mode. The corresponding variance (error x error) set to priors of the map's poses during localization. Set to 0 to only fix the first map's pose linked in odometry cache (i.e., other map's poses will be allowed to move during the optimization).</string>
                          </property>
                          <property name="wordWrap">
                           <bool>true</bool>

tools/Report/main.cpp

@@ -908,7 +908,16 @@ int main(int argc, char * argv[])
 					{
 						std::map<int, Transform> posesOut;
 						std::multimap<int, Link> linksOut;
+						// Find first valid id
 						int firstId = *ids.begin();
+						for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
+						{
+							if(iter->second.type() == Link::kNeighbor || iter->second.type() == Link::kNeighborMerged)
+							{
+								firstId = iter->first;
+								break;
+							}
+						}
 						rtabmap::Optimizer * optimizer = rtabmap::Optimizer::create(params);
 						bool useOdomGravity = Parameters::defaultMemUseOdomGravity();
 						Parameters::parse(params, Parameters::kMemUseOdomGravity(), useOdomGravity);