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

Fixed #80

Browse Source
This commit is contained in:
matlabbe
2016-06-01 14:41:06 -04:00
parent f6fb7a7099
commit 927ea90fbe
1 changed files with 16 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+16 -10
View File
@@ -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