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

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_) if(!paused_)
{ {
Transform odom = rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose); 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(); rtabmap_.triggerNewMap();
rotVariance_ = 0; rotVariance_ = 0;
transVariance_ = 0; transVariance_ = 0;
@@ -626,15 +626,21 @@ bool CoreWrapper::commonOdomUpdate(const nav_msgs::OdometryConstPtr & odomMsg)
lastPoseIntermediate_ = false; lastPoseIntermediate_ = false;
lastPose_ = odom; lastPose_ = odom;
lastPoseStamp_ = odomMsg->header.stamp; 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]); // Only update variance if odom is not null
if(uIsFinite(rotVariance) && rotVariance > rotVariance_) if(!odom.isNull())
{ {
rotVariance_ = rotVariance; // 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));
if(uIsFinite(transVariance) && transVariance > transVariance_) 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_)
transVariance_ = transVariance; {
rotVariance_ = rotVariance;
}
if(uIsFinite(transVariance) && transVariance > transVariance_)
{
transVariance_ = transVariance;
}
} }
// Throttle // Throttle