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

OptimizerG2O: track BA outliers per observation (#1741)

Browse Source 
* OptimizerG2O: track BA outliers per observation

Track rejected BA projections by word and pose, preserving a landmark's optimized estimate whenever at least one observation remains active. Add a deterministic g2o regression and focused Linux CTest coverage.

* Changed some error logs to warning

---------

Co-authored-by: happyman <[email protected]>
Co-authored-by: matlabbe <[email protected]>
This commit is contained in:
1m-pony
2026-08-09 17:36:27 -07:00
committed by GitHub
co-authored by happyman matlabbe
parent 338e142e58
commit df52523a0c
17 changed files with 823 additions and 235 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Memory.cpp
+15 -5
View File
@@ -3711,7 +3711,7 @@ Transform Memory::computeTransform(
UDEBUG("sba...start");
// set root negative to fix all other poses
std::set<int> sbaOutliers;
BAOutliers sbaOutliers;
UTimer bundleTimer;
OptimizerG2O sba(parameters_);
sba.setIterations(5);
@@ -3719,24 +3719,34 @@ Transform Memory::computeTransform(
bundlePoses = sba.optimizeBA(-toS.id(), bundlePoses, bundleLinks, bundleModels, points3DMap, wordReferences, &sbaOutliers);
UDEBUG("sba...end");
UDEBUG("bundleTime=%fs (poses=%d wordRef=%d outliers=%d)", bundleTime.ticks(), (int)bundlePoses.size(), totalWordReferences, (int)sbaOutliers.size());
int sbaOutliersCount = 0;
for(unsigned int i=0; i<info->inliersIDs.size(); ++i)
{
BAOutliers::const_iterator iter = sbaOutliers.find(info->inliersIDs[i]);
if(iter != sbaOutliers.end() && iter->second.find(toS.id()) != iter->second.end())
{
++sbaOutliersCount;
}
}
UDEBUG("bundleTime=%fs (poses=%d wordRef=%d outliers=%d)", bundleTime.ticks(), (int)bundlePoses.size(), totalWordReferences, sbaOutliersCount);
UDEBUG("Local Bundle Adjustment Before: %s", transform.prettyPrint().c_str());
if(!bundlePoses.rbegin()->second.isNull())
{
if(sbaOutliers.size())
if(sbaOutliersCount)
{
std::vector<int> newInliers(info->inliersIDs.size());
int oi=0;
for(unsigned int i=0; i<info->inliersIDs.size(); ++i)
{
if(sbaOutliers.find(info->inliersIDs[i]) == sbaOutliers.end())
BAOutliers::const_iterator iter = sbaOutliers.find(info->inliersIDs[i]);
if(iter == sbaOutliers.end() || iter->second.find(toS.id()) == iter->second.end())
{
newInliers[oi++] = info->inliersIDs[i];
}
}
newInliers.resize(oi);
UDEBUG("BA outliers ratio %f", float(sbaOutliers.size())/float(info->inliersIDs.size()));
UDEBUG("BA outliers ratio %f", float(sbaOutliersCount)/float(info->inliersIDs.size()));
info->inliers = (int)newInliers.size();
info->inliersIDs = newInliers;
}