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

Odometry: Don't assert if dt is negative, just show error if guess from motion / kalman filter is used (https://github.com/introlab/rtabmap_ros/issues/169)

Browse Source
This commit is contained in:
matlabbe
2017-05-05 16:25:03 -04:00
parent 4d025568dc
commit b25379c243
1 changed files with 14 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
corelib/src/Odometry.cpp
View File

@@ -269,8 +269,20 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
}
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());
Transform guess = dt>0.0 && guessFromMotion_ && !previousVelocityTransform_.isNull()?Transform::getIdentity():Transform();
if(!(dt>0.0 || (dt == 0.0 && previousVelocityTransform_.isNull())))
{
if(guessFromMotion_)
{
UERROR("Guess from motion is set but dt is invalid! Odometry is then computed without guess. (dt=%f previous transform=%s)", dt, previousVelocityTransform_.prettyPrint().c_str());
}
else if(_filteringStrategy==1)
{
UERROR("Kalman filtering is enalbed but dt is invalid! Odometry is then computed without Kalman filtering. (dt=%f previous transform=%s)", dt, previousVelocityTransform_.prettyPrint().c_str());
}
dt=0;
previousVelocityTransform_.setNull();
}
if(!previousVelocityTransform_.isNull())
{
if(guessFromMotion_)