From 704532ce62a78f0ce5403ae519536a4fbe8c553c Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 20 Apr 2016 18:15:25 -0400 Subject: [PATCH] CoreWrapper: using twist covariance instead of pose covariance (which could grow out of bounds depending of the odometry used) --- src/CoreWrapper.cpp | 8 ++++---- src/OdometryROS.cpp | 27 ++++++++++++++------------- 2 files changed, 18 insertions(+), 17 deletions(-) diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index fc92982d..ed9b1248 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -610,9 +610,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->pose.covariance[0] >= BAD_COVARIANCE)) + if(!lastPose_.isIdentity() && !odom.isNull() && (odom.isIdentity() || odomMsg->twist.covariance[0] >= BAD_COVARIANCE)) { - UWARN("Odometry is reset (identity pose or high variance (%f) detected). Increment map id!", odomMsg->pose.covariance[0]); + UWARN("Odometry is reset (identity pose or high variance (%f) detected). Increment map id!", odomMsg->twist.covariance[0]); rtabmap_.triggerNewMap(); rotVariance_ = 0; transVariance_ = 0; @@ -621,8 +621,8 @@ bool CoreWrapper::commonOdomUpdate(const nav_msgs::OdometryConstPtr & odomMsg) lastPoseIntermediate_ = false; lastPose_ = odom; lastPoseStamp_ = odomMsg->header.stamp; - float transVariance = uMax3(odomMsg->pose.covariance[0], odomMsg->pose.covariance[7], odomMsg->pose.covariance[14]); - float rotVariance = uMax3(odomMsg->pose.covariance[21], odomMsg->pose.covariance[28], odomMsg->pose.covariance[35]); + 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_) { rotVariance_ = rotVariance; diff --git a/src/OdometryROS.cpp b/src/OdometryROS.cpp index 02b36fce..a54f9102 100644 --- a/src/OdometryROS.cpp +++ b/src/OdometryROS.cpp @@ -379,12 +379,13 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp) odom.pose.pose.orientation = poseMsg.transform.rotation; //set covariance - odom.pose.covariance.at(0) = info.variance; // xx - odom.pose.covariance.at(7) = info.variance; // yy - odom.pose.covariance.at(14) = info.variance; // zz - odom.pose.covariance.at(21) = info.variance; // rr - odom.pose.covariance.at(28) = info.variance; // pp - odom.pose.covariance.at(35) = info.variance; // yawyaw + // libviso2 uses approximately vel variance * 2 + odom.pose.covariance.at(0) = info.variance*2; // xx + odom.pose.covariance.at(7) = info.variance*2; // yy + odom.pose.covariance.at(14) = info.variance*2; // zz + odom.pose.covariance.at(21) = info.variance*2; // rr + odom.pose.covariance.at(28) = info.variance*2; // pp + odom.pose.covariance.at(35) = info.variance*2; // yawyaw //set velocity bool setTwist = !odometry_->previousVelocityTransform().isNull(); @@ -399,13 +400,13 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp) odom.twist.twist.angular.y = pitch; odom.twist.twist.angular.z = yaw; } - // libviso2 uses approximately pose variance/2 - odom.twist.covariance.at(0) = setTwist?odom.pose.covariance.at(0)/2.0:BAD_COVARIANCE; // xx - odom.twist.covariance.at(7) = setTwist?odom.pose.covariance.at(7)/2.0:BAD_COVARIANCE; // yy - odom.twist.covariance.at(14) = setTwist?odom.pose.covariance.at(14)/2.0:BAD_COVARIANCE; // zz - odom.twist.covariance.at(21) = setTwist?odom.pose.covariance.at(21)/2.0:BAD_COVARIANCE; // rr - odom.twist.covariance.at(28) = setTwist?odom.pose.covariance.at(28)/2.0:BAD_COVARIANCE; // pp - odom.twist.covariance.at(35) = setTwist?odom.pose.covariance.at(35)/2.0:BAD_COVARIANCE; // yawyaw + + odom.twist.covariance.at(0) = setTwist?info.variance:BAD_COVARIANCE; // xx + odom.twist.covariance.at(7) = setTwist?info.variance:BAD_COVARIANCE; // yy + odom.twist.covariance.at(14) = setTwist?info.variance:BAD_COVARIANCE; // zz + odom.twist.covariance.at(21) = setTwist?info.variance:BAD_COVARIANCE; // rr + odom.twist.covariance.at(28) = setTwist?info.variance:BAD_COVARIANCE; // pp + odom.twist.covariance.at(35) = setTwist?info.variance:BAD_COVARIANCE; // yawyaw //publish the message odomPub_.publish(odom);