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

Fixed broken 2D icp from 0.11.0

Browse Source
This commit is contained in:
matlabbe
2016-02-19 15:10:28 -05:00
parent d5538ee99d
commit 9b16d93481
5 changed files with 12 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Rtabmap.cpp
+6 -5
View File
@@ -1002,7 +1002,7 @@ bool Rtabmap::process(
{
// set small variance
UDEBUG("Set small variance. The robot is not moving.");
_memory->updateLink(signature->id(), oldId, guess, 0.0001, 0.0001);
_memory->updateLink(oldId, signature->id(), guess, 0.0001, 0.0001);
}
}
else
@@ -1019,10 +1019,10 @@ bool Rtabmap::process(
if(!t.isNull())
{
UINFO("Scan matching: update neighbor link (%d->%d, variance=%f) from %s to %s",
signature->id(),
oldId,
signature->id(),
info.variance,
signature->getLinks().at(oldId).transform().prettyPrint().c_str(),
guess.prettyPrint().c_str(),
t.prettyPrint().c_str());
UASSERT(info.variance > 0.0);
_memory->updateLink(oldId, signature->id(), t, info.variance, info.variance);
@@ -1049,7 +1049,7 @@ bool Rtabmap::process(
if(info.variance > 0)
{
double sqrtVar = sqrt(info.variance);
_memory->updateLink(signature->id(), oldId, guess, sqrtVar, sqrtVar);
_memory->updateLink(oldId, signature->id(), guess, sqrtVar, sqrtVar);
}
}
statistics_.addStatistic(Statistics::kNeighborLinkRefiningAccepted(), !t.isNull()?1.0f:0);
@@ -1064,6 +1064,7 @@ bool Rtabmap::process(
UASSERT(oldS->hasLink(signature->id()));
UASSERT(uContains(_optimizedPoses, oldId));
newPose = _optimizedPoses.at(oldId) * oldS->getLinks().at(signature->id()).transform();
_mapCorrection = newPose * signature->getPose().inverse();
if(_mapCorrection.getNormSquared() > 0.001f && _optimizeFromGraphEnd)
@@ -1079,7 +1080,7 @@ bool Rtabmap::process(
newPose = _mapCorrection * signature->getPose();
}
UDEBUG("Added pose %s", newPose.prettyPrint().c_str());
UDEBUG("Added pose %s (odom=%s)", newPose.prettyPrint().c_str(), signature->getPose().prettyPrint().c_str());
// Update Poses and Constraints
_optimizedPoses.insert(std::make_pair(signature->id(), newPose));
_lastLocalizationPose = newPose; // keep in cache the latest corrected pose