OdomF2M: Scan gravity optimization

corelib/include/rtabmap/core/Parameters.h

@@ -529,6 +529,7 @@ class RTABMAP_CORE_EXPORT Parameters
     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());
+    RTABMAP_PARAM(OdomF2M, ScanGravity,         bool, false,  uFormat("[Geometry] With an IMU, keep the local scan map aligned with gravity: the scan keyframes of the local map are kept in a local graph, with their registration links and a gravity link from the IMU orientation at each of them (weighted by \"%s\"), optimized when a keyframe is added (the oldest one fixed). The map is then reassembled from the keyframes' clouds at their optimized poses, so roll and pitch don't drift. Requires g2o or GTSAM.", kOptimizerGravitySigma().c_str()));
     RTABMAP_PARAM(OdomF2M, ScanRange,           float, 0,     "[Geometry] Distance Range used to filter points of local map (when > 0). 0 means local map is updated using time and not range.");
     RTABMAP_PARAM(OdomF2M, ValidDepthRatio,     float, 0.75,  "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.");
 #if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM)

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

@@ -82,7 +82,22 @@ private:
 	Signature * map_;
 	Signature * lastFrame_;
 	int lastFrameOldestNewId_;
-	std::vector<std::pair<pcl::PointCloud<pcl::PointXYZINormal>::Ptr, pcl::IndicesPtr> > scansBuffer_;
+	// The scan keyframes the local scan map is assembled from
+	struct ScanKeyFrame
+	{
+		pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloud; // in odometry frame, at pose
+		pcl::IndicesPtr indices; // points kept in the map (not subtracted), all if empty
+		int id = 0;
+		Transform pose;          // odometry pose of the keyframe
+		Transform gravity;       // IMU orientation at the keyframe (null if not available)
+		Link link;               // registration from the previous keyframe (not set on the first)
+	};
+	std::vector<ScanKeyFrame> scansBuffer_;
+	pcl::PointCloud<pcl::PointXYZINormal>::Ptr assembleScanMap() const;
+	bool alignScanKeyFramesWithGravity();
+	bool scanGravity_;
+	Optimizer * scanOptimizer_;
+	int scanKeyFrameSeq_;
 
 	std::map<int, std::map<int, FeatureBA> > bundleWordReferences_; //<WordId, <FrameId, pt2D+depth>>
 	std::map<int, Transform> bundlePoses_;

corelib/src/odometry/OdometryF2M.cpp

@@ -79,6 +79,9 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
 	validDepthRatio_(Parameters::defaultOdomF2MValidDepthRatio()),
 	pointToPlaneK_(Parameters::defaultIcpPointToPlaneK()),
 	pointToPlaneRadius_(Parameters::defaultIcpPointToPlaneRadius()),
+	scanGravity_(Parameters::defaultOdomF2MScanGravity()),
+	scanOptimizer_(0),
+	scanKeyFrameSeq_(0),
 	map_(new Signature(-1)),
 	lastFrame_(new Signature(1)),
 	lastFrameOldestNewId_(0),
@@ -109,6 +112,31 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
 
 	Parameters::parse(parameters, Parameters::kIcpPointToPlaneK(), pointToPlaneK_);
 	Parameters::parse(parameters, Parameters::kIcpPointToPlaneRadius(), pointToPlaneRadius_);
+	Parameters::parse(parameters, Parameters::kOdomF2MScanGravity(), scanGravity_);
+	if(scanGravity_)
+	{
+		const Optimizer::Type type = Optimizer::isAvailable(Optimizer::kTypeG2O)?Optimizer::kTypeG2O:
+				Optimizer::isAvailable(Optimizer::kTypeGTSAM)?Optimizer::kTypeGTSAM:Optimizer::kTypeUndef;
+		if(type == Optimizer::kTypeUndef)
+		{
+			UWARN("\"%s\" is enabled but neither g2o nor GTSAM is available, the local scan map is "
+					"not aligned with gravity.", Parameters::kOdomF2MScanGravity().c_str());
+			scanGravity_ = false;
+		}
+		else
+		{
+			// Gravity links weighted by Optimizer/GravitySigma
+			scanOptimizer_ = Optimizer::create(type, parameters);
+			if(scanOptimizer_->gravitySigma() <= 0.0f)
+			{
+				UWARN("\"%s\" is enabled but \"%s\" is 0, the local scan map is not aligned with gravity.",
+						Parameters::kOdomF2MScanGravity().c_str(), Parameters::kOptimizerGravitySigma().c_str());
+				delete scanOptimizer_;
+				scanOptimizer_ = 0;
+				scanGravity_ = false;
+			}
+		}
+	}
 
 	UASSERT(bundleMaxFrames_ >= 0);
 	ParametersMap bundleParameters = parameters;
