i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

fixed optimization error ratio bug where comparison was done using not highest ratio link but on highest abs error link instead.

Browse Source
This commit is contained in:
matlabbe
2018-07-20 15:11:46 -04:00
parent 173bd49a26
commit e95cabb1fc
4 changed files with 73 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
corelib/src/OdometryMSCKF.cpp
View File

@@ -941,6 +941,18 @@ Transform OdometryMSCKF::computeTransform(
info->reg.covariance.at<double>(0, 0) = fabs(covBaseFrame.val[0])/10.0;
info->reg.covariance.at<double>(1, 1) = fabs(covBaseFrame.val[1])/10.0;
info->reg.covariance.at<double>(2, 2) = fabs(covBaseFrame.val[2])/10.0;
if(info->reg.covariance.at<double>(0, 0) < 0.0001)
{
info->reg.covariance.at<double>(0, 0) = 0.0001;
}
if(info->reg.covariance.at<double>(1, 1) < 0.0001)
{
info->reg.covariance.at<double>(1, 1) = 0.0001;
}
if(info->reg.covariance.at<double>(2, 2) < 0.0001)
{
info->reg.covariance.at<double>(2, 2) = 0.0001;
}
info->reg.covariance.at<double>(3, 3) = msckf_vio::IMUState::gyro_noise*10.0;
info->reg.covariance.at<double>(4, 4) = info->reg.covariance.at<double>(3, 3);
info->reg.covariance.at<double>(5, 5) = info->reg.covariance.at<double>(3, 3);

9
corelib/src/Rtabmap.cpp
View File

@@ -2352,9 +2352,12 @@ bool Rtabmap::process(
fabs(iter->second.transform().x() - t.x()),
fabs(iter->second.transform().y() - t.y()),
fabs(iter->second.transform().z() - t.z()));
if(linearError > maxLinearError)
float stddev = sqrt(iter->second.transVariance());
float linearErrorRatio = linearError/stddev;
if(linearErrorRatio > maxLinearErrorRatio)
{
maxLinearError = linearError;
maxLinearErrorRatio = linearErrorRatio;
maxLinearLink = &iter->second;
}
}
@@ -2363,8 +2366,6 @@ bool Rtabmap::process(
{
UINFO("Max optimization error = %f m (link %d->%d, var=%f, ratio error/std=%f)", maxLinearError, maxLinearLink->from(), maxLinearLink->to(), maxLinearLink->transVariance(), maxLinearError/sqrt(maxLinearLink->transVariance()));
float stddev = sqrt(maxLinearLink->transVariance());
maxLinearErrorRatio = maxLinearError/stddev;
if(maxLinearErrorRatio > _optimizationMaxLinearError)
{
UWARN("Rejecting all added loop closures (%d) in this "
@@ -2378,7 +2379,7 @@ bool Rtabmap::process(
maxLinearLink->to(),
maxLinearLink->type(),
maxLinearError,
stddev,
sqrt(maxLinearLink->transVariance()),
_optimizationMaxLinearError);
for(std::list<std::pair<int, int> >::iterator iter=loopClosureLinksAdded.begin(); iter!=loopClosureLinksAdded.end(); ++iter)
{