Skip to content

Commit c0e83ce

Browse files
matlabbematlabbe
authored
Adding options to bound OdometryF2M keyframes (#1448)
* Added OdomF2M/BundleAdjustmentMinMotion and OdomF2M/BundleAdjustmentMaxKeyFramesPerFeature parameters * Added new odom stats localBundleMaxKeyFramesForInlier * Adjusted how localBundleMaxKeyFramesForInlier is computed when OdomF2M/BundleAdjustmentMaxFrames is used. * optimization: Dont compute average pixel distance if parameter is not used
1 parent d77a3e3 commit c0e83ce

File tree

7 files changed

+251
-115
lines changed

7 files changed

+251
-115
lines changed

‎corelib/include/rtabmap/core/OdometryInfo.h‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -49,6 +49,8 @@ class OdometryInfo
4949
localBundleOutliers(0),
5050
localBundleConstraints(0),
5151
localBundleTime(0),
52+
localBundleAvgInlierDistance(0.0f),
53+
localBundleMaxKeyFramesForInlier(0),
5254
keyFrameAdded(false),
5355
timeDeskewing(0.0f),
5456
timeEstimation(0.0f),
@@ -76,6 +78,8 @@ class OdometryInfo
7678
output.localBundleTime = localBundleTime;
7779
output.localBundlePoses = localBundlePoses;
7880
output.localBundleModels = localBundleModels;
81+
output.localBundleAvgInlierDistance = localBundleAvgInlierDistance;
82+
output.localBundleMaxKeyFramesForInlier = localBundleMaxKeyFramesForInlier;
7983
output.keyFrameAdded = keyFrameAdded;
8084
output.timeDeskewing = timeDeskewing;
8185
output.timeEstimation = timeEstimation;
@@ -106,6 +110,8 @@ class OdometryInfo
106110
float localBundleTime;
107111
std::map<int, Transform> localBundlePoses;
108112
std::map<int, std::vector<CameraModel> > localBundleModels;
113+
float localBundleAvgInlierDistance;
114+
int localBundleMaxKeyFramesForInlier;
109115
bool keyFrameAdded;
110116
float timeDeskewing;
111117
float timeEstimation;

‎corelib/include/rtabmap/core/Parameters.h‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -488,6 +488,8 @@ class RTABMAP_CORE_EXPORT Parameters
488488
RTABMAP_PARAM(OdomF2M, BundleAdjustment, int, 0, "Local bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres.");
489489
#endif
490490
RTABMAP_PARAM(OdomF2M, BundleAdjustmentMaxFrames, int, 10, "Maximum frames used for bundle adjustment (0=inf or all current frames in the local map).");
491+
RTABMAP_PARAM(OdomF2M, BundleAdjustmentMinMotion, float, 0.0, "To create a new keyframe with bundle adjustment, a minimum motion (in pixels) can be required. The motion is computed by the average distance between inliers of the previous keyframe and new frame.");
492+
RTABMAP_PARAM(OdomF2M, BundleAdjustmentMaxKeyFramesPerFeature, int, 0, "Maximum keyframes per feature for bundle adjustment. 0 means not limit.");
491493

492494
// Odometry Mono
493495
RTABMAP_PARAM(OdomMono, InitMinFlow, float, 100, "Minimum optical flow required for the initialization step.");

‎corelib/include/rtabmap/core/odometry/OdometryF2M.h‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -69,6 +69,8 @@ class RTABMAP_CORE_EXPORT OdometryF2M : public Odometry
6969
float scanMapMaxRange_;
7070
int bundleAdjustment_;
7171
int bundleMaxFrames_;
72+
float bundleMinMotion_;
73+
int bundleMaxKeyFramesPerFeature_;
7274
float validDepthRatio_;
7375
int pointToPlaneK_;
7476
float pointToPlaneRadius_;

‎corelib/src/odometry/OdometryF2M.cpp‎

Lines changed: 75 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -67,6 +67,8 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
6767
scanMapMaxRange_(Parameters::defaultOdomF2MScanRange()),
6868
bundleAdjustment_(Parameters::defaultOdomF2MBundleAdjustment()),
6969
bundleMaxFrames_(Parameters::defaultOdomF2MBundleAdjustmentMaxFrames()),
70+
bundleMinMotion_(Parameters::defaultOdomF2MBundleAdjustmentMinMotion()),
71+
bundleMaxKeyFramesPerFeature_(Parameters::defaultOdomF2MBundleAdjustmentMaxKeyFramesPerFeature()),
7072
validDepthRatio_(Parameters::defaultOdomF2MValidDepthRatio()),
7173
pointToPlaneK_(Parameters::defaultIcpPointToPlaneK()),
7274
pointToPlaneRadius_(Parameters::defaultIcpPointToPlaneRadius()),
@@ -91,6 +93,8 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
9193
Parameters::parse(parameters, Parameters::kOdomF2MScanRange(), scanMapMaxRange_);
9294
Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustment(), bundleAdjustment_);
9395
Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustmentMaxFrames(), bundleMaxFrames_);
96+
Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustmentMinMotion(), bundleMinMotion_);
97+
Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustmentMaxKeyFramesPerFeature(), bundleMaxKeyFramesPerFeature_);
9498
Parameters::parse(parameters, Parameters::kOdomF2MValidDepthRatio(), validDepthRatio_);
9599

