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

reprocess: fixed crash when odometry returns empty covariance. Also added more checks about that in the code in general.

Browse Source
This commit is contained in:
matlabbe
2025-10-09 14:44:13 -07:00
parent d60c15593d
commit 059cda85dc
7 changed files with 18 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
corelib/src/DBReader.cpp
View File

@@ -858,7 +858,7 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info)
info->odomPose = pose;
info->odomCovariance = infMatrix.inv();
info->odomVelocity = s->getVelocity();
UDEBUG("odom variance = %f/%f", info->odomCovariance.at<double>(0,0), info->odomCovariance.at<double>(5,5));
UDEBUG("odom variance = %f/%f", info->odomCovariance.empty()?0:info->odomCovariance.at<double>(0,0), info->odomCovariance.empty()?0:info->odomCovariance.at<double>(5,5));
}
}
delete s;

8
corelib/src/OdometryInfo.cpp
View File

@@ -126,10 +126,10 @@ std::map<std::string, float> OdometryInfo::statistics(const Transform & pose)
stats.insert(std::make_pair("Odometry/ICPStructuralComplexity/", reg.icpStructuralComplexity));
stats.insert(std::make_pair("Odometry/ICPStructuralDistribution/", reg.icpStructuralDistribution));
stats.insert(std::make_pair("Odometry/ICPCorrespondences/", reg.icpCorrespondences));
stats.insert(std::make_pair("Odometry/StdDevLin/", sqrt((float)reg.covariance.at<double>(0,0))));
stats.insert(std::make_pair("Odometry/StdDevAng/", sqrt((float)reg.covariance.at<double>(5,5))));
stats.insert(std::make_pair("Odometry/VarianceLin/", (float)reg.covariance.at<double>(0,0)));
stats.insert(std::make_pair("Odometry/VarianceAng/", (float)reg.covariance.at<double>(5,5)));
stats.insert(std::make_pair("Odometry/StdDevLin/", reg.covariance.empty()?0:sqrt((float)reg.covariance.at<double>(0,0))));
stats.insert(std::make_pair("Odometry/StdDevAng/", reg.covariance.empty()?0:sqrt((float)reg.covariance.at<double>(5,5))));
stats.insert(std::make_pair("Odometry/VarianceLin/", reg.covariance.empty()?0:(float)reg.covariance.at<double>(0,0)));
stats.insert(std::make_pair("Odometry/VarianceAng/", reg.covariance.empty()?0:(float)reg.covariance.at<double>(5,5)));
stats.insert(std::make_pair("Odometry/TimeEstimation/ms", timeEstimation*1000.0f));
stats.insert(std::make_pair("Odometry/TimeFiltering/ms", timeParticleFiltering*1000.0f));
stats.insert(std::make_pair("Odometry/LocalMapSize/", localMapSize));

1
corelib/src/OdometryThread.cpp
View File

@@ -134,6 +134,7 @@ void OdometryThread::mainLoop()
UDEBUG("Odom pose = %s", pose.prettyPrint().c_str());
if(!pose.isNull()) {
_previousGuessPose = event.info().odomPose;
UASSERT(event.info().odomPose.isNull() || !info.reg.covariance.empty());
if(!event.info().odomPose.isNull() && info.reg.covariance.at<double>(0,0) >= 9999 &&
(pose.x() != 0.0f || pose.y() != 0.0f || pose.z() != 0.0f)) // not the first frame
{

14
corelib/src/Rtabmap.cpp
View File

@@ -1502,8 +1502,8 @@ bool Rtabmap::process(
float angleToClosestNodeInTheGraph = 0;
if(_rgbdSlamMode)
{
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));
double linVar = odomCovariance.empty()?0.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()?0.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);
@@ -1607,6 +1607,7 @@ bool Rtabmap::process(
Transform t = _memory->computeTransform(oldId, signature->id(), guess, &info);
if(!t.isNull())
{
UASSERT(!info.covariance.empty() && info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
UINFO("Odometry refining: update neighbor link (%d->%d, variance:lin=%f, ang=%f) from %s to %s",
oldId,
signature->id(),
@@ -1614,7 +1615,6 @@ bool Rtabmap::process(
info.covariance.at<double>(5,5),
guess.prettyPrint().c_str(),
t.prettyPrint().c_str());
UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
_memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), t, info.covariance.inv()));
if(_optimizeFromGraphEnd)
@@ -1894,7 +1894,7 @@ bool Rtabmap::process(
*iter,
transform.prettyPrint().c_str());
// Add a loop constraint
UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
UASSERT(!info.covariance.empty() && info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
if(_memory->addLink(Link(signature->id(), *iter, Link::kLocalTimeClosure, transform, getInformation(info.covariance))))
{
++proximityDetectionsInTimeFound;
@@ -2782,7 +2782,7 @@ bool Rtabmap::process(
signature->id(),
nearestId,
transform.prettyPrint().c_str());
UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
UASSERT(!info.covariance.empty() && info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
//for statistics
loopClosureVisualInliersMeanDist = info.inliersMeanDistance;
@@ -2993,7 +2993,7 @@ bool Rtabmap::process(
}
// set Identify covariance for laser scan matching only
UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
UASSERT(!info.covariance.empty() && info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, getInformation(info.covariance)/_proximityMergedScanCovFactor, scanMatchingIds));
loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));
@@ -3081,7 +3081,7 @@ bool Rtabmap::process(
if(!rejectedLoopClosure)
{
// 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);
UASSERT(!info.covariance.empty() && 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));

5
corelib/src/RtabmapThread.cpp
View File

@@ -506,9 +506,10 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
ignoreFrame = true;
}
}
UASSERT(!odomEvent.info().reg.covariance.empty());
if(!lastPose_.isIdentity() &&
(odomEvent.pose().isIdentity() ||
odomEvent.info().reg.covariance.at<double>(0,0)>=9999))
(odomEvent.pose().isIdentity() ||
odomEvent.info().reg.covariance.at<double>(0,0)>=9999))
{
if(odomEvent.pose().isIdentity())
{