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

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