i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +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

114
corelib/src/Rtabmap.cpp
View File

@@ -1012,71 +1012,83 @@ bool Rtabmap::process(
// Update optimizedPoses with the newly added node // Update optimizedPoses with the newly added node
Transform newPose; Transform newPose;
if(signature->getLinks().size() == 1 && if(_poseScanMatching &&
!smallDisplacement && signature->getLinks().size() == 1 &&
_memory->isIncremental()) // ignore pose matching in localization mode _memory->isIncremental() && // ignore pose matching in localization mode
rehearsedId == 0) // don't do it if rehearsal happened
{ {
int oldId = signature->getLinks().begin()->first; int oldId = signature->getLinks().begin()->first;
const Signature * oldS = _memory->getSignature(oldId); const Signature * oldS = _memory->getSignature(oldId);
UASSERT(oldS != 0); UASSERT(oldS != 0);
//============================================================ Transform guess = signature->getLinks().begin()->second.transform();
// Scan matching
//============================================================
if(_poseScanMatching &&
!signature->sensorData().laserScanCompressed().empty() &&
rehearsedId == 0) // don't do it if rehearsal happened
{
UINFO("Odometry correction by scan matching");
Transform guess = signature->getLinks().begin()->second.transform();
double variance = 1.0;
int inliers = 0;
float inliersRatio = 0;
std::string rejectedMsg;
Transform t = _memory->computeIcpTransform(oldId, signature->id(), guess, false, &rejectedMsg, &inliers, &variance, &inliersRatio);
if(!t.isNull())
{
UINFO("Scan matching: update neighbor link (%d->%d, variance=%f) from %s to %s",
signature->id(),
oldId,
variance,
signature->getLinks().at(oldId).transform().prettyPrint().c_str(),
t.prettyPrint().c_str());
UASSERT(variance > 0.0);
_memory->updateLink(signature->id(), oldId, t, variance, variance);
if(_optimizeFromGraphEnd) 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(!signature->sensorData().laserScanCompressed().empty())
{
UINFO("Odometry correction by scan matching");
double variance = 1.0;
int inliers = 0;
float inliersRatio = 0;
std::string rejectedMsg;
Transform t = _memory->computeIcpTransform(oldId, signature->id(), guess, false, &rejectedMsg, &inliers, &variance, &inliersRatio);
if(!t.isNull())
{ {
// update all previous nodes UINFO("Scan matching: update neighbor link (%d->%d, variance=%f) from %s to %s",
// Normally _mapCorrection should be identity, but if _optimizeFromGraphEnd signature->id(),
// parameters just changed state, we should put back all poses without map correction. oldId,
Transform u = guess.inverse() * t; variance,
std::map<int, Transform>::iterator jter = _optimizedPoses.find(oldId); signature->getLinks().at(oldId).transform().prettyPrint().c_str(),
UASSERT(jter!=_optimizedPoses.end()); t.prettyPrint().c_str());
Transform up = jter->second * u * jter->second.inverse(); UASSERT(variance > 0.0);
Transform mapCorrectionInv = _mapCorrection.inverse(); _memory->updateLink(signature->id(), oldId, t, variance, variance);
for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
if(_optimizeFromGraphEnd)
{ {
iter->second = mapCorrectionInv * up * iter->second; // update all previous nodes
// Normally _mapCorrection should be identity, but if _optimizeFromGraphEnd
// parameters just changed state, we should put back all poses without map correction.
Transform u = guess.inverse() * t;
std::map<int, Transform>::iterator jter = _optimizedPoses.find(oldId);
UASSERT(jter!=_optimizedPoses.end());
Transform up = jter->second * u * jter->second.inverse();
Transform mapCorrectionInv = _mapCorrection.inverse();
for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
{
iter->second = mapCorrectionInv * up * iter->second;
}
} }
} }
} else
else
{
UINFO("Scan matching rejected: %s", rejectedMsg.c_str());
if(variance > 0)
{ {
double sqrtVar = sqrt(variance); UINFO("Scan matching rejected: %s", rejectedMsg.c_str());
_memory->updateLink(signature->id(), oldId, guess, sqrtVar, sqrtVar); if(variance > 0)
{
double sqrtVar = sqrt(variance);
_memory->updateLink(signature->id(), oldId, guess, sqrtVar, sqrtVar);
}
} }
statistics_.addStatistic(Statistics::kOdomCorrectionAccepted(), !t.isNull()?1.0f:0);
statistics_.addStatistic(Statistics::kOdomCorrectionInliers(), inliers);
statistics_.addStatistic(Statistics::kOdomCorrectionInliers_ratio(), inliersRatio);
statistics_.addStatistic(Statistics::kOdomCorrectionVariance(), variance);
} }
statistics_.addStatistic(Statistics::kOdomCorrectionAccepted(), !t.isNull()?1.0f:0); timeScanMatching = timer.ticks();
statistics_.addStatistic(Statistics::kOdomCorrectionInliers(), inliers); ULOGGER_INFO("timeScanMatching=%fs", timeScanMatching);
statistics_.addStatistic(Statistics::kOdomCorrectionInliers_ratio(), inliersRatio);
statistics_.addStatistic(Statistics::kOdomCorrectionVariance(), variance);
} }
timeScanMatching = timer.ticks();
ULOGGER_INFO("timeScanMatching=%fs", timeScanMatching);
UASSERT(oldS->hasLink(signature->id())); UASSERT(oldS->hasLink(signature->id()));
UASSERT(uContains(_optimizedPoses, oldId)); UASSERT(uContains(_optimizedPoses, oldId));