i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-03 01:50:24 +08:00
Code Issues Packages Projects Releases Wiki Activity

Bump 0.20.19. OdomF2M: scale x0.1 covariance when local bundle adjustment is done. Rtabmap: fixed GT localization error when RGBD/MaxOdomCacheSize is used.

Browse Source
This commit is contained in:
matlabbe
2022-02-06 17:41:53 -05:00
parent 9ad6b626e4
commit b44b212218
8 changed files with 55 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
corelib/src/odometry/OdometryF2M.cpp
View File

@@ -494,6 +494,23 @@ Transform OdometryF2M::computeTransform(
info->gravityRollError = fabs(rollImu - roll);
info->gravityPitchError = fabs(pitchImu - pitch);
}
// With bundle adjustment, scale down covariance by 10
UASSERT(regInfo.covariance.cols==6 && regInfo.covariance.rows == 6 && regInfo.covariance.type() == CV_64FC1);
double thrLin = Registration::COVARIANCE_LINEAR_EPSILON*10.0;
double thrAng = Registration::COVARIANCE_ANGULAR_EPSILON*10.0;
if(regInfo.covariance.at<double>(0,0)>thrLin)
regInfo.covariance.at<double>(0,0) *= 0.1;
if(regInfo.covariance.at<double>(1,1)>thrLin)
regInfo.covariance.at<double>(1,1) *= 0.1;
if(regInfo.covariance.at<double>(2,2)>thrLin)
regInfo.covariance.at<double>(2,2) *= 0.1;
if(regInfo.covariance.at<double>(3,3)>thrAng)
regInfo.covariance.at<double>(3,3) *= 0.1;
if(regInfo.covariance.at<double>(4,4)>thrAng)
regInfo.covariance.at<double>(4,4) *= 0.1;
if(regInfo.covariance.at<double>(5,5)>thrAng)
regInfo.covariance.at<double>(5,5) *= 0.1;
}
}
UDEBUG("Local Bundle Adjustment After : %s", transform.prettyPrint().c_str());