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 odom reset detection (new odom should not be null)

Browse Source
This commit is contained in:
matlabbe
2016-04-20 10:59:07 -04:00
parent d4d5d9f069
commit e7e12c85ef
1 changed files with 1 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+1 -1
View File
@@ -610,7 +610,7 @@ bool CoreWrapper::commonOdomUpdate(const nav_msgs::OdometryConstPtr & odomMsg)
if(!paused_)
{
Transform odom = rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose);
if(!lastPose_.isIdentity() && (odom.isIdentity() || odomMsg->pose.covariance[0] >= BAD_COVARIANCE))
if(!lastPose_.isIdentity() && !odom.isNull() && (odom.isIdentity() || odomMsg->pose.covariance[0] >= BAD_COVARIANCE))
{
UWARN("Odometry is reset (identity pose or high variance (%f) detected). Increment map id!", odomMsg->pose.covariance[0]);
rtabmap_.triggerNewMap();