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/odometry/OdometryF2M.cpp
+17 -7
View File
@@ -26,7 +26,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "rtabmap/core/OdometryInfo.h"
#include "rtabmap/core/Memory.h"
#include "rtabmap/core/Signature.h"
#include "rtabmap/core/RegistrationVis.h"
#include "rtabmap/core/util3d.h"
@@ -41,6 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UTimer.h"
#include "rtabmap/utilite/UMath.h"
#include "rtabmap/utilite/UStl.h"
#include "rtabmap/utilite/UConversion.h"
#if CV_MAJOR_VERSION < 5
#include <opencv2/calib3d/calib3d.hpp>
@@ -473,14 +473,23 @@ Transform OdometryF2M::computeTransform(
UDEBUG("sba...start");
// set root negative to fix all other poses
std::set<int> sbaOutliers;
BAOutliers sbaOutliers;
UTimer bundleTimer;
bundlePoses = sba_->optimizeBA(-lastFrame_->id(), bundlePoses, bundleLinks, bundleModels, points3DMap, wordReferences, &sbaOutliers);
bundleTime = bundleTimer.ticks();
UDEBUG("sba...end");
totalBundleOutliers = (int)sbaOutliers.size();
int sbaOutliersCount = 0;
for(unsigned int i=0; i<regInfo.inliersIDs.size(); ++i)
{
BAOutliers::const_iterator iter = sbaOutliers.find(regInfo.inliersIDs[i]);
if(iter != sbaOutliers.end() && iter->second.find(lastFrame_->id()) != iter->second.end())
{
++sbaOutliersCount;
}
}
totalBundleOutliers = sbaOutliersCount;
UDEBUG("bundleTime=%fs (poses=%d wordRef=%d outliers=%d)", bundleTime, (int)bundlePoses.size(), (int)bundleWordReferences_.size(), (int)sbaOutliers.size());
UDEBUG("bundleTime=%fs (poses=%d wordRef=%d outliers=%d)", bundleTime, (int)bundlePoses.size(), (int)bundleWordReferences_.size(), sbaOutliersCount);
if(info)
{
info->localBundlePoses = bundlePoses;
@@ -497,14 +506,15 @@ Transform OdometryF2M::computeTransform(
{
info->localBundleOutliersPerCam = std::vector<int>(lastFrameModels.size(),0);
}
if(sbaOutliers.size())
if(sbaOutliersCount)
{
regInfo.inliersPerCam = std::vector<int>(lastFrameModels.size(),0);
std::vector<int> newInliers(regInfo.inliersIDs.size());
int oi=0;
for(unsigned int i=0; i<regInfo.inliersIDs.size(); ++i)
{
if(sbaOutliers.find(regInfo.inliersIDs[i]) == sbaOutliers.end())
BAOutliers::const_iterator iter = sbaOutliers.find(regInfo.inliersIDs[i]);
if(iter == sbaOutliers.end() || iter->second.find(lastFrame_->id()) == iter->second.end())
{
newInliers[oi++] = regInfo.inliersIDs[i];
regInfo.inliersPerCam[wordReferences.at(regInfo.inliersIDs[i]).at(lastFrame_->id()).cameraIndex] += 1;
@@ -515,7 +525,7 @@ Transform OdometryF2M::computeTransform(
}
}
newInliers.resize(oi);
UDEBUG("BA outliers ratio %f", float(sbaOutliers.size())/float(regInfo.inliersIDs.size()));
UDEBUG("BA outliers ratio %f", float(sbaOutliersCount)/float(regInfo.inliersIDs.size()));
regInfo.inliers = (int)newInliers.size();
regInfo.inliersIDs = newInliers;
}