i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 09:30:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

Generalized neighbor link refining using registration done in Memory (can be visual, visual+icp or, icp)

Browse Source
This commit is contained in:
matlabbe
2016-05-19 15:35:30 -04:00
parent 2637f74094
commit 1ecaae364c
1 changed files with 6 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
corelib/src/Rtabmap.cpp
View File

@@ -1013,17 +1013,16 @@ bool Rtabmap::process(
else else
{ {
//============================================================ //============================================================
// Scan matching // Refine neighbor links
//============================================================ //============================================================
if(!signature->sensorData().laserScanCompressed().empty()) if(!signature->sensorData().laserScanCompressed().empty())
{ {
UINFO("Odometry correction by scan matching"); UINFO("Odometry refining");
Transform guess = signature->getLinks().begin()->second.transform().inverse();
RegistrationInfo info; RegistrationInfo info;
Transform t = _memory->computeIcpTransform(oldId, signature->id(), guess, &info); Transform t = _memory->computeTransform(oldId, signature->id(), guess, &info);
if(!t.isNull()) if(!t.isNull())
{ {
UINFO("Scan matching: update neighbor link (%d->%d, variance=%f) from %s to %s", UINFO("Odometry refining: update neighbor link (%d->%d, variance=%f) from %s to %s",
oldId, oldId,
signature->id(), signature->id(),
info.variance, info.variance,
@@ -1050,7 +1049,7 @@ bool Rtabmap::process(
} }
else else
{ {
UINFO("Scan matching rejected: %s", info.rejectedMsg.c_str()); UINFO("Odometry refining rejected: %s", info.rejectedMsg.c_str());
if(info.variance > 0) if(info.variance > 0)
{ {
double sqrtVar = sqrt(info.variance); double sqrtVar = sqrt(info.variance);
@@ -1065,7 +1064,7 @@ bool Rtabmap::process(
} }
} }
timeNeighborLinkRefining = timer.ticks(); timeNeighborLinkRefining = timer.ticks();
ULOGGER_INFO("timeScanMatching=%fs", timeNeighborLinkRefining); ULOGGER_INFO("timeOdometryRefining=%fs", timeNeighborLinkRefining);
UASSERT(oldS->hasLink(signature->id())); UASSERT(oldS->hasLink(signature->id()));
UASSERT(uContains(_optimizedPoses, oldId)); UASSERT(uContains(_optimizedPoses, oldId));