i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 00:37:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

rtabmap: odometry variance summation instead of taking largest one

Browse Source
This commit is contained in:
matlabbe
2017-01-05 17:32:12 -05:00
parent 3fb6a82825
commit d343e8cb71
1 changed files with 4 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+4 -4
View File
@@ -671,13 +671,13 @@ bool CoreWrapper::odomUpdate(const nav_msgs::OdometryConstPtr & odomMsg)
rotVariance = uMax3(MIN(odomMsg->pose.covariance[21]/2.0,BAD_COVARIANCE), MIN(odomMsg->pose.covariance[28]/2.0, BAD_COVARIANCE), odomMsg->pose.covariance[35]/2.0); rotVariance = uMax3(MIN(odomMsg->pose.covariance[21]/2.0,BAD_COVARIANCE), MIN(odomMsg->pose.covariance[28]/2.0, BAD_COVARIANCE), odomMsg->pose.covariance[35]/2.0);
} }
if(uIsFinite(rotVariance) && rotVariance > rotVariance_) if(uIsFinite(rotVariance) && rotVariance != 1.0f)
{ {
rotVariance_ = rotVariance; rotVariance_ += rotVariance;
} }
if(uIsFinite(transVariance) && transVariance > transVariance_) if(uIsFinite(transVariance) && transVariance != 1.0f)
{ {
transVariance_ = transVariance; transVariance_ += transVariance;
} }
} }