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

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

1
corelib/include/rtabmap/core/Memory.h
View File

@@ -302,6 +302,7 @@ private:
int _visMaxFeatures;
int _visCorType;
bool _imagesAlreadyRectified;
bool _covOffDiagonalIgnored;
int _idCount;
int _idMapCount;

1
corelib/include/rtabmap/core/Parameters.h
View File

@@ -221,6 +221,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Mem, LaserScanNormalK, int, 0, "If > 0 and laser scans don't have normals, normals will be computed with K search neighbors when creating a signature.");
RTABMAP_PARAM(Mem, LaserScanNormalRadius, int, 0, "If > 0 m and laser scans don't have normals, normals will be computed with radius search neighbors when creating a signature.");
RTABMAP_PARAM(Mem, UseOdomFeatures, bool, true, "Use odometry features.");
RTABMAP_PARAM(Mem, CovOffDiagIgnored, bool, true, "Ignore off diagonal values of the covariance matrix.");
// KeypointMemory (Keypoint-based)
RTABMAP_PARAM(Kp, NNStrategy, int, 1, "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4");

18
corelib/src/Memory.cpp
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))
{

2
corelib/src/OptimizerG2O.cpp
View File

@@ -535,7 +535,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
UDEBUG("Initial optimization...");
optimizer.initializeOptimization();
UASSERT_MSG(optimizer.verifyInformationMatrices(),
UASSERT_MSG(optimizer.verifyInformationMatrices(true),
"This error can be caused by (1) bad covariance matrix "
"set in odometry messages "
"(see requirements in g2o::OptimizableGraph::verifyInformationMatrices() function) "