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

Added RGBD/ProximityMergedScanCovFactor parameter. Localization: fixed output height when Reg/Force3DoF=true but input poses are 6DoF. Transform: added is3DoF() and is4DoF() functions. GTSAM: when Reg/Force3DoF=true, copy input roll,pitch,z values for output poses. RegIcp: fixed working memory dir '~' conversion. RGBD/ProximityGlobalScanMap: fixed map::at error when some nodes don't have scans.

Browse Source
This commit is contained in:
matlabbe
2022-01-12 16:14:42 -05:00
parent a4ec95963e
commit fcec98105d
14 changed files with 122 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Optimizer.cpp
+17 -9
View File
@@ -190,7 +190,8 @@ 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) const
std::multimap<int, Link> & linksOut,
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);
UASSERT(fromId>0);
@@ -243,23 +244,30 @@ void Optimizer::getConnectedGraph(
{
if(!uContains(posesOut, toId))
{
if(isSlam2d() && kter->second.type() == Link::kLandmark && toId>0)
if(adjustPosesWithConstraints)
{
Transform t;
if(kter->second.from()==currentId)
if(isSlam2d() && kter->second.type() == Link::kLandmark && toId>0)
{
t = kter->second.transform();
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
{
t = kter->second.transform().inverse();
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).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(*posesIn.find(toId));
}
// add prior links
for(std::multimap<int, Link>::const_iterator pter=linksIn.find(toId); pter!=linksIn.end() && pter->first==toId; ++pter)