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

Localization: Refactored getConnectedGraph() for 2d slam and tags (https://github.com/introlab/rtabmap_ros/issues/1057). Transform: Fixed is3DoF() and is 4DoF()

Browse Source
This commit is contained in:
matlabbe
2023-11-16 22:44:32 -08:00
parent 757d3eb90b
commit 9c56a429cc
4 changed files with 41 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Rtabmap.cpp
+19 -22
View File
@@ -3149,20 +3149,12 @@ bool Rtabmap::process(
if(iterPose != _optimizedPoses.end() && poses.find(iterPose->first) == poses.end())
{
poses.insert(*iterPose);
if(iterPose->first > 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 (type=%s)", iterPose->first, iterPose->first, Link::typeName(Link::kPosePrior).c_str());
}
// 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);
}
UDEBUG("Constraint %d->%d (type=%s, var = %f %f)", iter->second.from(), iter->second.to(), 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());
}
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
UDEBUG("Pose %d %s", iter->first, iter->second.prettyPrint().c_str());
}
std::map<int, Transform> posesOut;
std::multimap<int, Link> edgeConstraintsOut;
@@ -3170,14 +3162,20 @@ bool Rtabmap::process(
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, !_graphOptimizer->isSlam2d());
_graphOptimizer->getConnectedGraph(signature->id(), poses, constraints, posesOut, edgeConstraintsOut);
if(ULogger::level() == ULogger::kDebug)
{
for(std::map<int, Transform>::iterator iter=posesOut.begin(); iter!=posesOut.end(); ++iter)
{
UDEBUG("Pose %d %s", iter->first, iter->second.prettyPrint().c_str());
}
}
cv::Mat locOptCovariance;
std::map<int, Transform> optPoses;
if(poses.lower_bound(1) != poses.end() &&
_odomCachePoses.lower_bound(1) != poses.end() &&
poses.lower_bound(1)->first < _odomCachePoses.lower_bound(1)->first)
if(!posesOut.empty() &&
posesOut.begin()->first < _odomCachePoses.begin()->first)
{
optPoses = _graphOptimizer->optimize(poses.lower_bound(1)->first, posesOut, edgeConstraintsOut, locOptCovariance);
optPoses = _graphOptimizer->optimize(posesOut.begin()->first, posesOut, edgeConstraintsOut, locOptCovariance);
}
else
{
@@ -3321,13 +3319,12 @@ bool Rtabmap::process(
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, !_graphOptimizer->isSlam2d());
_graphOptimizer->getConnectedGraph(signature->id(), poses, constraints, posesOut, edgeConstraintsOut);
optPoses.clear();
if(poses.lower_bound(1) != poses.end() &&
_odomCachePoses.lower_bound(1) != poses.end() &&
poses.lower_bound(1)->first < _odomCachePoses.lower_bound(1)->first)
if(!posesOut.empty() &&
posesOut.begin()->first < _odomCachePoses.begin()->first)
{
optPoses = _graphOptimizer->optimize(poses.lower_bound(1)->first, posesOut, edgeConstraintsOut, locOptCovariance);
optPoses = _graphOptimizer->optimize(posesOut.begin()->first, posesOut, edgeConstraintsOut, locOptCovariance);
}
else
{