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

24
corelib/src/Optimizer.cpp
View File

@@ -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);
@@ -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)
{

41
corelib/src/Rtabmap.cpp
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
{

16
corelib/src/Transform.cpp
View File

@@ -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
@@ -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