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/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;
-	nextPoses.insert(fromId);
+	std::map<LinkIdKey, Transform> nextPoses;
+	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
-		nextPoses.erase(*nextPoses.rbegin());
+		// Fill up all nodes before landmarks
+		// 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));
-
-			// 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))
 			{
-				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);
-						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())
 						{
-							if(poseToIn.is3DoF())
-							{
-								posesOut.insert(std::make_pair(toId, (posesOut.at(currentId) * t).to3DoF()));
-							}
-							else
-							{
-								posesOut.insert(std::make_pair(toId, (posesOut.at(currentId) * t).to4DoF()));
-							}
+							pose = (posesOut.at(currentId) * t).to3DoF();
 						}
 						else
 						{
-							posesOut.insert(std::make_pair(toId, posesOut.at(currentId)* t));
+							pose = (posesOut.at(currentId) * t).to4DoF();
 						}
-
-						// add prior links
-						for(std::multimap<int, Link>::const_iterator pter=linksIn.find(toId); pter!=linksIn.end() && pter->first==toId; ++pter)
-						{
-							if(pter->second.from() == pter->second.to() && (!priorsIgnored() || pter->second.type() != Link::kPosePrior))
-							{
-								linksOut.insert(*pter);
-							}
-						}
-
-						nextPoses.insert(toId);
+					}
+					else
+					{
+						pose = posesOut.at(currentId)* t;
 					}
 
-					// only add unique links
-					if(graph::findLink(linksOut, currentId, toId, true, kter->second.type()) == linksOut.end())
+					nextPoses.insert(std::make_pair(LinkIdKey(toId, type), pose));
+				}
+
+				// 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()));
-						}
-						else
-						{
-							linksOut.insert(*kter);
-						}
+						// For landmarks, make sure fromId is the landmark
+						linksOut.insert(std::make_pair(kter->second.to(), kter->second.inverse()));
+					}
+					else
+					{
+						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);
-	_localizationPriorInf = 1.0/(localizationPriorError*localizationPriorError);
+	UASSERT(localizationPriorError>=0.0);
+	_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
-						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);
+						if(_localizationPriorInf > 0)
+						{
+							// 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");
-				_graphOptimizer->setPriorsIgnored(false); //temporary set false to use priors above to fix nodes of the map
+				if(_localizationPriorInf > 0)
+				{
+					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(v2 != 0);
+						UASSERT_MSG(v1 != 0, uFormat("v1=%d v2=%d", id1, id2).c_str());
+						UASSERT_MSG(v2 != 0, uFormat("v1=%d v2=%d", id1, id2).c_str());
 						e->setVertex(0, v1);
 						e->setVertex(1, v2);
 						e->setMeasurement(constraint);