Repository: introlab/rtabmap
Pull request #1460: Improved depth estimation of mono features
Author: login: matlabbe
Created: 2025-03-04T23:11:12Z | Merged: 2025-03-09T04:30:26Z

Added parameters:
* `OdomF2M/BundleUpdateFeatureMapOnAllFrames`: Update 3D local feature map on every frame with bundle adjustment. Recommended if `Vis/DepthAsMask=false` and `Mem/UseOdomFeatures=true` so that features without depth are better triangulated on every frame (not only on keyframes). If disabled, the feature map is updated only when a new keyframe is added (legacy approach).
* `OdomF2M/FloorThreshold`: Only track features in 3D feature map that are over this threshold (height in base frame). Can be useful to ignore reflections on the floor. 0 means disabled.
* `OdomF2M/InitDepthFactor`: Depth factor used to initialize depth of features without depth. Depth = Factor * fx.

TODO: Re-run vslam benchmark datasets with and without `OdomF2M/BundleUpdateFeatureMapOnAllFrames` to see if it doesn't affect too negatively the results. Ideally, we would want `OdomF2M/BundleUpdateFeatureMapOnAllFrames` to be true by default so that depth of mono features can be correctly estimated all the time.


Diff:
diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h
index 8b4e5336e4..2bbc506c29 100644
--- a/corelib/include/rtabmap/core/Parameters.h
+++ b/corelib/include/rtabmap/core/Parameters.h
@@ -477,6 +477,8 @@ class RTABMAP_CORE_EXPORT Parameters
     // Odometry Frame-to-Map
     RTABMAP_PARAM(OdomF2M, MaxSize,             int, 2000,    "[Visual] Local map size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words.");
     RTABMAP_PARAM(OdomF2M, MaxNewFeatures,      int, 0,       "[Visual] Maximum features (sorted by keypoint response) added to local map from a new key-frame. 0 means no limit.");
+    RTABMAP_PARAM(OdomF2M, InitDepthFactor,     float, 0.05,  "[Visual] Depth factor used to initialize depth of features without depth. Depth = Factor * fx.");
+    RTABMAP_PARAM(OdomF2M, FloorThreshold,      float, 0.0,   "[Visual] Only track features in 3D feature map that are over this threshold (height in base frame). Can be useful to ignore reflections on the floor. 0 means disabled.");
     RTABMAP_PARAM(OdomF2M, ScanMaxSize,         int, 2000,    "[Geometry] Maximum local scan map size.");
     RTABMAP_PARAM(OdomF2M, ScanSubtractRadius,  float, 0.05,  "[Geometry] Radius used to filter points of a new added scan to local map. This could match the voxel size of the scans.");
     RTABMAP_PARAM(OdomF2M, ScanSubtractAngle,   float, 45,    uFormat("[Geometry] Max angle (degrees) used to filter points of a new added scan to local map (when \"%s\">0). 0 means any angle.", kOdomF2MScanSubtractRadius().c_str()).c_str());
@@ -490,6 +492,7 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(OdomF2M, BundleAdjustmentMaxFrames, int, 10, "Maximum frames used for bundle adjustment (0=inf or all current frames in the local map).");
     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.");
     RTABMAP_PARAM(OdomF2M, BundleAdjustmentMaxKeyFramesPerFeature, int, 0, "Maximum keyframes per feature for bundle adjustment. 0 means not limit.");
+    RTABMAP_PARAM(OdomF2M, BundleUpdateFeatureMapOnAllFrames, bool, false, uFormat("Update 3D local feature map on every frame with bundle adjustment. Recommended if %s=false and %s=true so that features without depth are better triangulated on every frame (not only on keyframes). If disabled, the feature map is updated only when a new keyframe is added (legacy approach).", kVisDepthAsMask().c_str(), kMemUseOdomFeatures().c_str()));
 
     // Odometry Mono
     RTABMAP_PARAM(OdomMono, InitMinFlow,        float, 100,  "Minimum optical flow required for the initialization step.");
diff --git a/corelib/include/rtabmap/core/odometry/OdometryF2M.h b/corelib/include/rtabmap/core/odometry/OdometryF2M.h
index feeb4cc1ac..356e2d95b4 100644
--- a/corelib/include/rtabmap/core/odometry/OdometryF2M.h
+++ b/corelib/include/rtabmap/core/odometry/OdometryF2M.h
@@ -62,6 +62,8 @@ class RTABMAP_CORE_EXPORT OdometryF2M : public Odometry
 	float keyFrameThr_;
 	int visKeyFrameThr_;
 	int maxNewFeatures_;
+	float initDepthFactor_;
+	float floorThreshold_;
 	float scanKeyFrameThr_;
 	int scanMaximumMapSize_;
 	float scanSubtractRadius_;
@@ -71,6 +73,7 @@ class RTABMAP_CORE_EXPORT OdometryF2M : public Odometry
 	int bundleMaxFrames_;
 	float bundleMinMotion_;
 	int bundleMaxKeyFramesPerFeature_;
+	bool bundleUpdateFeatureMapOnAllFrames_;
 	float validDepthRatio_;
 	int pointToPlaneK_;
 	float pointToPlaneRadius_;
