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

Neighbor link refining: set small variance if small displacement and variance=1

Browse Source
This commit is contained in:
matlabbe
2015-11-27 13:04:01 -05:00
parent ee81bfc1d7
commit 096684ed94
1 changed files with 63 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Rtabmap.cpp
+19 -7
View File
@@ -1012,23 +1012,34 @@ bool Rtabmap::process(
// Update optimizedPoses with the newly added node
Transform newPose;
if(signature->getLinks().size() == 1 &&
!smallDisplacement &&
_memory->isIncremental()) // ignore pose matching in localization mode
if(_poseScanMatching &&
signature->getLinks().size() == 1 &&
_memory->isIncremental() && // ignore pose matching in localization mode
rehearsedId == 0) // don't do it if rehearsal happened
{
int oldId = signature->getLinks().begin()->first;
const Signature * oldS = _memory->getSignature(oldId);
UASSERT(oldS != 0);
Transform guess = signature->getLinks().begin()->second.transform();
if(smallDisplacement)
{
if(signature->getLinks().begin()->second.transVariance() == 1)
{
// set small variance
UDEBUG("Set small variance. The robot is not moving.");
_memory->updateLink(signature->id(), oldId, guess, 0.0001, 0.0001);
}
}
else
{
//============================================================
// Scan matching
//============================================================
if(_poseScanMatching &&
!signature->sensorData().laserScanCompressed().empty() &&
rehearsedId == 0) // don't do it if rehearsal happened
if(!signature->sensorData().laserScanCompressed().empty())
{
UINFO("Odometry correction by scan matching");
Transform guess = signature->getLinks().begin()->second.transform();
double variance = 1.0;
int inliers = 0;
float inliersRatio = 0;
@@ -1077,6 +1088,7 @@ bool Rtabmap::process(
}
timeScanMatching = timer.ticks();
ULOGGER_INFO("timeScanMatching=%fs", timeScanMatching);
}
UASSERT(oldS->hasLink(signature->id()));
UASSERT(uContains(_optimizedPoses, oldId));