Skip to content

Commit

Permalink
Localization: Refactored getConnectedGraph() for 2d slam and tags (in…
Browse files Browse the repository at this point in the history 
…trolab/rtabmap_ros#1057). Transform: Fixed is3DoF() and is 4DoF()
  • Loading branch information
@matlabbe
matlabbe committed Nov 17, 2023
1 parent 757d3eb commit 9c56a42
Show file tree
Hide file tree
Showing 4 changed files with 41 additions and 43 deletions.
3 changes: 1 addition & 2 deletions corelib/include/rtabmap/core/Optimizer.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -79,8 +79,7 @@ class RTABMAP_CORE_EXPORT Optimizer
const std::map<int, Transform> & posesIn,
const std::multimap<int, Link> & linksIn,
std::map<int, Transform> & posesOut,
std::multimap<int, Link> & linksOut,
bool adjustPosesWithConstraints = true) const;
std::multimap<int, Link> & linksOut) const;

public:
virtual ~Optimizer() {}
Expand Down
24 changes: 8 additions & 16 deletions corelib/src/Optimizer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -190,8 +190,7 @@ void Optimizer::getConnectedGraph(
const std::map<int, Transform> & posesIn,
const std::multimap<int, Link> & linksIn,
std::map<int, Transform> & posesOut,
std::multimap<int, Link> & linksOut,
bool adjustPosesWithConstraints) const
std::multimap<int, Link> & linksOut) 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);
UASSERT(fromId>0);
Expand Down Expand Up @@ -245,31 +244,24 @@ void Optimizer::getConnectedGraph(
{
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()));
}
else
{
Transform t = posesOut.at(currentId) * (kter->second.from()==currentId?kter->second.transform():kter->second.transform().inverse());
posesOut.insert(std::make_pair(toId, t));
posesOut.insert(std::make_pair(toId, (posesOut.at(currentId) * t).to4DoF()));
}
}
else
{
posesOut.insert(*posesIn.find(toId));
posesOut.insert(std::make_pair(toId, posesOut.at(currentId)* t));
}

// add prior links
for(std::multimap<int, Link>::const_iterator pter=linksIn.find(toId); pter!=linksIn.end() && pter->first==toId; ++pter)
{
Expand Down
41 changes: 19 additions & 22 deletions corelib/src/Rtabmap.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3149,35 +3149,33 @@ 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;
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 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
{
Expand Down Expand Up @@ -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
{
Expand Down
16 changes: 13 additions & 3 deletions corelib/src/Transform.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -211,14 +211,24 @@ Transform Transform::to3DoF() const
{
float 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
{
float 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
Expand All @@ -232,7 +242,7 @@ bool Transform::is4DoF() const
r23() == 0.0 &&
r31() == 0.0 &&
r32() == 0.0 &&
r33() == 0.0;
r33() == 1.0;
}

cv::Mat Transform::rotationMatrix() const
Expand Down

0 comments on commit 9c56a42

Please sign in to comment.