diff --git a/corelib/src/odometry/OdometryF2M.cpp b/corelib/src/odometry/OdometryF2M.cpp
index 73e4961f2b..1ab4c296ed 100644
--- a/corelib/src/odometry/OdometryF2M.cpp
+++ b/corelib/src/odometry/OdometryF2M.cpp
@@ -60,6 +60,8 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
 	keyFrameThr_(Parameters::defaultOdomKeyFrameThr()),
 	visKeyFrameThr_(Parameters::defaultOdomVisKeyFrameThr()),
 	maxNewFeatures_(Parameters::defaultOdomF2MMaxNewFeatures()),
+	initDepthFactor_(Parameters::defaultOdomF2MInitDepthFactor()),
+	floorThreshold_(Parameters::defaultOdomF2MFloorThreshold()),
 	scanKeyFrameThr_(Parameters::defaultOdomScanKeyFrameThr()),
 	scanMaximumMapSize_(Parameters::defaultOdomF2MScanMaxSize()),
 	scanSubtractRadius_(Parameters::defaultOdomF2MScanSubtractRadius()),
@@ -69,6 +71,7 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
 	bundleMaxFrames_(Parameters::defaultOdomF2MBundleAdjustmentMaxFrames()),
 	bundleMinMotion_(Parameters::defaultOdomF2MBundleAdjustmentMinMotion()),
 	bundleMaxKeyFramesPerFeature_(Parameters::defaultOdomF2MBundleAdjustmentMaxKeyFramesPerFeature()),
+	bundleUpdateFeatureMapOnAllFrames_(Parameters::defaultOdomF2MBundleUpdateFeatureMapOnAllFrames()),
 	validDepthRatio_(Parameters::defaultOdomF2MValidDepthRatio()),
 	pointToPlaneK_(Parameters::defaultIcpPointToPlaneK()),
 	pointToPlaneRadius_(Parameters::defaultIcpPointToPlaneRadius()),
@@ -83,6 +86,8 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
 	Parameters::parse(parameters, Parameters::kOdomKeyFrameThr(), keyFrameThr_);
 	Parameters::parse(parameters, Parameters::kOdomVisKeyFrameThr(), visKeyFrameThr_);
 	Parameters::parse(parameters, Parameters::kOdomF2MMaxNewFeatures(), maxNewFeatures_);
+	Parameters::parse(parameters, Parameters::kOdomF2MInitDepthFactor(), initDepthFactor_);
+	Parameters::parse(parameters, Parameters::kOdomF2MFloorThreshold(), floorThreshold_);
 	Parameters::parse(parameters, Parameters::kOdomScanKeyFrameThr(), scanKeyFrameThr_);
 	Parameters::parse(parameters, Parameters::kOdomF2MScanMaxSize(), scanMaximumMapSize_);
 	Parameters::parse(parameters, Parameters::kOdomF2MScanSubtractRadius(), scanSubtractRadius_);
@@ -95,6 +100,7 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
 	Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustmentMaxFrames(), bundleMaxFrames_);
 	Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustmentMinMotion(), bundleMinMotion_);
 	Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustmentMaxKeyFramesPerFeature(), bundleMaxKeyFramesPerFeature_);
+	Parameters::parse(parameters, Parameters::kOdomF2MBundleUpdateFeatureMapOnAllFrames(), bundleUpdateFeatureMapOnAllFrames_);
 	Parameters::parse(parameters, Parameters::kOdomF2MValidDepthRatio(), validDepthRatio_);
 
 	Parameters::parse(parameters, Parameters::kIcpPointToPlaneK(), pointToPlaneK_);
@@ -124,6 +130,7 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
 	UASSERT(visKeyFrameThr_>=0);
 	UASSERT(scanKeyFrameThr_ >= 0.0f && scanKeyFrameThr_<=1.0f);
 	UASSERT(maxNewFeatures_ >= 0);
+	UASSERT(initDepthFactor_>0.0f);
 
 	int corType = Parameters::defaultVisCorType();
 	Parameters::parse(parameters, Parameters::kVisCorType(), corType);