@@ -182,6 +210,7 @@ OdometryF2M::~OdometryF2M()
 	delete map_;
 	delete lastFrame_;
 	delete sba_;
+	delete scanOptimizer_;
 	delete regPipeline_;
 	UDEBUG("");
 }
@@ -196,6 +225,7 @@ void OdometryF2M::reset(const Transform & initialPose)
 	*lastFrame_ = Signature(1);
 	*map_ = Signature(-1);
 	scansBuffer_.clear();
+	scanKeyFrameSeq_ = 0;
 	bundleWordReferences_.clear();
 	bundlePoses_.clear();
 	bundleLinks_.clear();
@@ -205,6 +235,73 @@ void OdometryF2M::reset(const Transform & initialPose)
 	lastFrameOldestNewId_ = 0;
 }
 
+pcl::PointCloud<pcl::PointXYZINormal>::Ptr OdometryF2M::assembleScanMap() const
+{
+	// As when the map is trimmed: the oldest keyframe is added whole (what it overlapped
+	// is no longer in the map), the others only with the points they added.
+	pcl::PointCloud<pcl::PointXYZINormal>::Ptr map(new pcl::PointCloud<pcl::PointXYZINormal>);
+	for(const ScanKeyFrame & keyFrame : scansBuffer_)
+	{
+		if(&keyFrame != &scansBuffer_.front() && keyFrame.indices->size())
+		{
+			pcl::PointCloud<pcl::PointXYZINormal> tmp;
+			pcl::copyPointCloud(*keyFrame.cloud, *keyFrame.indices, tmp);
+			*map += tmp;
+		}
+		else
+		{
+			*map += *keyFrame.cloud;
+		}
+	}
+	return map;
+}
+
+bool OdometryF2M::alignScanKeyFramesWithGravity()
+{
+	// Local graph of the keyframes of the scan map: their registration links, and a
+	// gravity link at each one with an IMU orientation. The oldest one is fixed, so as the
+	// map moves on, its orientation converges to the IMU's instead of keeping the first
+	// keyframe's (and the drift since) forever.
+	if(!scanOptimizer_ || scansBuffer_.size() < 2)
+	{
+		return false;
+	}
+	std::map<int, Transform> poses;
+	std::multimap<int, Link> links;
+	int gravityLinks = 0;
+	for(const ScanKeyFrame & keyFrame : scansBuffer_)
+	{
+		poses.insert(std::make_pair(keyFrame.id, keyFrame.pose));
+		if(keyFrame.link.isValid() && poses.find(keyFrame.link.from()) != poses.end())
+		{
+			links.insert(std::make_pair(keyFrame.link.from(), keyFrame.link));
+		}
+		if(!keyFrame.gravity.isNull())
+		{
+			links.insert(std::make_pair(keyFrame.id, Link(keyFrame.id, keyFrame.id, Link::kGravity, keyFrame.gravity)));
+			++gravityLinks;
+		}
+	}
+	if(gravityLinks == 0 || links.size() < poses.size()-1+gravityLinks)
+	{
+		return false;
+	}
+	const std::map<int, Transform> optimized = scanOptimizer_->optimize(scansBuffer_.front().id, poses, links);
+	if(optimized.size() != poses.size())
+	{
+		UWARN("Optimization of the local scan map with gravity failed, it is kept as is.");
+		return false;
+	}
+	for(ScanKeyFrame & keyFrame : scansBuffer_)
+	{
+		const Transform & pose = optimized.at(keyFrame.id);
+		const Transform correction = pose * keyFrame.pose.inverse();
+		keyFrame.cloud = util3d::transformPointCloud(keyFrame.cloud, correction);
+		keyFrame.pose = pose;
+	}
+	return true;
+}
+
 // return not null transform if odometry is correctly computed
 Transform OdometryF2M::computeTransform(
 		SensorData & data,
@@ -231,6 +328,13 @@ Transform OdometryF2M::computeTransform(
 		}
 	}
 
+	// IMU orientation at the frame, for the gravity links of the local scan map
+	Transform scanImuT;
+	if(scanOptimizer_ && !imus().empty())
+	{
+		scanImuT = Transform::getTransform(imus(), data.stamp());
+	}
+
 	RegistrationInfo regInfo;
 	int nFeatures = 0;
 