96100
Parameters::parse(parameters, Parameters::kIcpPointToPlaneK(), pointToPlaneK_);
@@ -280,6 +284,7 @@ Transform OdometryF2M::computeTransform(
280284
std::map<int, Transform> bundlePoses;
281285
std::multimap<int, Link> bundleLinks;
282286
std::map<int, std::vector<CameraModel> > bundleModels;
287+
float bundleAvgInlierDistance = 0.0f;
283288

284289
for(int guessIteration=0;
285290
guessIteration<(!guess.isNull()&&regPipeline_->isImageRequired()?2:1) && transform.isNull();
@@ -386,6 +391,7 @@ Transform OdometryF2M::computeTransform(
386391

387392
UDEBUG("Fill matches (%d)", (int)regInfo.inliersIDs.size());
388393
std::map<int, std::map<int, FeatureBA> > wordReferences;
394+
size_t maxKeyFramesForInlier = 0;
389395
for(unsigned int i=0; i<regInfo.inliersIDs.size(); ++i)
390396
{
391397
int wordId =regInfo.inliersIDs[i];
@@ -398,6 +404,10 @@ Transform OdometryF2M::computeTransform(
398404
// all other references
399405
std::map<int, std::map<int, FeatureBA> >::iterator refIter = bundleWordReferences_.find(wordId);
400406
UASSERT_MSG(refIter != bundleWordReferences_.end(), uFormat("wordId=%d", wordId).c_str());
407+
if(info && refIter->second.size() > maxKeyFramesForInlier)
408+
{
409+
maxKeyFramesForInlier = refIter->second.size();
410+
}
401411

402412
std::map<int, FeatureBA> references;
403413
int step = bundleMaxFrames_>0?(refIter->second.size() / bundleMaxFrames_):1;
@@ -472,6 +482,7 @@ Transform OdometryF2M::computeTransform(
472482
{
473483
info->localBundlePoses = bundlePoses;
474484
info->localBundleModels = bundleModels;
485+
info->localBundleMaxKeyFramesForInlier = maxKeyFramesForInlier;
475486
}
476487

477488
UDEBUG("Local Bundle Adjustment Before: %s", transform.prettyPrint().c_str());
@@ -534,13 +545,52 @@ Transform OdometryF2M::computeTransform(
534545
regInfo.covariance.at<double>(4,4) *= 0.1;
535546
if(regInfo.covariance.at<double>(5,5)>thrAng)
536547
regInfo.covariance.at<double>(5,5) *= 0.1;
548+
549+
// Estimate how much the new frame moved from previous frame in term of pixels
550+
if(bundleMinMotion_ > 0.0f)
551+
{
552+
UASSERT(!bundlePoses_.empty());
553+
int count = 0;
554+
for(unsigned int i=0; i<regInfo.inliersIDs.size(); ++i)
555+
{
556+
std::map<int, std::map<int, FeatureBA> >::iterator wter = wordReferences.find(regInfo.inliersIDs[i]);
557+
if(wter != wordReferences.end())
558+
{
559+
std::map<int, FeatureBA>::iterator fter = wter->second.find(bundlePoses_.rbegin()->first);
560+
if(fter != wter->second.end())
561+
{
562+
const FeatureBA & f1 = fter->second; // previous key-frame
563+
const FeatureBA & f2 = wter->second.find(lastFrame_->id())->second; // current key-frame
564+
float dx = f1.kpt.pt.x - f2.kpt.pt.x;
565+
float dy = f1.kpt.pt.y - f2.kpt.pt.y;
566+
bundleAvgInlierDistance += sqrt(dx*dx + dy*dy);
567+
++count;
568+
}
569+
}
570+
}
571+
if(count)
572+
{
573+
bundleAvgInlierDistance /= count;
574+
}
575+
UDEBUG("Average pixel distance between %d inliers: %f", count, bundleAvgInlierDistance);
576+
if(info)
577+
{
578+
info->localBundleAvgInlierDistance = bundleAvgInlierDistance;
579+
}
580+
}
537581
}
582+
UDEBUG("Local Bundle Adjustment After : %s", transform.prettyPrint().c_str());
583+
}
584+
else
585+
{
586+
regInfo.rejectedMsg = "Last bundle pose is null?!";
587+
transform.setNull();
538588
}
539-
UDEBUG("Local Bundle Adjustment After : %s", transform.prettyPrint().c_str());
540589
}
541590
else
542591
{
543-
UWARN("Local bundle adjustment failed! transform is not refined.");
592+
regInfo.rejectedMsg = "Local bundle adjustment failed!";
593+
transform.setNull();
544594
}
545595
}
546596
}
@@ -598,15 +648,24 @@ Transform OdometryF2M::computeTransform(
598648
(keyFrameThr_ == 0.0f ||
599649
visKeyFrameThr_ == 0 ||
600650
float(regInfo.inliers) <= (keyFrameThr_*float(lastFrame_->getWords().size())) ||
601-
regInfo.inliers <= visKeyFrameThr_);
651+
regInfo.inliers <= visKeyFrameThr_) &&
652+
(bundleAdjustment_==0 || bundleAvgInlierDistance >= bundleMinMotion_);
602653

603654
bool addGeometricKeyFrame = regPipeline_->isScanRequired() &&
604655
(scanKeyFrameThr_==0 || regInfo.icpInliersRatio <= scanKeyFrameThr_);
605656

606-
addKeyFrame = false;//bundleLinks.rbegin()->second.transform().getNorm() > 5.0f*0.075f;
607-
addKeyFrame = addKeyFrame || addVisualKeyFrame || addGeometricKeyFrame;
657+
addKeyFrame = addVisualKeyFrame || addGeometricKeyFrame;
658+
659+
UDEBUG("keyframeThr=%f visKeyFrameThr_=%d matches=%d inliers=%d (avg dist=%f, min=%f) features=%d mp=%d",
660+
keyFrameThr_,
661+
visKeyFrameThr_,
662+
regInfo.matches,
663+
regInfo.inliers,
664+
bundleAvgInlierDistance,
665+
bundleMinMotion_,
666+
(int)lastFrame_->sensorData().keypoints().size(),
667+
(int)mapPoints.size());
608668

609-
UDEBUG("keyframeThr=%f visKeyFrameThr_=%d matches=%d inliers=%d features=%d mp=%d", keyFrameThr_, visKeyFrameThr_, regInfo.matches, regInfo.inliers, (int)lastFrame_->sensorData().keypoints().size(), (int)mapPoints.size());
610669
if(addKeyFrame)
611670
{
612671
//Visual
@@ -733,7 +792,16 @@ Transform OdometryF2M::computeTransform(
733792
}
734793
else
735794
{
736-
bundleWordReferences_.find(iter->first)->second.insert(std::make_pair(lastFrame_->id(), FeatureBA(kpt, depth, cv::Mat(), cameraIndex)));
795+
std::map<int, rtabmap::FeatureBA> & keyframes = bundleWordReferences_.find(iter->first)->second;
796+
if(bundleMaxKeyFramesPerFeature_ != 0 && (int)keyframes.size() > bundleMaxKeyFramesPerFeature_)
797+
{
798+
// To keep number of keyframes looking at same feature bounded
799+
int frameId = keyframes.rbegin()->first;
800+
UASSERT(bundlePoseReferences_.find(frameId) != bundlePoseReferences_.end());
801+
bundlePoseReferences_.at(frameId) -= 1;
802+
keyframes.erase(frameId);
803+
}
804+
keyframes.insert(std::make_pair(lastFrame_->id(), FeatureBA(kpt, depth, cv::Mat(), cameraIndex)));
737805
}
738806
}
739807
}

‎guilib/src/MainWindow.cpp‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -659,6 +659,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
659659
_ui->statsToolBox->updateStat("Odometry/localBundleOutliers/", false);
660660
_ui->statsToolBox->updateStat("Odometry/localBundleConstraints/", false);
661661
_ui->statsToolBox->updateStat("Odometry/localBundleTime/ms", false);
662+
_ui->statsToolBox->updateStat("Odometry/localBundleAvgInlierDistance/pix", false);
663+
_ui->statsToolBox->updateStat("Odometry/localBundleMaxKeyFramesForInlier/", false);
662664
_ui->statsToolBox->updateStat("Odometry/KeyFrameAdded/", false);
663665
_ui->statsToolBox->updateStat("Odometry/Interval/ms", false);
664666
_ui->statsToolBox->updateStat("Odometry/Speed/kph", false);
@@ -1829,6 +1831,8 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
18291831
_ui->statsToolBox->updateStat("Odometry/localBundleOutliers/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().localBundleOutliers, _preferencesDialog->isCacheSavedInFigures());
18301832
_ui->statsToolBox->updateStat("Odometry/localBundleConstraints/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().localBundleConstraints, _preferencesDialog->isCacheSavedInFigures());
18311833
_ui->statsToolBox->updateStat("Odometry/localBundleTime/ms", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().localBundleTime*1000.0f, _preferencesDialog->isCacheSavedInFigures());
1834+
_ui->statsToolBox->updateStat("Odometry/localBundleAvgInlierDistance/pix", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().localBundleAvgInlierDistance, _preferencesDialog->isCacheSavedInFigures());
1835+
_ui->statsToolBox->updateStat("Odometry/localBundleMaxKeyFramesForInlier/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().localBundleMaxKeyFramesForInlier, _preferencesDialog->isCacheSavedInFigures());
18321836
}
18331837
_ui->statsToolBox->updateStat("Odometry/KeyFrameAdded/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().keyFrameAdded?1.0f:0.0f, _preferencesDialog->isCacheSavedInFigures());
18341838
_ui->statsToolBox->updateStat("Odometry/ID/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)data->id(), _preferencesDialog->isCacheSavedInFigures());

‎guilib/src/PreferencesDialog.cpp‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1410,6 +1410,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
14101410
_ui->odom_f2m_validDepthRatio->setObjectName(Parameters::kOdomF2MValidDepthRatio().c_str());
14111411
_ui->odom_f2m_bundleStrategy->setObjectName(Parameters::kOdomF2MBundleAdjustment().c_str());
14121412
_ui->odom_f2m_bundleMaxFrames->setObjectName(Parameters::kOdomF2MBundleAdjustmentMaxFrames().c_str());
1413+
_ui->odom_f2m_bundleMinMotion->setObjectName(Parameters::kOdomF2MBundleAdjustmentMinMotion().c_str());
1414+
_ui->odom_f2m_bundleMaxKeyFramesPerFeature->setObjectName(Parameters::kOdomF2MBundleAdjustmentMaxKeyFramesPerFeature().c_str());
14131415

14141416
//Odometry Frame To Frame
14151417
_ui->comboBox_odomf2f_corType->setObjectName(Parameters::kVisCorType().c_str());

0 commit comments

Comments
 (0)