i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-06 18:17:47 +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/Optimizer.cpp
+8 -16
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)
{