i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-01 17:10:26 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' of github.com:introlab/rtabmap into devel

Browse Source
This commit is contained in:
matlabbe
2016-06-27 16:08:53 -04:00
parent 1398b71b60 756e176878
commit 3609c961ea
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

@@ -262,7 +262,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_)
@@ -551,7 +551,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