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

CoreWrapper: using twist covariance instead of pose covariance (which could grow out of bounds depending of the odometry used)

Browse Source
This commit is contained in:
matlabbe
2016-04-26 10:19:29 -04:00
parent af89e0c009
commit 76cb4365dd
2 changed files with 18 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+4 -4
View File
@@ -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;
src/OdometryROS.cpp
+14 -13
View File
@@ -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);