From ba80c887b4f707583ac93822048a5c2177175807 Mon Sep 17 00:00:00 2001 From: Mathieu Labbe Date: Thu, 2 Apr 2015 11:44:21 -0400 Subject: [PATCH] fixed rtabmapviz error on inf variance --- src/GuiWrapper.cpp | 28 ++++++++++++++-------------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/src/GuiWrapper.cpp b/src/GuiWrapper.cpp index fb0052ed..c70e6224 100644 --- a/src/GuiWrapper.cpp +++ b/src/GuiWrapper.cpp @@ -390,8 +390,8 @@ void GuiWrapper::defaultCallback(const nav_msgs::OdometryConstPtr & odomMsg) float transVariance = max3(odomMsg->pose.covariance[0], odomMsg->pose.covariance[7], odomMsg->pose.covariance[14]); float rotVariance = max3(odomMsg->pose.covariance[21], odomMsg->pose.covariance[28], odomMsg->pose.covariance[35]); data.setPose(odom, - rotVariance>0?rotVariance:1, - transVariance>0?transVariance:1); + uIsFinite(rotVariance) && rotVariance>0?rotVariance:1, + uIsFinite(transVariance) && transVariance>0?transVariance:1); rtabmap::OdometryInfo info; QMetaObject::invokeMethod(mainWindow_, "processOdometry", Q_ARG(rtabmap::SensorData, data), Q_ARG(rtabmap::OdometryInfo, info)); @@ -456,8 +456,8 @@ void GuiWrapper::depthCallback( cy, localTransform, rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose), - rotVariance>0?rotVariance:1, - transVariance>0?transVariance:1, + uIsFinite(rotVariance) && rotVariance>0?rotVariance:1, + uIsFinite(transVariance) && transVariance>0?transVariance:1, odomMsg->header.seq, rtabmap_ros::timestampFromROS(odomMsg->header.stamp)); @@ -524,8 +524,8 @@ void GuiWrapper::depthOdomInfoCallback( cy, localTransform, rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose), - rotVariance>0?rotVariance:1, - transVariance>0?transVariance:1, + uIsFinite(rotVariance) && rotVariance>0?rotVariance:1, + uIsFinite(transVariance) && transVariance>0?transVariance:1, odomMsg->header.seq, rtabmap_ros::timestampFromROS(odomMsg->header.stamp)); @@ -602,8 +602,8 @@ void GuiWrapper::depthScanCallback( cy, localTransform, rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose), - rotVariance>0?rotVariance:1, - transVariance>0?transVariance:1, + uIsFinite(rotVariance) && rotVariance>0?rotVariance:1, + uIsFinite(transVariance) && transVariance>0?transVariance:1, odomMsg->header.seq, rtabmap_ros::timestampFromROS(odomMsg->header.stamp)); @@ -704,8 +704,8 @@ void GuiWrapper::stereoScanCallback( cy, localTransform, rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose), - rotVariance>0?rotVariance:1, - transVariance>0?transVariance:1, + uIsFinite(rotVariance) && rotVariance>0?rotVariance:1, + uIsFinite(transVariance) && transVariance>0?transVariance:1, odomMsg->header.seq, rtabmap_ros::timestampFromROS(odomMsg->header.stamp)); @@ -796,8 +796,8 @@ void GuiWrapper::stereoOdomInfoCallback( cy, localTransform, rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose), - rotVariance>0?rotVariance:1, - transVariance>0?transVariance:1, + uIsFinite(rotVariance) && rotVariance>0?rotVariance:1, + uIsFinite(transVariance) && transVariance>0?transVariance:1, odomMsg->header.seq, rtabmap_ros::timestampFromROS(odomMsg->header.stamp)); @@ -887,8 +887,8 @@ void GuiWrapper::stereoCallback( cy, localTransform, rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose), - rotVariance>0?rotVariance:1, - transVariance>0?transVariance:1, + uIsFinite(rotVariance) && rotVariance>0?rotVariance:1, + uIsFinite(transVariance) && transVariance>0?transVariance:1, odomMsg->header.seq, rtabmap_ros::timestampFromROS(odomMsg->header.stamp));