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

Fixed smallMovement when rehearsal is not trigdered at the same time

Browse Source
This commit is contained in:
matlabbe
2026-05-13 23:35:22 -07:00
parent fe5071b2fd
commit 49a8dcfe1a
1 changed files with 23 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Rtabmap.cpp
+23 -8
View File
@@ -1522,16 +1522,31 @@ bool Rtabmap::process(
Transform t;
if(_memory->isIncremental())
{
const std::multimap<int, Link> & links = signature->getLinks();
if(links.size() && links.begin()->second.type() == Link::kNeighbor)
// Check small motion if current node is not an intermediate node already
if(signature->getWeight() >= 0)
{
const Signature * s = _memory->getSignature(links.begin()->second.to());
UASSERT(s!=0);
// Check small motion if current node is not an intermediate node already
if(signature->getWeight() >= 0)
// It should contain only the query and its first (non-intermediate) neighbor (smaller id)
std::map<int, int> neighbors = _memory->getNeighborsId(signature->id(), 2, 0, true, true, true, true);
if(neighbors.size() == 2)
{
t = links.begin()->second.transform();
linkedToIntermediateNode = s->getWeight() == -1;
int nid = neighbors.begin()->first;
const std::multimap<int, Link> & links = signature->getLinks();
if(links.find(nid) != links.end())
{
// direct neighbor
t = links.find(nid)->second.transform();
}
else
{
// Use optimized poses to check how far it is from the latest non-intermediate node
std::map<int, Transform>::iterator niter = _optimizedPoses.find(nid);
if(niter != _optimizedPoses.end())
{
t = niter->second.inverse() * _mapCorrection * signature->getPose();
}
// not direct link, it means there are intermediate nodes
linkedToIntermediateNode = true;
}
}
}
}