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

rtabmap: use largest odom covariance instead of summation

Browse Source
This commit is contained in:
matlabbe
2017-07-13 11:39:19 -04:00
parent e944786ee5
commit 7f392f3796
3 changed files with 24 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+5 -6
View File
@@ -763,16 +763,15 @@ bool CoreWrapper::odomUpdate(const nav_msgs::OdometryConstPtr & odomMsg)
covariance = cv::Mat(6,6,CV_64FC1, (void*)odomMsg->twist.covariance.data()).clone();
}
if(uIsFinite(covariance.at<double>(0,0)) && covariance.at<double>(0,0) != 1.0 && covariance.at<double>(0,0)>0.0)
if(uIsFinite(covariance.at<double>(0,0)) &&
covariance.at<double>(0,0) != 1.0 &&
covariance.at<double>(0,0)>0.0)
{
if(covariance_.empty())
// Use largest covariance error (to be independent of the odometry frame rate)
if(covariance_.empty() || covariance.at<double>(0,0) > covariance_.at<double>(0,0))
{
covariance_ = covariance;
}
else
{
covariance_ += covariance;
}
}
}