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

normalize variance in loop closure

Browse Source
This commit is contained in:
matlabbe
2017-01-05 17:22:49 -05:00
parent dfb965cfa9
commit 716337b71a
3 changed files with 26 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
corelib/src/Memory.cpp
View File

@@ -2235,6 +2235,12 @@ Transform Memory::computeTransform(
info->rejectedMsg = msg;
}
}
else if(info && !transform.isIdentity())
{
//normalize variance
info->varianceLin *= transform.getNorm();
info->varianceAng *= transform.getAngle();
}
}
}
return transform;
@@ -2282,6 +2288,13 @@ Transform Memory::computeIcpTransform(
// compute transform fromId -> toId
std::vector<int> inliersV;
t = _registrationIcp->computeTransformation(fromS->sensorData(), toS->sensorData(), guess, info);
if(!t.isNull() && !t.isIdentity() && info)
{
// normalize variance
info->varianceLin *= t.getNorm();
info->varianceAng *= t.getAngle();
}
}
else
{