i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-06 18:17: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/RegistrationVis.cpp
+15 -5
View File
@@ -1964,7 +1964,7 @@ Transform RegistrationVis::computeTransformationImpl(
models.insert(std::make_pair(2, cameraModelsTo));
std::map<int, std::map<int, FeatureBA> > wordReferences;
std::set<int> sbaOutliers;
BAOutliers sbaOutliers;
UDEBUG("");
for(unsigned int i=0; i<inliers.size(); ++i)
{
@@ -2041,25 +2041,35 @@ Transform RegistrationVis::computeTransformationImpl(
{
UDEBUG("Pose optimization: %s -> %s", transform.prettyPrint().c_str(), optimizedPoses.rbegin()->second.prettyPrint().c_str());
if(sbaOutliers.size())
int sbaOutliersCount = 0;
for(unsigned int i=0; i<inliers.size(); ++i)
{
BAOutliers::const_iterator iter = sbaOutliers.find(inliers[i]);
if(iter != sbaOutliers.end() && iter->second.find(2) != iter->second.end())
{
++sbaOutliersCount;
}
}
if(sbaOutliersCount)
{
std::vector<int> newInliers(inliers.size());
int oi=0;
for(unsigned int i=0; i<inliers.size(); ++i)
{
if(sbaOutliers.find(inliers[i]) == sbaOutliers.end())
BAOutliers::const_iterator iter = sbaOutliers.find(inliers[i]);
if(iter == sbaOutliers.end() || iter->second.find(2) == iter->second.end())
{
newInliers[oi++] = inliers[i];
}
}
newInliers.resize(oi);
UDEBUG("BA outliers ratio %f", float(sbaOutliers.size())/float(inliers.size()));
UDEBUG("BA outliers ratio %f", float(sbaOutliersCount)/float(inliers.size()));
inliers = newInliers;
}
if((int)inliers.size() < _minInliers)
{
msg = uFormat("Not enough inliers after bundle adjustment %d/%d (matches=%d) between %d and %d",
(int)inliers.size(), _minInliers, (int)((int)inliers.size()+sbaOutliers.size()), fromSignature.id(), toSignature.id());
(int)inliers.size(), _minInliers, (int)(int)inliers.size()+sbaOutliersCount, fromSignature.id(), toSignature.id());
transform.setNull();
}
else