@@ -644,6 +651,51 @@ Transform OdometryF2M::computeTransform(
 				std::vector<cv::Point3f> mapPoints = tmpMap.getWords3();
 				cv::Mat mapDescriptors = tmpMap.getWordsDescriptors();
 
+				// update last frame features without depth (if bundle adjustment was done)
+				// Do this before adding bundle frames to keep mono observations without depth
+				bool lastFrameWords3Updated = false;
+				std::vector<cv::Point3f> lastFrameWords3;
+				if( regPipeline_->isImageRequired() &&
+					!visDepthAsMask &&
+				    bundleAdjustment_>0 &&
+					!lastFrame_->getWords().empty() &&
+					lastFrame_->getWords().size() == lastFrame_->getWords3().size() &&
+					!points3DMap.empty())
+				{
+					lastFrameWords3 = lastFrame_->getWords3();
+					Transform newFramePoseInv = newFramePose.inverse();
+					for(std::multimap<int, int>::const_iterator iter=lastFrame_->getWords().begin();
+					    iter!=lastFrame_->getWords().end();
+						++iter)
+					{
+						cv::Point3f & pt = lastFrameWords3.at(iter->second);
+						if(!util3d::isFinite(pt))
+						{
+							std::map<int, cv::Point3f>::iterator mapIter = points3DMap.find(iter->first);
+							if(mapIter != points3DMap.end())
+							{
+								// in base frame
+								pt = util3d::transformPoint(mapIter->second, newFramePoseInv);
+								lastFrameWords3Updated = true;
+							}
+						}
+					}
+				}
+
+				if( regPipeline_->isImageRequired() &&
+					bundleAdjustment_>0 &&
+					bundleUpdateFeatureMapOnAllFrames_ &&
+					!points3DMap.empty())
+				{
+					// update local map 3D points (if bundle adjustment was done)
+					for(std::map<int, cv::Point3f>::iterator iter=points3DMap.begin(); iter!=points3DMap.end(); ++iter)
+					{
+						UASSERT(mapWords.count(iter->first) == 1);
+						mapPoints[mapWords.find(iter->first)->second] = iter->second;
+					}
+					modified = true;
+				}
+
 				bool addVisualKeyFrame = regPipeline_->isImageRequired() &&
 						 (keyFrameThr_ == 0.0f ||
 						  visKeyFrameThr_ == 0 ||
@@ -674,6 +726,7 @@ Transform OdometryF2M::computeTransform(
 					UTimer tmpTimer;
 
 					UDEBUG("Update local map");
+					modified = bundleAdjustment_>0; // We always add new references even if we don't add/remove points
 
 					// update local map
 					UASSERT(mapWords.size() == mapPoints.size());
@@ -698,12 +751,14 @@ Transform OdometryF2M::computeTransform(
 						bundleModels_.insert(*bundleModels.find(lastFrame_->id()));
 						iterBundlePosesRef = bundlePoseReferences_.find(lastFrame_->id());
 
-						// update local map 3D points (if bundle adjustment was done)
-						for(std::map<int, cv::Point3f>::iterator iter=points3DMap.begin(); iter!=points3DMap.end(); ++iter)
+						if(!bundleUpdateFeatureMapOnAllFrames_)
 						{
-							UASSERT(mapWords.count(iter->first) == 1);
-							//UDEBUG("Updated %d (%f,%f,%f) -> (%f,%f,%f)", iter->first, mapPoints[mapWords.find(iter->first)->second].x, mapPoints[mapWords.find(iter->first)->second].y, mapPoints[mapWords.find(iter->first)->second].z, iter->second.x, iter->second.y, iter->second.z);
-							mapPoints[mapWords.find(iter->first)->second] = iter->second;
+							// update local map 3D points (if bundle adjustment was done)
+							for(std::map<int, cv::Point3f>::iterator iter=points3DMap.begin(); iter!=points3DMap.end(); ++iter)
+							{
+								UASSERT(mapWords.count(iter->first) == 1);
+								mapPoints[mapWords.find(iter->first)->second] = iter->second;
+							}
 						}
 					}
 
@@ -818,6 +873,28 @@ Transform OdometryF2M::computeTransform(
 						if(maxNewFeatures_ == 0  || added < maxNewFeatures_)
 						{
 							int cameraIndex = iter->second.second.second.second.second;
+							cv::Point3f pt = iter->second.second.second.first;
+							if(!util3d::isFinite(pt))
+							{
+								// get the ray instead
+								float x = iter->second.second.first.pt.x; //subImageWidth should be already removed
+								float y = iter->second.second.first.pt.y;
+								Eigen::Vector3f ray = util3d::projectDepthTo3DRay(
+										lastFrameModels[cameraIndex].imageSize(),
+										x,
+										y,
+										lastFrameModels[cameraIndex].cx(),
+										lastFrameModels[cameraIndex].cy(),
+										lastFrameModels[cameraIndex].fx(),
+										lastFrameModels[cameraIndex].fy());
+								float scaleInf = initDepthFactor_ * lastFrameModels[cameraIndex].fx();
+								pt = util3d::transformPoint(cv::Point3f(ray[0]*scaleInf, ray[1]*scaleInf, ray[2]*scaleInf), lastFrameModels[cameraIndex].localTransform()); // in base_link frame
+							}
+							if(floorThreshold_ != 0.0f && pt.z < floorThreshold_)
+							{
+								continue;
+							}
+
 							if(bundleAdjustment_>0)
 							{
 								if(lastFrame_->getWords().count(iter->second.first) == 1)
@@ -846,23 +923,6 @@ Transform OdometryF2M::computeTransform(
 
 							mapWords.insert(mapWords.end(), std::make_pair(iter->second.first, mapWords.size()));
 							mapWordsKpts.push_back(iter->second.second.first);
-							cv::Point3f pt = iter->second.second.second.first;
-							if(!util3d::isFinite(pt))
-							{
-								// get the ray instead
-								float x = iter->second.second.first.pt.x; //subImageWidth should be already removed
-								float y = iter->second.second.first.pt.y;
-								Eigen::Vector3f ray = util3d::projectDepthTo3DRay(
-										lastFrameModels[cameraIndex].imageSize(),
-										x,
-										y,
-										lastFrameModels[cameraIndex].cx(),
-										lastFrameModels[cameraIndex].cy(),
-										lastFrameModels[cameraIndex].fx(),
-										lastFrameModels[cameraIndex].fy());
-								float scaleInf = (0.05 * lastFrameModels[cameraIndex].fx()) / 0.01;
-								pt = util3d::transformPoint(cv::Point3f(ray[0]*scaleInf, ray[1]*scaleInf, ray[2]*scaleInf), lastFrameModels[cameraIndex].localTransform()); // in base_link frame
-							}
 							mapPoints.push_back(util3d::transformPoint(pt, newFramePose));
 							mapDescriptors.push_back(iter->second.second.second.second.first);
 							if(lastFrameOldestNewId_ > iter->second.first)
@@ -871,6 +931,10 @@ Transform OdometryF2M::computeTransform(
 							}
 							++added;
 						}
+						else
+						{
+							break;
+						}
 					}
 					UDEBUG("");
 
@@ -1193,6 +1257,12 @@ Transform OdometryF2M::computeTransform(
 
 					map_->setWords(mapWords, mapWordsKpts, mapPoints, mapDescriptors);
 				}
+
+				if(lastFrameWords3Updated)
+				{
+					// update output with refined 3d points from bundle adjustment
+					data.setFeatures(lastFrame_->getWordsKpts(), lastFrameWords3, lastFrame_->getWordsDescriptors());
+				}
 			}
 
 			if(info)
diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp
index 1f7756c6e4..86dd0eff0b 100644
--- a/guilib/src/PreferencesDialog.cpp
+++ b/guilib/src/PreferencesDialog.cpp
@@ -1406,15 +1406,18 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	//Odometry Frame to Map
 	_ui->odom_localHistory->setObjectName(Parameters::kOdomF2MMaxSize().c_str());
 	_ui->spinBox_odom_f2m_maxNewFeatures->setObjectName(Parameters::kOdomF2MMaxNewFeatures().c_str());
+	_ui->doubleSpinBox_odom_f2m_floorThreshold->setObjectName(Parameters::kOdomF2MFloorThreshold().c_str());
 	_ui->spinBox_odom_f2m_scanMaxSize->setObjectName(Parameters::kOdomF2MScanMaxSize().c_str());
 	_ui->doubleSpinBox_odom_f2m_scanRadius->setObjectName(Parameters::kOdomF2MScanSubtractRadius().c_str());
 	_ui->doubleSpinBox_odom_f2m_scanAngle->setObjectName(Parameters::kOdomF2MScanSubtractAngle().c_str());
 	_ui->doubleSpinBox_odom_f2m_scanRange->setObjectName(Parameters::kOdomF2MScanRange().c_str());
 	_ui->odom_f2m_validDepthRatio->setObjectName(Parameters::kOdomF2MValidDepthRatio().c_str());
+	_ui->odom_f2m_initDepthFactor->setObjectName(Parameters::kOdomF2MInitDepthFactor().c_str());
 	_ui->odom_f2m_bundleStrategy->setObjectName(Parameters::kOdomF2MBundleAdjustment().c_str());
 	_ui->odom_f2m_bundleMaxFrames->setObjectName(Parameters::kOdomF2MBundleAdjustmentMaxFrames().c_str());
 	_ui->odom_f2m_bundleMinMotion->setObjectName(Parameters::kOdomF2MBundleAdjustmentMinMotion().c_str());
 	_ui->odom_f2m_bundleMaxKeyFramesPerFeature->setObjectName(Parameters::kOdomF2MBundleAdjustmentMaxKeyFramesPerFeature().c_str());
+	_ui->odom_f2m_bundleUpdateFeatureMapOnAllFrames->setObjectName(Parameters::kOdomF2MBundleUpdateFeatureMapOnAllFrames().c_str());
 
 	//Odometry Frame To Frame
 	_ui->comboBox_odomf2f_corType->setObjectName(Parameters::kVisCorType().c_str());
diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui
index 81ba069945..999c1412e6 100644
--- a/guilib/src/ui/preferencesDialog.ui
+++ b/guilib/src/ui/preferencesDialog.ui
@@ -63,7 +63,7 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-639</y>
+            <y>-989</y>
             <width>713</width>
             <height>4705</height>
            </rect>
@@ -95,7 +95,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>5</number>
+                <number>19</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,0">
@@ -15880,10 +15880,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                            </item>
                            <item>
                             <layout class="QGridLayout" name="gridLayout_29" columnstretch="0,1">
-                             <item row="6" column="1">
-                              <widget class="QLabel" name="label_524">
+                             <item row="5" column="1">
+                              <widget class="QLabel" name="label_430">
                                <property name="text">
-                                <string>[Visual] If a new frame has points without valid depth, they are added to local feature map only if points with valid depth on total points is over this ratio. Setting to 1 means no points without valid depth are added to local feature map.</string>
+                                <string>[Geometry] Max angle (degrees) used to filter points of a new added scan to local map (when Radius above is &gt;0). 0 means any angle.</string>
                                </property>
                                <property name="wordWrap">
                                 <bool>true</bool>
@@ -15893,26 +15893,36 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                                </property>
                               </widget>
                              </item>
-                             <item row="5" column="0">
-                              <widget class="QDoubleSpinBox" name="doubleSpinBox_odom_f2m_scanRange">
-                               <property name="suffix">
-                                <string> m</string>
+                             <item row="4" column="1">
+                              <widget class="QLabel" name="label_215">
+                               <property name="text">
+                                <string>[Geometry] Radius used to filter points of a new added scan to local map. This could match the voxel size of the laser scans.</string>
                                </property>
-                               <property name="decimals">
-                                <number>0</number>
+                               <property name="wordWrap">
+                                <bool>true</bool>
                                </property>
-                               <property name="maximum">
-                                <double>9999.000000000000000</double>
+                               <property name="textInteractionFlags">
+                                <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
                                </property>
-                               <property name="value">
-                                <double>0.000000000000000</double>
+                              </widget>
+                             </item>
+                             <item row="9" column="1">
+                              <widget class="QLabel" name="label_357">
+                               <property name="text">
+                                <string>[Visual] Local bundle adjustment. See Optimizer panel. This will not work if Optical Flow correspondences strategy is selected in Visual Registration panel.</string>
+                               </property>
+                               <property name="wordWrap">
+                                <bool>true</bool>
+                               </property>
+                               <property name="textInteractionFlags">
+                                <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
                                </property>
                               </widget>
                              </item>
-                             <item row="3" column="1">
-                              <widget class="QLabel" name="label_215">
+                             <item row="10" column="1">
+                              <widget class="QLabel" name="label_358">
                                <property name="text">
-                                <string>[Geometry] Radius used to filter points of a new added scan to local map. This could match the voxel size of the laser scans.</string>
+                                <string>[Visual] Maximum frames used for bundle adjustment (0=inf or all current frames in the local map).</string>
                                </property>
                                <property name="wordWrap">
                                 <bool>true</bool>
@@ -15938,10 +15948,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                                </property>
                               </widget>
                              </item>
-                             <item row="2" column="1">
-                              <widget class="QLabel" name="label_195">
+                             <item row="13" column="1">
+                              <widget class="QLabel" name="label_762">
                                <property name="text">
-                                <string>[Geometry] Maximum local scan map size (points).</string>
+                                <string>[Visual] Maximum keyframes per feature for bundle adjustment. 0 means not limit.</string>
                                </property>
                                <property name="wordWrap">
                                 <bool>true</bool>
@@ -15951,20 +15961,14 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                                </property>
                               </widget>
                              </item>
-                             <item row="0" column="1">
-                              <widget class="QLabel" name="label_190">
+                             <item row="14" column="0">
+                              <widget class="QCheckBox" name="odom_f2m_bundleUpdateFeatureMapOnAllFrames">
                                <property name="text">
-                                <string>[Visual] Maximum map size: If &gt; 0 (example 5000), the odometry will maintain a local map of X maximum features. This will decrease odometry drifting when the camera is not moving.</string>
-                               </property>
-                               <property name="wordWrap">
-                                <bool>true</bool>
-                               </property>
-                               <property name="textInteractionFlags">
-                                <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                <string/>
                                </property>
                               </widget>
                              </item>
-                             <item row="10" column="1">
+                             <item row="12" column="1">
                               <widget class="QLabel" name="label_761">
                                <property name="text">
                                 <string>[Visual] 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.</string>
@@ -15977,23 +15981,17 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                                </property>
                               </widget>
                              </item>
-                             <item row="8" column="1">
-                              <widget class="QLabel" name="label_358">
-                               <property name="text">
-                                <string>[Visual] Maximum frames used for bundle adjustment (0=inf or all current frames in the local map).</string>
-                               </property>
-                               <property name="wordWrap">
-                                <bool>true</bool>
-                               </property>
-                               <property name="textInteractionFlags">
-                                <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                             <item row="3" column="0">
+                              <widget class="QSpinBox" name="spinBox_odom_f2m_scanMaxSize">
+                               <property name="maximum">
+                                <number>999999999</number>
                                </property>
                               </widget>
                              </item>
-                             <item row="9" column="1">
-                              <widget class="QLabel" name="label_598">
+                             <item row="0" column="1">
+                              <widget class="QLabel" name="label_190">
                                <property name="text">
-                                <string>[Visual] Gravity sigma used for bundle adjustment (&lt;0, use same value than Optimizer/GravitySigma parameter)</string>
+                                <string>[Visual] Maximum map size: If &gt; 0 (example 5000), the odometry will maintain a local map of X maximum features. This will decrease odometry drifting when the camera is not moving.</string>
                                </property>
                                <property name="wordWrap">
                                 <bool>true</bool>
@@ -16003,7 +16001,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                                </property>
                               </widget>
                              </item>
-                             <item row="4" column="0">
+                             <item row="5" column="0">
                               <widget class="QDoubleSpinBox" name="doubleSpinBox_odom_f2m_scanAngle">
                                <property name="suffix">
                                 <string> deg</string>
@@ -16019,17 +16017,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                                </property>
                               </widget>
                              </item>
-                             <item row="8" column="0">
-                              <widget class="QSpinBox" name="odom_f2m_bundleMaxFrames">
-                               <property name="maximum">
-                                <number>999999</number>
-                               </property>
-                              </widget>
-                             </item>
-                             <item row="7" column="1">
-                              <widget class="QLabel" name="label_357">
+                             <item row="6" column="1">
+                              <widget class="QLabel" name="label_564">
                                <property name="text">
-                                <string>[Visual] Local bundle adjustment. See Optimizer panel. This will not work if Optical Flow correspondences strategy is selected in Visual Registration panel.</string>
+                                <string>[Geometry] Distance Range used to filter points of local map (when &gt; 0). 0 means local map is updated using time and not range.</string>
                                </property>
                                <property name="wordWrap">
                                 <bool>true</bool>
@@ -16039,23 +16030,36 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                                </property>
                               </widget>
                              </item>
-                             <item row="1" column="0">
-                              <widget class="QSpinBox" name="spinBox_odom_f2m_maxNewFeatures">
+                             <item row="11" column="0">
+                              <widget class="QDoubleSpinBox" name="odom_f2m_gravitySigma">
+                               <property name="suffix">
+                                <string/>
+                               </property>
+                               <property name="decimals">
+                                <number>4</number>
+                               </property>
                                <property name="minimum">
-                                <number>0</number>
+                                <double>-1.000000000000000</double>
                                </property>
                                <property name="maximum">
-                                <number>999999</number>
+                                <double>10.000000000000000</double>
                                </property>
                                <property name="singleStep">
-                                <number>1</number>
+                                <double>0.100000000000000</double>
                                </property>
                                <property name="value">
-                                <number>0</number>
+                                <double>-1.000000000000000</double>
                                </property>
                               </widget>
                              </item>
                              <item row="10" column="0">
+                              <widget class="QSpinBox" name="odom_f2m_bundleMaxFrames">
+                               <property name="maximum">
+                                <number>999999</number>
+                               </property>
+                              </widget>
+                             </item>
+                             <item row="12" column="0">
                               <widget class="QDoubleSpinBox" name="odom_f2m_bundleMinMotion">
                                <property name="suffix">
                                 <string> pixels</string>
@@ -16075,32 +16079,64 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                               </widget>
                              </item>
                              <item row="2" column="0">
-                              <widget class="QSpinBox" name="spinBox_odom_f2m_scanMaxSize">
-                               <property name="maximum">
-                                <number>999999999</number>
-                               </property>
-                              </widget>
-                             </item>
-                             <item row="6" column="0">
-                              <widget class="QDoubleSpinBox" name="odom_f2m_validDepthRatio">
+                              <widget class="QDoubleSpinBox" name="doubleSpinBox_odom_f2m_floorThreshold">
                                <property name="suffix">
-                                <string/>
+                                <string> m</string>
                                </property>
                                <property name="decimals">
                                 <number>2</number>
                                </property>
-                               <property name="maximum">
-                                <double>1.000000000000000</double>
+                               <property name="minimum">
+                                <double>-99.000000000000000</double>
                                </property>
                                <property name="singleStep">
-                                <double>0.050000000000000</double>
+                                <double>0.100000000000000</double>
                                </property>
                                <property name="value">
-                                <double>0.750000000000000</double>
+                                <double>0.000000000000000</double>
                                </property>
                               </widget>
                              </item>
-                             <item row="3" column="0">
+                             <item row="11" column="1">
+                              <widget class="QLabel" name="label_598">
+                               <property name="text">
+                                <string>[Visual] Gravity sigma used for bundle adjustment (&lt;0, use same value than Optimizer/GravitySigma parameter)</string>
+                               </property>
+                               <property name="wordWrap">
+                                <bool>true</bool>
+                               </property>
+                               <property name="textInteractionFlags">
+                                <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                               </property>
+                              </widget>
+                             </item>
+                             <item row="8" column="1">
+                              <widget class="QLabel" name="label_765">
+                               <property name="text">
+                                <string>[Visual] Depth factor used to initialize depth of features without depth. Depth = Factor * fx.</string>
+                               </property>
+                               <property name="wordWrap">
+                                <bool>true</bool>
+                               </property>
+                               <property name="textInteractionFlags">
+                                <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                               </property>
+                              </widget>
+                             </item>
+                             <item row="7" column="1">
+                              <widget class="QLabel" name="label_524">
+                               <property name="text">
+                                <string>[Visual] If a new frame has points without valid depth, they are added to local feature map only if points with valid depth on total points is over this ratio. Setting to 1 means no points without valid depth are added to local feature map.</string>
+                               </property>
+                               <property name="wordWrap">
+                                <bool>true</bool>
+                               </property>
+                               <property name="textInteractionFlags">
+                                <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                               </property>
+                              </widget>
+                             </item>
+                             <item row="4" column="0">
                               <widget class="QDoubleSpinBox" name="doubleSpinBox_odom_f2m_scanRadius">
                                <property name="suffix">
                                 <string> m</string>
@@ -16113,33 +16149,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                                </property>
                               </widget>
                              </item>
-                             <item row="7" column="0">
-                              <widget class="QComboBox" name="odom_f2m_bundleStrategy">
-                               <property name="sizeAdjustPolicy">
-                                <enum>QComboBox::AdjustToContents</enum>
-                               </property>
-                               <item>
-                                <property name="text">
-                                 <string>Disabled</string>
-                                </property>
-                               </item>
-                               <item>
-                                <property name="text">
-                                 <string>g2o</string>
-                                </property>
-                               </item>
-                               <item>
-                                <property name="text">
-                                 <string>cvsba</string>
-                                </property>
-                               </item>
-                               <item>
-                                <property name="text">
-                                 <string>Ceres</string>
-                                </property>
-                               </item>
-                              </widget>
-                             </item>
                              <item row="1" column="1">
                               <widget class="QLabel" name="label_194">
                                <property name="text">
@@ -16153,10 +16162,29 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                                </property>
                               </widget>
                              </item>
-                             <item row="4" column="1">
-                              <widget class="QLabel" name="label_430">
+                             <item row="7" column="0">
+                              <widget class="QDoubleSpinBox" name="odom_f2m_validDepthRatio">
+                               <property name="suffix">
+                                <string/>
+                               </property>
+                               <property name="decimals">
+                                <number>2</number>
+                               </property>
+                               <property name="maximum">
+                                <double>1.000000000000000</double>
+                               </property>
+                               <property name="singleStep">
+                                <double>0.050000000000000</double>
+                               </property>
+                               <property name="value">
+                                <double>0.750000000000000</double>
+                               </property>
+                              </widget>
+                             </item>
+                             <item row="2" column="1">
+                              <widget class="QLabel" name="label_764">
                                <property name="text">
-                                <string>[Geometry] Max angle (degrees) used to filter points of a new added scan to local map (when Radius above is &gt;0). 0 means any angle.</string>
+                                <string>[Visual] Floor threshold. Only track features in 3D feature map that are over this threshold (height in base frame). Can be useful to ignore reflections on the floor. 0 means disabled.</string>
                                </property>
                                <property name="wordWrap">
                                 <bool>true</bool>
@@ -16166,32 +16194,49 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                                </property>
                               </widget>
                              </item>
-                             <item row="9" column="0">
-                              <widget class="QDoubleSpinBox" name="odom_f2m_gravitySigma">
-                               <property name="suffix">
-                                <string/>
-                               </property>
-                               <property name="decimals">
-                                <number>4</number>
+                             <item row="13" column="0">
+                              <widget class="QSpinBox" name="odom_f2m_bundleMaxKeyFramesPerFeature">
+                               <property name="maximum">
+                                <number>999999</number>
                                </property>
+                              </widget>
+                             </item>
+                             <item row="1" column="0">
+                              <widget class="QSpinBox" name="spinBox_odom_f2m_maxNewFeatures">
                                <property name="minimum">
-                                <double>-1.000000000000000</double>
+                                <number>0</number>
                                </property>
                                <property name="maximum">
-                                <double>10.000000000000000</double>
+                                <number>999999</number>
                                </property>
                                <property name="singleStep">
-                                <double>0.100000000000000</double>
+                                <number>1</number>
                                </property>
                                <property name="value">
-                                <double>-1.000000000000000</double>
+                                <number>0</number>
                                </property>
                               </widget>
                              </item>
-                             <item row="5" column="1">
-                              <widget class="QLabel" name="label_564">
+                             <item row="6" column="0">
+                              <widget class="QDoubleSpinBox" name="doubleSpinBox_odom_f2m_scanRange">
+                               <property name="suffix">
+                                <string> m</string>
+                               </property>
+                               <property name="decimals">
+                                <number>0</number>
+                               </property>
+                               <property name="maximum">
+                                <double>9999.000000000000000</double>
+                               </property>
+                               <property name="value">
+                                <double>0.000000000000000</double>
+                               </property>
+                              </widget>
+                             </item>
+                             <item row="14" column="1">
+                              <widget class="QLabel" name="label_763">
                                <property name="text">
-                                <string>[Geometry] Distance Range used to filter points of local map (when &gt; 0). 0 means local map is updated using time and not range.</string>
+                                <string>[Visual] Update 3D local feature map on every frames with bundle adjustment. Recommended if Vis/DepthAsMask=false and Mem/UseOdomFeatures=true so that features without depth are better triangulated on every frame (not only on keyframes). If disabled, the feature map is updated only when a new keyframe is added (legacy approach).</string>
                                </property>
                                <property name="wordWrap">
                                 <bool>true</bool>
@@ -16201,10 +16246,37 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                                </property>
                               </widget>
                              </item>
-                             <item row="11" column="1">
-                              <widget class="QLabel" name="label_762">
+                             <item row="9" column="0">
+                              <widget class="QComboBox" name="odom_f2m_bundleStrategy">
+                               <property name="sizeAdjustPolicy">
+                                <enum>QComboBox::AdjustToContents</enum>
+                               </property>
+                               <item>
+                                <property name="text">
+                                 <string>Disabled</string>
+                                </property>
+                               </item>
+                               <item>
+                                <property name="text">
+                                 <string>g2o</string>
+                                </property>
+                               </item>
+                               <item>
+                                <property name="text">
+                                 <string>cvsba</string>
+                                </property>
+                               </item>
+                               <item>
+                                <property name="text">
+                                 <string>Ceres</string>
+                                </property>
+                               </item>
+                              </widget>
+                             </item>
+                             <item row="3" column="1">
+                              <widget class="QLabel" name="label_195">
                                <property name="text">
-                                <string>[Visual] Maximum keyframes per feature for bundle adjustment. 0 means not limit.</string>
+                                <string>[Geometry] Maximum local scan map size (points).</string>
                                </property>
                                <property name="wordWrap">
                                 <bool>true</bool>
@@ -16214,10 +16286,25 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                                </property>
                               </widget>
                              </item>
-                             <item row="11" column="0">
-                              <widget class="QSpinBox" name="odom_f2m_bundleMaxKeyFramesPerFeature">
+                             <item row="8" column="0">
+                              <widget class="QDoubleSpinBox" name="odom_f2m_initDepthFactor">
+                               <property name="suffix">
+                                <string/>
+                               </property>
+                               <property name="decimals">
+                                <number>3</number>
+                               </property>
+                               <property name="minimum">
+                                <double>0.001000000000000</double>
+                               </property>
                                <property name="maximum">
-                                <number>999999</number>
+                                <double>99.000000000000000</double>
+                               </property>
+                               <property name="singleStep">
+                                <double>0.050000000000000</double>
+                               </property>
+                               <property name="value">
+                                <double>0.050000000000000</double>
                                </property>
                               </widget>
                              </item>


Reviews:
user: login: matlabbe
state: COMMENTED
author_association: MEMBER
submitted_at: 2025-03-06T18:57:23Z
user: login: matlabbe
state: COMMENTED
author_association: MEMBER
submitted_at: 2025-03-09T03:54:43Z

Review comments:
diff_hunk: @@ -818,6 +870,28 @@ Transform OdometryF2M::computeTransform(
 						if(maxNewFeatures_ == 0  || added < maxNewFeatures_)
 						{
 							int cameraIndex = iter->second.second.second.second.second;
+							cv::Point3f pt = iter->second.second.second.first;
+							if(!util3d::isFinite(pt))
+							{
+								// get the ray instead
+								float x = iter->second.second.first.pt.x; //subImageWidth should be already removed
+								float y = iter->second.second.first.pt.y;
+								Eigen::Vector3f ray = util3d::projectDepthTo3DRay(
+										lastFrameModels[cameraIndex].imageSize(),
+										x,
+										y,
+										lastFrameModels[cameraIndex].cx(),
+										lastFrameModels[cameraIndex].cy(),
+										lastFrameModels[cameraIndex].fx(),
+										lastFrameModels[cameraIndex].fy());
+								float scaleInf = 0.05 * lastFrameModels[cameraIndex].fx();
path: corelib/src/odometry/OdometryF2M.cpp
user: login: matlabbe
body: Doublecheck how much this affects the results. Maybe Euroc dataset didn't work well because of this change.
created_at: 2025-03-06T18:57:22Z
author_association: MEMBER
diff_hunk: @@ -818,6 +870,28 @@ Transform OdometryF2M::computeTransform(
 						if(maxNewFeatures_ == 0  || added < maxNewFeatures_)
 						{
 							int cameraIndex = iter->second.second.second.second.second;
+							cv::Point3f pt = iter->second.second.second.first;
+							if(!util3d::isFinite(pt))
+							{
+								// get the ray instead
+								float x = iter->second.second.first.pt.x; //subImageWidth should be already removed
+								float y = iter->second.second.first.pt.y;
+								Eigen::Vector3f ray = util3d::projectDepthTo3DRay(
+										lastFrameModels[cameraIndex].imageSize(),
+										x,
+										y,
+										lastFrameModels[cameraIndex].cx(),
+										lastFrameModels[cameraIndex].cy(),
+										lastFrameModels[cameraIndex].fx(),
+										lastFrameModels[cameraIndex].fy());
+								float scaleInf = 0.05 * lastFrameModels[cameraIndex].fx();
path: corelib/src/odometry/OdometryF2M.cpp
user: login: matlabbe
body: The dataset evaluation is using `Vis/DepthAsMask=true` so this is ignored anyway. I still added a parameter called `OdomF2M/InitDepthFactor` to tune 0.05 if needed. 
created_at: 2025-03-09T03:54:42Z
author_association: MEMBER

PR comments:
user: login: matlabbe
created_at: 2025-03-06T18:54:25Z
body: Euroc datasets are the more negatively impacted by `OdomF2M/BundleUpdateFeatureMapOnAllFrames=true`, we should keep it to false by default.
author_association: MEMBER
user: login: matlabbe
created_at: 2025-03-09T03:57:57Z
body: I tested again Euroc with live visualization and the results at 1 Hz are pretty close to original results. It seems that a not so accurate loop closure "sometimes" happens, which add significant error, though if I remove that link, the results are close to paper.

I'll still keep `OdomF2M/BundleUpdateFeatureMapOnAllFrames=false` by default till I have more real experiments with `OdomF2M/BundleUpdateFeatureMapOnAllFrames=true`.
author_association: MEMBER