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 ICP-only registration with already provided guess (no need to do visual guess, as the guess can be already good)

Browse Source
This commit is contained in:
matlabbe
2016-06-23 11:08:20 -04:00
parent 22766e958f
commit f2d48cb894
2 changed files with 7 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Rtabmap.cpp
+2 -1
View File
@@ -1834,7 +1834,8 @@ bool Rtabmap::process(
_optimizedPoses.at(signature->id()).getDistanceSquared(_optimizedPoses.at(nearestId)) < _proximityFilteringRadius*_proximityFilteringRadius))
{
RegistrationInfo info;
Transform transform = _memory->computeTransform(signature->id(), nearestId, Transform(), &info);
Transform guess = _optimizedPoses.at(signature->id()).inverse() * _optimizedPoses.at(nearestId);
Transform transform = _memory->computeTransform(signature->id(), nearestId, guess, &info);
if(!transform.isNull())
{
if(_proximityFilteringRadius <= 0 || transform.getNormSquared() <= _proximityFilteringRadius*_proximityFilteringRadius)