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

Added Vis/PnPSplitLinearCovComponents parameter (default false -> same as before)

Browse Source
This commit is contained in:
matlabbe
2024-02-07 14:43:16 -08:00
parent 1dadd50cf2
commit 510aef19e4
11 changed files with 259 additions and 120 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Rtabmap.cpp
+7 -4
View File
@@ -1442,8 +1442,10 @@ bool Rtabmap::process(
float angleToClosestNodeInTheGraph = 0;
if(_rgbdSlamMode)
{
statistics_.addStatistic(Statistics::kMemoryOdometry_variance_lin(), odomCovariance.empty()?1.0f:(float)odomCovariance.at<double>(0,0));
statistics_.addStatistic(Statistics::kMemoryOdometry_variance_ang(), odomCovariance.empty()?1.0f:(float)odomCovariance.at<double>(5,5));
double linVar = odomCovariance.empty()?1.0f:uMax3(odomCovariance.at<double>(0,0), odomCovariance.at<double>(1,1)>=9999?0:odomCovariance.at<double>(1,1), odomCovariance.at<double>(2,2)>=9999?0:odomCovariance.at<double>(2,2));
double angVar = odomCovariance.empty()?1.0f:uMax3(odomCovariance.at<double>(3,3)>=9999?0:odomCovariance.at<double>(3,3), odomCovariance.at<double>(4,4)>=9999?0:odomCovariance.at<double>(4,4), odomCovariance.at<double>(5,5));
statistics_.addStatistic(Statistics::kMemoryOdometry_variance_lin(), (float)linVar);
statistics_.addStatistic(Statistics::kMemoryOdometry_variance_ang(), (float)angVar);
//Verify if there was a rehearsal
int rehearsedId = (int)uValue(statistics_.data(), Statistics::kMemoryRehearsal_merged(), 0.0f);
@@ -2959,9 +2961,10 @@ bool Rtabmap::process(
{
// Make the new one the parent of the old one
UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
loopClosureLinearVariance = uMax3(info.covariance.at<double>(0,0), info.covariance.at<double>(1,1)>=9999?0:info.covariance.at<double>(1,1), info.covariance.at<double>(2,2)>=9999?0:info.covariance.at<double>(2,2));
loopClosureAngularVariance = uMax3(info.covariance.at<double>(3,3)>=9999?0:info.covariance.at<double>(3,3), info.covariance.at<double>(4,4)>=9999?0:info.covariance.at<double>(4,4), info.covariance.at<double>(5,5));
cv::Mat information = getInformation(info.covariance);
loopClosureLinearVariance = 1.0/information.at<double>(0,0);
loopClosureAngularVariance = 1.0/information.at<double>(5,5);
rejectedGlobalLoopClosure = !_memory->addLink(Link(signature->id(), _loopClosureHypothesis.first, Link::kGlobalClosure, transform, information));
if(!rejectedGlobalLoopClosure)
{