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

Update Odometry.cpp

Browse Source 
Odometry: Added additional debug information on dt>0 assert
This commit is contained in:
matlabbe
2016-06-27 16:08:25 -04:00
committed by GitHub
parent f69ca56179
commit 756e176878
1 changed files with 2 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
corelib/src/Odometry.cpp
View File

@@ -202,7 +202,7 @@ Transform Odometry::process(SensorData & data, OdometryInfo * info)
double dt = previousStamp_>0.0f?data.stamp() - previousStamp_:0.0;
Transform guess = dt && guessFromMotion_?Transform::getIdentity():Transform();
UASSERT(dt>0.0 || (dt == 0.0 && previousVelocityTransform_.isNull()));
UASSERT_MSG(dt>0.0 || (dt == 0.0 && previousVelocityTransform_.isNull()), uFormat("dt=%f previous transform=%s", dt, previousVelocityTransform_.c_str()).c_str());
if(!previousVelocityTransform_.isNull())
{
if(guessFromMotion_)
@@ -491,7 +491,7 @@ void Odometry::initKalmanFilter(const Transform & initialPose, float vx, float v
0, 0, 0, 0, 0, 0.09 } };
*/
kalmanFilter_.init(nStates, nMeasurements, nInputs); // init Kalman Filter
cv::setIdentity(kalmanFilter_.processNoiseCov, cv::Scalar::all(_kalmanProcessNoise)); // set process noise
cv::setIdentity(kalmanFilter_.measurementNoiseCov, cv::Scalar::all(_kalmanMeasurementNoise)); // set measurement noise