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

rtabmapviz: using twist covariance instead of pose covariance

Browse Source
This commit is contained in:
matlabbe
2016-08-05 14:22:45 -04:00
parent 30ed81a32c
commit 8ad683736b
2 changed files with 17 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+1 -1
View File
@@ -681,7 +681,7 @@ bool CoreWrapper::commonOdomUpdate(const nav_msgs::OdometryConstPtr & odomMsg)
// Only update variance if odom is not null
if(!odom.isNull())
{
// using MIN in case of 3DoF mapping (maybe not parameters are set, except x and yaw for the twist)
// using MIN in case of 3DoF mapping (maybe no parameters are set, except x and yaw for the twist)
float transVariance = uMax3(odomMsg->twist.covariance[0], MIN(odomMsg->twist.covariance[7], BAD_COVARIANCE), MIN(odomMsg->twist.covariance[14], BAD_COVARIANCE));
float rotVariance = uMax3(MIN(odomMsg->twist.covariance[21],BAD_COVARIANCE), MIN(odomMsg->twist.covariance[28], BAD_COVARIANCE), odomMsg->twist.covariance[35]);
if(uIsFinite(rotVariance) && rotVariance > rotVariance_)