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

ignoring inf variance

Browse Source
This commit is contained in:
Mathieu Labbe
2015-04-02 11:16:12 -04:00
parent 58bd102340
commit 282adbfedf
1 changed files with 3 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+3 -2
View File
@@ -49,6 +49,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UStl.h> #include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UTimer.h> #include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UConversion.h> #include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UMath.h>
#include <opencv2/highgui/highgui.hpp> #include <opencv2/highgui/highgui.hpp>
#include <pcl_ros/transforms.h> #include <pcl_ros/transforms.h>
@@ -571,11 +572,11 @@ bool CoreWrapper::commonOdomUpdate(const nav_msgs::OdometryConstPtr & odomMsg)
lastPose_ = odom; lastPose_ = odom;
float transVariance = max3(odomMsg->pose.covariance[0], odomMsg->pose.covariance[7], odomMsg->pose.covariance[14]); 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]); float rotVariance = max3(odomMsg->pose.covariance[21], odomMsg->pose.covariance[28], odomMsg->pose.covariance[35]);
if(rotVariance > rotVariance_) if(uIsFinite(rotVariance) && rotVariance > rotVariance_)
{ {
rotVariance_ = rotVariance; rotVariance_ = rotVariance;
} }
if(transVariance > transVariance_) if(uIsFinite(transVariance) && transVariance > transVariance_)
{ {
transVariance_ = transVariance; transVariance_ = transVariance;
} }