i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-13 05:19:51 +08:00
Code Issues Packages Projects Releases Wiki Activity

Localization: updated how poses with fixed priors are fed to optimization for faster convergence (see https://github.com/introlab/rtabmap_ros/issues/1371)

Browse Source
This commit is contained in:
matlabbe
2025-11-23 13:47:15 -08:00
parent 1edb6a8647
commit 3093df8e71
1 changed files with 5 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Rtabmap.cpp
+5 -7
View File
@@ -3260,7 +3260,7 @@ bool Rtabmap::process(
} }
cv::Mat priorInfMat = cv::Mat::eye(6,6, CV_64FC1)*_localizationPriorInf; cv::Mat priorInfMat = cv::Mat::eye(6,6, CV_64FC1)*_localizationPriorInf;
std::list<int> addedPriors; std::map<int, Transform> addedPriors;
for(std::multimap<int, Link>::iterator iter=constraints.begin(); iter!=constraints.end(); ++iter) for(std::multimap<int, Link>::iterator iter=constraints.begin(); iter!=constraints.end(); ++iter)
{ {
std::map<int, Transform>::iterator iterPose = _optimizedPoses.find(iter->second.to()); std::map<int, Transform>::iterator iterPose = _optimizedPoses.find(iter->second.to());
@@ -3271,15 +3271,10 @@ bool Rtabmap::process(
// make the poses in the map fixed // make the poses in the map fixed
constraints.insert(std::make_pair(iterPose->first, Link(iterPose->first, iterPose->first, Link::kPosePrior, iterPose->second, priorInfMat))); 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); 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); addedPriors.insert(*iterPose);
} }
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()); 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::map<int, Transform> posesOut;
std::multimap<int, Link> edgeConstraintsOut; std::multimap<int, Link> edgeConstraintsOut;
@@ -3290,6 +3285,9 @@ bool Rtabmap::process(
// If slam2d: get connected graph while keeping original roll,pitch,z values. // If slam2d: get connected graph while keeping original roll,pitch,z values.
_graphOptimizer->getConnectedGraph(signature->id(), poses, constraints, posesOut, edgeConstraintsOut); _graphOptimizer->getConnectedGraph(signature->id(), poses, constraints, posesOut, edgeConstraintsOut);
// Set back map's poses corresponding to priors so that g2o can converge correctly (https://github.com/introlab/rtabmap_ros/issues/1371)
uInsert(posesOut, addedPriors);
if(ULogger::level() == ULogger::kDebug) if(ULogger::level() == ULogger::kDebug)
{ {
for(std::map<int, Transform>::iterator iter=posesOut.begin(); iter!=posesOut.end(); ++iter) for(std::map<int, Transform>::iterator iter=posesOut.begin(); iter!=posesOut.end(); ++iter)