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

Improved localization mode accuracy (decreasing jumps on consecutive loop closures or landmark detections). Updated usage of parameter RGBD/MaxOdomCacheSize (default 0->10)

Browse Source
This commit is contained in:
matlabbe
2021-12-10 17:48:36 -05:00
parent 4ab0090ecd
commit 8cf12c6135
6 changed files with 434 additions and 292 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Optimizer.cpp
+9 -9
View File
@@ -216,15 +216,15 @@ void Optimizer::getConnectedGraph(
while(nextPoses.size())
{
int fromId = *nextPoses.rbegin(); // fill up all nodes before landmarks
int currentId = *nextPoses.rbegin(); // fill up all nodes before landmarks
nextPoses.erase(*nextPoses.rbegin());
if(posesOut.empty())
{
posesOut.insert(std::make_pair(fromId, posesIn.find(fromId)->second));
posesOut.insert(std::make_pair(currentId, posesIn.find(currentId)->second));
// add prior links
for(std::multimap<int, Link>::const_iterator pter=linksIn.find(fromId); pter!=linksIn.end() && pter->first==fromId; ++pter)
for(std::multimap<int, Link>::const_iterator pter=linksIn.find(currentId); pter!=linksIn.end() && pter->first==currentId; ++pter)
{
if(pter->second.from() == pter->second.to() && (!priorsIgnored() || pter->second.type() != Link::kPosePrior))
{
@@ -233,12 +233,12 @@ void Optimizer::getConnectedGraph(
}
}
for(std::multimap<int, int>::const_iterator iter=biLinks.find(fromId); iter!=biLinks.end() && iter->first==fromId; ++iter)
for(std::multimap<int, int>::const_iterator iter=biLinks.find(currentId); iter!=biLinks.end() && iter->first==currentId; ++iter)
{
int toId = iter->second;
if(posesIn.find(toId) != posesIn.end() && (!landmarksIgnored() || toId>0))
{
std::multimap<int, Link>::const_iterator kter = graph::findLink(linksIn, fromId, toId);
std::multimap<int, Link>::const_iterator kter = graph::findLink(linksIn, currentId, toId);
if(nextPoses.find(toId) == nextPoses.end())
{
if(!uContains(posesOut, toId))
@@ -246,7 +246,7 @@ void Optimizer::getConnectedGraph(
if(isSlam2d() && kter->second.type() == Link::kLandmark && toId>0)
{
Transform t;
if(kter->second.from()==fromId)
if(kter->second.from()==currentId)
{
t = kter->second.transform();
}
@@ -254,11 +254,11 @@ void Optimizer::getConnectedGraph(
{
t = kter->second.transform().inverse();
}
posesOut.insert(std::make_pair(toId, (posesOut.at(fromId) * t).to3DoF()));
posesOut.insert(std::make_pair(toId, (posesOut.at(currentId) * t).to3DoF()));
}
else
{
Transform t = posesOut.at(fromId) * (kter->second.from()==fromId?kter->second.transform():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));
}
// add prior links
@@ -274,7 +274,7 @@ void Optimizer::getConnectedGraph(
}
// only add unique links
if(graph::findLink(linksOut, fromId, toId) == linksOut.end())
if(graph::findLink(linksOut, currentId, toId) == linksOut.end())
{
if(kter->second.to() < 0)
{