i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-04 17:17: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
+22 -7
View File
@@ -1522,16 +1522,31 @@ bool Rtabmap::process(
Transform t; Transform t;
if(_memory->isIncremental()) if(_memory->isIncremental())
{ {
const std::multimap<int, Link> & links = signature->getLinks();
if(links.size() && links.begin()->second.type() == Link::kNeighbor)
{
const Signature * s = _memory->getSignature(links.begin()->second.to());
UASSERT(s!=0);
// Check small motion if current node is not an intermediate node already // Check small motion if current node is not an intermediate node already
if(signature->getWeight() >= 0) if(signature->getWeight() >= 0)
{ {
t = links.begin()->second.transform(); // It should contain only the query and its first (non-intermediate) neighbor (smaller id)
linkedToIntermediateNode = s->getWeight() == -1; std::map<int, int> neighbors = _memory->getNeighborsId(signature->id(), 2, 0, true, true, true, true);
if(neighbors.size() == 2)
{
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;
}
} }
} }
} }