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

fixed covariance to identity when invalid covariance is received

Browse Source
This commit is contained in:
matlabbe
2017-05-24 16:21:08 -04:00
parent 252d5ed355
commit 04bbb59870
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
@@ -1208,7 +1208,7 @@ void CoreWrapper::process(
cv::Mat covariance = odomCovariance; cv::Mat covariance = odomCovariance;
if(covariance.empty() || !uIsFinite(covariance.at<double>(0,0)) || covariance.at<double>(0,0)<=0.0f) if(covariance.empty() || !uIsFinite(covariance.at<double>(0,0)) || covariance.at<double>(0,0)<=0.0f)
{ {
covariance = cv::Mat::ones(6,6,CV_64FC1); covariance = cv::Mat::eye(6,6,CV_64FC1);
if(odomDefaultLinVariance_ > 0.0f) if(odomDefaultLinVariance_ > 0.0f)
{ {
covariance.at<double>(0,0) = odomDefaultLinVariance_; covariance.at<double>(0,0) = odomDefaultLinVariance_;