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

Odom: Added twist covariance (pose covariance/2)

Browse Source
This commit is contained in:
matlabbe
2016-04-26 10:19:29 -04:00
parent e18365e28f
commit 3f6811b156
3 changed files with 25 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+2 -1
View File
@@ -59,6 +59,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <octomap_msgs/conversions.h>
#endif
#define BAD_COVARIANCE 9999
//msgs
#include "rtabmap_ros/Info.h"
@@ -609,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] >= 9999))
if(!lastPose_.isIdentity() && (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();