@@ -1162,7 +1266,18 @@ Transform OdometryF2M::computeTransform(
 								copyPointCloud(*normals, *mapCloudNormals);
 
 							} else {
-								scansBuffer_.push_back(std::make_pair(frameCloudNormals, frameCloudNormalsIndices));
+								ScanKeyFrame keyFrame;
+								keyFrame.cloud = frameCloudNormals;
+								keyFrame.indices = frameCloudNormalsIndices;
+								keyFrame.id = ++scanKeyFrameSeq_;
+								keyFrame.pose = newFramePose;
+								keyFrame.gravity = scanImuT;
+								if(!scansBuffer_.empty())
+								{
+									keyFrame.link = Link(scansBuffer_.back().id, keyFrame.id, Link::kNeighbor,
+											scansBuffer_.back().pose.inverse()*newFramePose, regInfo.covariance.inv());
+								}
+								scansBuffer_.push_back(keyFrame);
 
 								//remove points if too big
 								UDEBUG("scansBuffer=%d, mapSize=%d newPoints=%d maxPoints=%d",
@@ -1180,31 +1295,31 @@ Transform OdometryF2M::computeTransform(
 									int i = int(scansBuffer_.size())-1;
 									for(; i>=0; --i)
 									{
-										int pointsToAdd = scansBuffer_[i].second->size()?scansBuffer_[i].second->size():scansBuffer_[i].first->size();
+										int pointsToAdd = scansBuffer_[i].indices->size()?scansBuffer_[i].indices->size():scansBuffer_[i].cloud->size();
 										if((int)mapCloudNormals->size() + pointsToAdd > scanMaximumMapSize_ ||
 										   i == 0)
 										{
-											*mapCloudNormals += *scansBuffer_[i].first;
+											*mapCloudNormals += *scansBuffer_[i].cloud;
 											break;
 										}
 										else
 										{
-											if(scansBuffer_[i].second->size())
+											if(scansBuffer_[i].indices->size())
 											{
 												pcl::PointCloud<pcl::PointXYZINormal> tmp;
-												pcl::copyPointCloud(*scansBuffer_[i].first, *scansBuffer_[i].second, tmp);
+												pcl::copyPointCloud(*scansBuffer_[i].cloud, *scansBuffer_[i].indices, tmp);
 												*mapCloudNormals += tmp;
 											}
 											else
 											{
-												*mapCloudNormals += *scansBuffer_[i].first;
+												*mapCloudNormals += *scansBuffer_[i].cloud;
 											}
 										}
 									}
 									// remove old clouds
 									if(i > 0)
 									{
-										std::vector<std::pair<pcl::PointCloud<pcl::PointXYZINormal>::Ptr, pcl::IndicesPtr> > scansTmp(scansBuffer_.size()-i);
+										std::vector<ScanKeyFrame> scansTmp(scansBuffer_.size()-i);
 										int oi = 0;
 										for(; i<(int)scansBuffer_.size(); ++i)
 										{
@@ -1217,19 +1332,28 @@ Transform OdometryF2M::computeTransform(
 								else
 								{
 									// just append the last cloud
-									if(scansBuffer_.back().second->size())
+									if(scansBuffer_.back().indices->size())
 									{
 										pcl::PointCloud<pcl::PointXYZINormal> tmp;
-										pcl::copyPointCloud(*scansBuffer_.back().first, *scansBuffer_.back().second, tmp);
+										pcl::copyPointCloud(*scansBuffer_.back().cloud, *scansBuffer_.back().indices, tmp);
 										*mapCloudNormals += tmp;
 									}
 									else
 									{
-										*mapCloudNormals += *scansBuffer_.back().first;
+										*mapCloudNormals += *scansBuffer_.back().cloud;
 									}
 								}
 							}
 
+							if(scanMapMaxRange_ <= 0 && alignScanKeyFramesWithGravity())
+							{
+								// The keyframes moved: the map is reassembled from them, and this
+								// frame (the newest keyframe) follows its optimized pose.
+								mapCloudNormals = assembleScanMap();
+								newFramePose = scansBuffer_.back().pose;
+								output = this->getPose().inverse() * newFramePose;
+							}
+
 							if(mapScan.is2d())
 							{
 								Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(),0,0,0,0);
@@ -1493,7 +1617,13 @@ Transform OdometryF2M::computeTransform(
 						if (scanMapMaxRange_ > 0 ){
 							UINFO("Local map will be updated using range instead of time with range threshold set at %f", scanMapMaxRange_);
 						} else {
-							scansBuffer_.push_back(std::make_pair(mapCloudNormals, pcl::IndicesPtr(new std::vector<int>)));
+							ScanKeyFrame keyFrame;
+							keyFrame.cloud = mapCloudNormals;
+							keyFrame.indices = pcl::IndicesPtr(new std::vector<int>);
+							keyFrame.id = ++scanKeyFrameSeq_;
+							keyFrame.pose = newFramePose;
+							keyFrame.gravity = scanImuT;
+							scansBuffer_.push_back(keyFrame);
 						}
 						if(lastFrame_->sensorData().laserScanRaw().is2d())
 						{

corelib/test/test_odometry.cpp

@@ -12,6 +12,7 @@
 #include <opencv2/imgcodecs.hpp>
 #include <memory>
 #include <rtabmap/core/IMU.h>
+#include <rtabmap/core/Optimizer.h>
 #include <rtabmap/utilite/UStl.h>
 #include <cmath>
 #include <limits>
@@ -793,3 +794,56 @@ TEST(OdometryTest, InitialOrientationIsTheImuOrientationAtTheFirstFrame)
 	}
 	EXPECT_NEAR(given->getPose().theta(), 1.0, 1e-6);
 }
+
+TEST(OdometryTest, ScanMapRealignsWithGravity)
+{
+	// Odometry starts with a roll 5 degrees off (an initial pose given with a rotation is
+	// not overridden by the IMU), while the IMU says the sensor is level. Without gravity in
+	// the local scan map, the error stays; with it, the map's keyframes are pulled toward the
+	// IMU's gravity and the odometry's roll converges.
+	if(!Optimizer::isAvailable(Optimizer::kTypeG2O) && !Optimizer::isAvailable(Optimizer::kTypeGTSAM))
+	{
+		GTEST_SKIP() << "Requires g2o or GTSAM";
+	}
+	Trajectory moving;
+	moving.yaw = 0.3;
+	moving.acceleration = 1.0;
+	const double initialRoll = 5.0 * M_PI / 180.0;
+	auto finalRoll = [&](bool scanGravity)
+	{
+		ParametersMap parameters;
+		parameters.insert(ParametersPair(Parameters::kRegStrategy(), "1"));
+		parameters.insert(ParametersPair(Parameters::kIcpVoxelSize(), "0.1"));
+		parameters.insert(ParametersPair(Parameters::kIcpPointToPlane(), "true"));
+		parameters.insert(ParametersPair(Parameters::kIcpPointToPlaneK(), "10"));
+		parameters.insert(ParametersPair(Parameters::kIcpMaxTranslation(), "0"));
+		parameters.insert(ParametersPair(Parameters::kIcpMaxCorrespondenceDistance(), "0.5"));
+		parameters.insert(ParametersPair(Parameters::kOdomScanKeyFrameThr(), "0.95"));
+		parameters.insert(ParametersPair(Parameters::kOdomF2MScanGravity(), scanGravity?"true":"false"));
+		// Gravity links with Optimizer/GravitySigma's default (0.3 rad)
+		std::unique_ptr<Odometry> odometry(Odometry::create(parameters));
+		odometry->reset(Transform(0, 0, 0, float(initialRoll), 0, float(moving.yaw)));
+		double imuStamp = 0.0;
+		Transform pose;
+		for(int i=1; i<=30; ++i)
+		{
+			const double stamp = 0.1*i;
+			for(; imuStamp <= stamp + kSweep + 0.005; imuStamp += 0.005)
+			{
+				SensorData imu(makeImu(moving, imuStamp), 0, imuStamp);
+				odometry->process(imu);
+			}
+			SensorData data(makeSweep(moving, stamp), cv::Mat(), cv::Mat(), CameraModel(), i, stamp);
+			OdometryInfo info;
+			pose = odometry->process(data, &info);
+			EXPECT_FALSE(pose.isNull()) << "frame " << i << " not registered (scan gravity " << scanGravity << ")";
+		}
+		float roll, pitch, yaw;
+		pose.getEulerAngles(roll, pitch, yaw);
+		return std::fabs(roll);
+	};
+	const double withoutGravity = finalRoll(false);
+	const double withGravity = finalRoll(true);
+	EXPECT_NEAR(withoutGravity, initialRoll, 0.01) << "without gravity, the initial error should stay";
+	EXPECT_LT(withGravity, initialRoll * 0.3) << "with gravity, the roll should converge to the IMU's";
+}

guilib/src/PreferencesDialog.cpp

@@ -1517,6 +1517,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_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_scanGravity->setObjectName(Parameters::kOdomF2MScanGravity().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());

guilib/src/ui/preferencesDialog.ui

@@ -16952,7 +16952,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo
                                </property>
                               </widget>
                              </item>
-                             <item row="9" column="1">
+                             <item row="10" 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>
@@ -16965,7 +16965,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                                </property>
                               </widget>
                              </item>
-                             <item row="10" column="1">
+                             <item row="11" 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>
@@ -16994,7 +16994,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                                </property>
                               </widget>
                              </item>
-                             <item row="13" column="1">
+                             <item row="14" column="1">
                               <widget class="QLabel" name="label_762">
                                <property name="text">
                                 <string>[Visual] Maximum keyframes per feature for bundle adjustment. 0 means not limit.</string>
@@ -17007,14 +17007,14 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                                </property>
                               </widget>
                              </item>
-                             <item row="14" column="0">
+                             <item row="15" column="0">
                               <widget class="QCheckBox" name="odom_f2m_bundleUpdateFeatureMapOnAllFrames">
                                <property name="text">
                                 <string/>
                                </property>
                               </widget>
                              </item>
-                             <item row="12" column="1">
+                             <item row="13" 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>
@@ -17076,7 +17076,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                                </property>
                               </widget>
                              </item>
-                             <item row="11" column="0">
+                             <item row="12" column="0">
                               <widget class="QDoubleSpinBox" name="odom_f2m_gravitySigma">
                                <property name="suffix">
                                 <string/>
@@ -17098,14 +17098,14 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                                </property>
                               </widget>
                              </item>
-                             <item row="10" column="0">
+                             <item row="11" column="0">
                               <widget class="QSpinBox" name="odom_f2m_bundleMaxFrames">
                                <property name="maximum">
                                 <number>999999</number>
                                </property>
                               </widget>
                              </item>
-                             <item row="12" column="0">
+                             <item row="13" column="0">
                               <widget class="QDoubleSpinBox" name="odom_f2m_bundleMinMotion">
                                <property name="suffix">
                                 <string> pixels</string>
@@ -17143,7 +17143,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                                </property>
                               </widget>
                              </item>
-                             <item row="11" column="1">
+                             <item row="12" 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>
@@ -17156,7 +17156,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                                </property>
                               </widget>
                              </item>
-                             <item row="8" column="1">
+                             <item row="9" 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>
@@ -17169,7 +17169,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                                </property>
                               </widget>
                              </item>
-                             <item row="7" column="1">
+                             <item row="8" 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>
@@ -17208,7 +17208,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                                </property>
                               </widget>
                              </item>
-                             <item row="7" column="0">
+                             <item row="8" column="0">
                               <widget class="QDoubleSpinBox" name="odom_f2m_validDepthRatio">
                                <property name="suffix">
                                 <string/>
@@ -17240,7 +17240,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                                </property>
                               </widget>
                              </item>
-                             <item row="13" column="0">
+                             <item row="14" column="0">
                               <widget class="QSpinBox" name="odom_f2m_bundleMaxKeyFramesPerFeature">
                                <property name="maximum">
                                 <number>999999</number>
@@ -17279,7 +17279,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                                </property>
                               </widget>
                              </item>
-                             <item row="14" column="1">
+                             <item row="15" column="1">
                               <widget class="QLabel" name="label_7631">
                                <property name="text">
                                 <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>
@@ -17292,7 +17292,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                                </property>
                               </widget>
                              </item>
-                             <item row="9" column="0">
+                             <item row="10" column="0">
                               <widget class="QComboBox" name="odom_f2m_bundleStrategy">
                                <property name="sizeAdjustPolicy">
                                 <enum>QComboBox::AdjustToContents</enum>
@@ -17332,7 +17332,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                                </property>
                               </widget>
                              </item>
-                             <item row="8" column="0">
+                             <item row="9" column="0">
                               <widget class="QDoubleSpinBox" name="odom_f2m_initDepthFactor">
                                <property name="suffix">
                                 <string/>
@@ -17354,6 +17354,26 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                                </property>
                               </widget>
                              </item>
+                             <item row="7" column="0">
+                              <widget class="QCheckBox" name="odom_f2m_scanGravity">
+                               <property name="text">
+                                <string/>
+                               </property>
+                              </widget>
+                             </item>
+                             <item row="7" column="1">
+                              <widget class="QLabel" name="label_odom_f2m_scanGravity">
+                               <property name="text">
+                                <string>[Geometry] With an IMU, keep the local scan map aligned with gravity: the scan keyframes of the local map are kept in a local graph, with their registration links and a gravity link from the IMU orientation at each of them (weighted by the optimizer's gravity sigma), optimized when a keyframe is added. The map is then reassembled from the keyframes at their optimized poses, so roll and pitch don't drift. Requires g2o or GTSAM.</string>
+                               </property>
+                               <property name="wordWrap">
+                                <bool>true</bool>
+                               </property>
+                               <property name="textInteractionFlags">
+                                <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                               </property>
+                              </widget>
+                             </item>
                             </layout>
                            </item>
                           </layout>