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 odometry when guess from motion is enabled

Browse Source
This commit is contained in:
matlabbe
2016-08-07 20:54:56 -04:00
parent 93f1ae501c
commit 6ef9034a67
7 changed files with 11 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Odometry.cpp
-9
View File
@@ -200,8 +200,6 @@ Transform Odometry::process(SensorData & data, OdometryInfo * info)
Transform Odometry::process(SensorData & data, const Transform & guessIn, OdometryInfo * info)
{
UASSERT(!data.imageRaw().empty());
// Ground alignment
if(_pose.isIdentity() && _alignWithGround)
{
@@ -258,13 +256,6 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
}
}
if(!data.stereoCameraModel().isValidForProjection() &&
(data.cameraModels().size() == 0 || !data.cameraModels()[0].isValidForProjection()))
{
UERROR("Rectified images required! Calibrate your camera.");
return Transform();
}
double dt = previousStamp_>0.0f?data.stamp() - previousStamp_:0.0;
Transform guess = dt && guessFromMotion_ && !previousVelocityTransform_.isNull()?Transform::getIdentity():Transform();
UASSERT_MSG(dt>0.0 || (dt == 0.0 && previousVelocityTransform_.isNull()), uFormat("dt=%f previous transform=%s", dt, previousVelocityTransform_.prettyPrint().c_str()).c_str());