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

Added parameter Mem/CovOffDiagIgnored (default true)

Browse Source
This commit is contained in:
matlabbe
2018-08-05 12:51:50 -04:00
parent f938e8ce29
commit d5128ddc18
6 changed files with 144 additions and 105 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Memory.cpp
+17 -1
View File
@@ -103,6 +103,7 @@ Memory::Memory(const ParametersMap & parameters) :
_visMaxFeatures(Parameters::defaultVisMaxFeatures()),
_visCorType(Parameters::defaultVisCorType()),
_imagesAlreadyRectified(Parameters::defaultRtabmapImagesAlreadyRectified()),
_covOffDiagonalIgnored(Parameters::defaultMemCovOffDiagIgnored()),
_idCount(kIdStart),
_idMapCount(kIdStart),
_lastSignature(0),
@@ -499,6 +500,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
uInsert(params, ParametersPair(Parameters::kVisCorType(), "0"));
}
Parameters::parse(params, Parameters::kRtabmapImagesAlreadyRectified(), _imagesAlreadyRectified);
Parameters::parse(params, Parameters::kMemCovOffDiagIgnored(), _covOffDiagonalIgnored);
UASSERT_MSG(_maxStMemSize >= 0, uFormat("value=%d", _maxStMemSize).c_str());
@@ -811,7 +813,21 @@ void Memory::addSignatureToStm(Signature * signature, const cv::Mat & covariance
!_signatures.at(*_stMem.rbegin())->getPose().isNull())
{
UASSERT(covariance.cols == 6 && covariance.rows == 6 && covariance.type() == CV_64FC1);
cv::Mat infMatrix = covariance.inv();
cv::Mat infMatrix;
if(_covOffDiagonalIgnored)
{
infMatrix = cv::Mat::zeros(6,6,CV_64FC1);
infMatrix.at<double>(0,0) = 1.0 / covariance.at<double>(0,0);
infMatrix.at<double>(1,1) = 1.0 / covariance.at<double>(1,1);
infMatrix.at<double>(2,2) = 1.0 / covariance.at<double>(2,2);
infMatrix.at<double>(3,3) = 1.0 / covariance.at<double>(3,3);
infMatrix.at<double>(4,4) = 1.0 / covariance.at<double>(4,4);
infMatrix.at<double>(5,5) = 1.0 / covariance.at<double>(5,5);
}
else
{
infMatrix = covariance.inv();
}
if((uIsFinite(covariance.at<double>(0,0)) && covariance.at<double>(0,0)>0.0) &&
!(uIsFinite(infMatrix.at<double>(0,0)) && infMatrix.at<double>(0,0)>0.0))
{