i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +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

3
corelib/include/rtabmap/core/Optimizer.h
View File

@@ -79,8 +79,7 @@ public:
const std::map<int, Transform> & posesIn, const std::map<int, Transform> & posesIn,
const std::multimap<int, Link> & linksIn, const std::multimap<int, Link> & linksIn,
std::map<int, Transform> & posesOut, std::map<int, Transform> & posesOut,
std::multimap<int, Link> & linksOut, std::multimap<int, Link> & linksOut) const;
bool adjustPosesWithConstraints = true) const;
public: public:
virtual ~Optimizer() {} virtual ~Optimizer() {}

24
corelib/src/Optimizer.cpp
View File

@@ -190,8 +190,7 @@ void Optimizer::getConnectedGraph(
const std::map<int, Transform> & posesIn, const std::map<int, Transform> & posesIn,
const std::multimap<int, Link> & linksIn, const std::multimap<int, Link> & linksIn,
std::map<int, Transform> & posesOut, std::map<int, Transform> & posesOut,
std::multimap<int, Link> & linksOut, std::multimap<int, Link> & linksOut) const
bool adjustPosesWithConstraints) const
{ {
UDEBUG("IN: fromId=%d poses=%d links=%d priorsIgnored=%d landmarksIgnored=%d", fromId, (int)posesIn.size(), (int)linksIn.size(), priorsIgnored()?1:0, landmarksIgnored()?1:0); UDEBUG("IN: fromId=%d poses=%d links=%d priorsIgnored=%d landmarksIgnored=%d", fromId, (int)posesIn.size(), (int)linksIn.size(), priorsIgnored()?1:0, landmarksIgnored()?1:0);
UASSERT(fromId>0); UASSERT(fromId>0);
@@ -245,31 +244,24 @@ void Optimizer::getConnectedGraph(
{ {
if(!uContains(posesOut, toId)) if(!uContains(posesOut, toId))
{ {
if(adjustPosesWithConstraints) 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(isSlam2d() && kter->second.type() == Link::kLandmark && toId>0) if(poseToIn.is3DoF())
{ {
Transform t;
if(kter->second.from()==currentId)
{
t = kter->second.transform();
}
else
{
t = kter->second.transform().inverse();
}
posesOut.insert(std::make_pair(toId, (posesOut.at(currentId) * t).to3DoF())); posesOut.insert(std::make_pair(toId, (posesOut.at(currentId) * t).to3DoF()));
} }
else else
{ {
Transform t = posesOut.at(currentId) * (kter->second.from()==currentId?kter->second.transform():kter->second.transform().inverse()); posesOut.insert(std::make_pair(toId, (posesOut.at(currentId) * t).to4DoF()));
posesOut.insert(std::make_pair(toId, t));
} }
} }
else else
{ {
posesOut.insert(*posesIn.find(toId)); posesOut.insert(std::make_pair(toId, posesOut.at(currentId)* t));
} }
// add prior links // add prior links
for(std::multimap<int, Link>::const_iterator pter=linksIn.find(toId); pter!=linksIn.end() && pter->first==toId; ++pter) for(std::multimap<int, Link>::const_iterator pter=linksIn.find(toId); pter!=linksIn.end() && pter->first==toId; ++pter)
{ {

41
corelib/src/Rtabmap.cpp
View File

@@ -3149,20 +3149,12 @@ bool Rtabmap::process(
if(iterPose != _optimizedPoses.end() && poses.find(iterPose->first) == poses.end()) if(iterPose != _optimizedPoses.end() && poses.find(iterPose->first) == poses.end())
{ {
poses.insert(*iterPose); 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)));
// make the poses in the map fixed 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);
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());
}
} }
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::map<int, Transform> posesOut;
std::multimap<int, Link> edgeConstraintsOut; std::multimap<int, Link> edgeConstraintsOut;
@@ -3170,14 +3162,20 @@ bool Rtabmap::process(
UDEBUG("priorsIgnored was %s", priorsIgnored?"true":"false"); UDEBUG("priorsIgnored was %s", priorsIgnored?"true":"false");
_graphOptimizer->setPriorsIgnored(false); //temporary set false to use priors above to fix nodes of the map _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. // 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; cv::Mat locOptCovariance;
std::map<int, Transform> optPoses; std::map<int, Transform> optPoses;
if(poses.lower_bound(1) != poses.end() && if(!posesOut.empty() &&
_odomCachePoses.lower_bound(1) != poses.end() && posesOut.begin()->first < _odomCachePoses.begin()->first)
poses.lower_bound(1)->first < _odomCachePoses.lower_bound(1)->first)
{ {
optPoses = _graphOptimizer->optimize(poses.lower_bound(1)->first, posesOut, edgeConstraintsOut, locOptCovariance); optPoses = _graphOptimizer->optimize(posesOut.begin()->first, posesOut, edgeConstraintsOut, locOptCovariance);
} }
else else
{ {
@@ -3321,13 +3319,12 @@ bool Rtabmap::process(
UDEBUG("priorsIgnored was %s", priorsIgnored?"true":"false"); UDEBUG("priorsIgnored was %s", priorsIgnored?"true":"false");
_graphOptimizer->setPriorsIgnored(false); //temporary set false to use priors above to fix nodes of the map _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. // 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(); optPoses.clear();
if(poses.lower_bound(1) != poses.end() && if(!posesOut.empty() &&
_odomCachePoses.lower_bound(1) != poses.end() && posesOut.begin()->first < _odomCachePoses.begin()->first)
poses.lower_bound(1)->first < _odomCachePoses.lower_bound(1)->first)
{ {
optPoses = _graphOptimizer->optimize(poses.lower_bound(1)->first, posesOut, edgeConstraintsOut, locOptCovariance); optPoses = _graphOptimizer->optimize(posesOut.begin()->first, posesOut, edgeConstraintsOut, locOptCovariance);
} }
else else
{ {

16
corelib/src/Transform.cpp
View File

@@ -211,14 +211,24 @@ Transform Transform::to3DoF() const
{ {
float x,y,z,roll,pitch,yaw; float x,y,z,roll,pitch,yaw;
this->getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); this->getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
return Transform(x,y,0, 0,0,yaw); float A = std::cos(yaw);
float B = std::sin(yaw);
return Transform(
A,-B, 0, x,
B, A, 0, y,
0, 0, 1, 0);
} }
Transform Transform::to4DoF() const Transform Transform::to4DoF() const
{ {
float x,y,z,roll,pitch,yaw; float x,y,z,roll,pitch,yaw;
this->getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); this->getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
return Transform(x,y,z, 0,0,yaw); float A = std::cos(yaw);
float B = std::sin(yaw);
return Transform(
A,-B, 0, x,
B, A, 0, y,
0, 0, 1, z);
} }
bool Transform::is3DoF() const bool Transform::is3DoF() const
@@ -232,7 +242,7 @@ bool Transform::is4DoF() const
r23() == 0.0 && r23() == 0.0 &&
r31() == 0.0 && r31() == 0.0 &&
r32() == 0.0 && r32() == 0.0 &&
r33() == 0.0; r33() == 1.0;
} }
cv::Mat Transform::rotationMatrix() const cv::Mat Transform::rotationMatrix() const