From 927ea90fbed1480f57edd9f605616728a32591a0 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 1 Jun 2016 14:41:06 -0400 Subject: [PATCH] Fixed #80 --- src/CoreWrapper.cpp | 26 ++++++++++++++++---------- 1 file changed, 16 insertions(+), 10 deletions(-) diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index 7a0866df..c5533530 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -615,9 +615,9 @@ bool CoreWrapper::commonOdomUpdate(const nav_msgs::OdometryConstPtr & odomMsg) if(!paused_) { Transform odom = rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose); - if(!lastPose_.isIdentity() && !odom.isNull() && (odom.isIdentity() || odomMsg->twist.covariance[0] >= BAD_COVARIANCE)) + if(!lastPose_.isIdentity() && !odom.isNull() && (odom.isIdentity() || (odomMsg->pose.covariance[0] >= BAD_COVARIANCE && odomMsg->twist.covariance[0] >= BAD_COVARIANCE))) { - UWARN("Odometry is reset (identity pose or high variance (%f) detected). Increment map id!", odomMsg->twist.covariance[0]); + UWARN("Odometry is reset (identity pose or high variance (%f) detected). Increment map id!", MAX(odomMsg->pose.covariance[0], odomMsg->twist.covariance[0])); rtabmap_.triggerNewMap(); rotVariance_ = 0; transVariance_ = 0; @@ -626,15 +626,21 @@ bool CoreWrapper::commonOdomUpdate(const nav_msgs::OdometryConstPtr & odomMsg) lastPoseIntermediate_ = false; lastPose_ = odom; lastPoseStamp_ = odomMsg->header.stamp; - float transVariance = uMax3(odomMsg->twist.covariance[0], odomMsg->twist.covariance[7], odomMsg->twist.covariance[14]); - float rotVariance = uMax3(odomMsg->twist.covariance[21], odomMsg->twist.covariance[28], odomMsg->twist.covariance[35]); - if(uIsFinite(rotVariance) && rotVariance > rotVariance_) + + // Only update variance if odom is not null + if(!odom.isNull()) { - rotVariance_ = rotVariance; - } - if(uIsFinite(transVariance) && transVariance > transVariance_) - { - transVariance_ = transVariance; + // using MIN in case of 3DoF mapping (maybe not 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_) + { + rotVariance_ = rotVariance; + } + if(uIsFinite(transVariance) && transVariance > transVariance_) + { + transVariance_ = transVariance; + } } // Throttle