Imu motion predictor (#1791)

* Imu motion predictor

* simplified parameters

* refactored odom's imu init orientation

* fixing sign issue with x86-64-v3

* fixing M_PI not defined in tests

* Added auto gravity estimation mode

* Fixing crash on windows when uFormat() is to long

corelib/include/rtabmap/core/ImuMotionPredictor.h

@@ -0,0 +1,233 @@
+/*
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef RTABMAP_CORE_IMUMOTIONPREDICTOR_H_
+#define RTABMAP_CORE_IMUMOTIONPREDICTOR_H_
+
+#include <rtabmap/core/rtabmap_core_export.h>
+#include <rtabmap/core/Transform.h>
+#include <rtabmap/core/IMU.h>
+
+#include <Eigen/Geometry>
+
+#include <map>
+#include <vector>
+
+namespace rtabmap {
+
+/**
+ * @brief Predicts the pose of the base frame from the last odometry pose and the IMU.
+ *
+ * Between two odometry updates, the pose is propagated with the IMU: the orientation is
+ * the IMU's own, re-expressed in the odometry frame, and the position is integrated from
+ * the velocity at the last odometry update and the gravity-compensated acceleration. That
+ * velocity is re-estimated at every odometry update, from the displacement since an
+ * odometry pose a short window back (see velocityWindow) corrected by the acceleration
+ * measured in between, so the position never drifts for long: only the motion since the
+ * last update is predicted.
+ *
+ * This is what lidar deskewing needs: the pose at every point's time, during a sweep that
+ * started after the last pose odometry estimated.
+ *
+ * The IMU samples are given as they are measured (see addImu()):
+ * the orientation of the IMU in its world frame and its specific force, from which gravity
+ * is removed here. That world frame must be gravity aligned with +z up, as in ROS (REP-103,
+ * e.g. ENU); its yaw doesn't matter. An orientation given in a frame with z down (NED)
+ * must be converted first, otherwise gravity is added instead of removed. The lever arm between the IMU and the base origin is
+ * ignored: its centripetal and tangential accelerations are small over the fraction of a
+ * second this predicts.
+ *
+ * The gravity removed can be estimated from the IMU itself (see the constructor): an
+ * accelerometer can read a few percent off at rest (scale or bias), and that error,
+ * integrated as an acceleration, would bias the predicted velocity along gravity. The
+ * accelerometer is averaged over the windows where it is quiet: with a gyro and an
+ * accelerometer that barely change, the IMU is not accelerating (at rest or at constant
+ * velocity), so it only measures gravity. The IMU must then be held still (or at constant
+ * velocity) for a moment, e.g., before moving: until then, standard gravity is used, and
+ * a steady acceleration without rotation would be taken for gravity. Only the error along
+ * gravity is corrected, which is all of it for an IMU that stays about level; one changing
+ * attitude would need its axes calibrated.
+ *
+ * Not thread-safe: a caller sharing it between threads must lock around every call.
+ */
+class RTABMAP_CORE_EXPORT ImuMotionPredictor
+{
+public:
+	/**
+	 * The IMU acceleration is used only with a velocity window (> 0): over a single frame,
+	 * the velocity is too noisy to be carried forward with it. Without it (window of 0, or
+	 * no acceleration in the IMU samples), the position follows the last velocity
+	 * (constant velocity model).
+	 *
+	 * @param maxPoseInterval odometry poses older (s) than this are not used to estimate
+	 *                        the velocity, which is null without one
+	 * @param velocityWindow  the velocity is estimated from the displacement since the
+	 *                        newest pose at least this old (s), corrected by the
+	 *                        acceleration measured since, so that it is the velocity at the
+	 *                        last pose, not an average. Over a single frame interval, the
+	 *                        noise of the odometry poses would be of the order of the
+	 *                        velocity itself. 0: the displacement since the previous pose,
+	 *                        without acceleration.
+	 * @param gravity         magnitude (m/s^2) of the gravity removed from the specific force
+	 *                        given to addImu(), standard gravity by default. <= 0: estimated
+	 *                        from the IMU when it is still (see the class description), with
+	 *                        standard gravity until then.
+	 *
+	 * These are fixed for the life of the predictor (there is no setter): changing them
+	 * while it estimates would mix poses and samples taken under different settings.
+	 */
+	explicit ImuMotionPredictor(
+			double maxPoseInterval = 1.0,
+			double velocityWindow = 0.5,
+			double gravity = 9.80665);
+
+	double maxPoseInterval() const {return maxPoseInterval_;}
+	double velocityWindow() const {return velocityWindow_;}
+	/// Gravity (m/s^2) removed from the specific force: the one given, or the estimate
+	double gravity() const {return gravity_;}
+	/// Whether the gravity is estimated from the IMU (see the constructor)
+	bool isGravityEstimated() const {return gravityEstimated_;}
+	/// Number of quiet IMU windows the gravity estimate is from (0: not estimated yet)
+	size_t gravityWindows() const {return gravityWindowsCount_;}
+
+	/**
+	 * @brief Adds an IMU measurement.
+	 * @param stamp time of the measurement (s)
+	 * @param imu   orientation of the IMU in its world frame (gravity aligned, +z up), linear
+	 *              acceleration as measured (the specific force, which includes the
+	 *              reaction to gravity) and the transform from the base frame to the IMU.
+	 *              Without orientation, the measurement is ignored; without linear
+	 *              acceleration (all zeros, or a covariance of -1), only its orientation
+	 *              is used.
+	 */
+	void addImu(double stamp, const IMU & imu);
+
+	/**
+	 * @brief Adds an odometry pose, from which the next poses are predicted.
+	 * @param stamp time of the pose (s)
+	 * @param pose  pose of the base frame in the odometry frame; a null pose (odometry
+	 *              lost) resets the prediction until the next valid pose
+	 */
+	void addPose(double stamp, const rtabmap::Transform & pose);
+
+	/// Forgets the poses and the IMU samples. The gravity estimate is kept: it is the
+	/// sensor's.
+	void reset();
+
+	/**
+	 * @brief Predicts the pose of the base frame in the odometry frame.
+	 * @param stamp time (s) of the prediction, normally after the last pose
+	 * @return the predicted pose, null if there is no IMU sample yet. Without a pose (none
+	 *         yet, or the last one was null), the orientation alone is predicted, in the
+	 *         IMU's world frame and at the origin: still the relative rotation between two
+	 *         stamps, which is what deskewing needs most.
+	 */
+	rtabmap::Transform predict(double stamp) const;
+
+	/// Whether there is a pose to predict from (none yet, or the last one was null).
+	bool hasPose() const;
+	/// Stamp of the last pose added, 0 if there is none.
+	double lastPoseStamp() const;
+	/// Velocity (m/s) of the base in the odometry frame at the last pose.
+	Eigen::Vector3d velocity() const;
+	/// Number of IMU samples kept.
+	size_t samples() const;
+
+private:
+	// orientation: of the base frame in the IMU's world frame; acceleration: of the base in
+	// that frame, gravity removed
+	// specificForce: in that frame too, including the reaction to gravity (see
+	// hasAcceleration)
+	void addSample(double stamp, const Eigen::Quaterniond & orientation,
+			const Eigen::Vector3d & specificForce, bool hasAcceleration);
+	// Gravity estimation, from the IMU's own specific force and angular velocity norm
+	void estimateGravity(double stamp, const Eigen::Vector3d & specificForce, double angularVelocity);
+
+	struct Sample
+	{
+		Eigen::Quaterniond orientation;
+		// Kept with gravity, which is removed when used: the estimate can change
+		Eigen::Vector3d specificForce;
+		bool hasAcceleration;
+	};
+	// Acceleration of a sample, gravity removed (null without acceleration)
+	Eigen::Vector3d accelerationOf(const Sample & sample) const;
+	Eigen::Quaterniond orientationAt(double stamp) const;
+	Eigen::Vector3d accelerationAt(double stamp) const;
+	void integrate(double from, double to, const Eigen::Quaterniond & rotation,
+			Eigen::Vector3d & velocity, Eigen::Vector3d & position) const;
+	void updateIntegration() const;
+
+	// The acceleration integrated from the last pose up to a sample's stamp, in the
+	// odometry frame: predicting a stamp then only integrates from the sample before it.
+	struct Integrated
+	{
+		Eigen::Vector3d acceleration; // at that stamp
+		Eigen::Vector3d velocity;     // change since the last pose
+		Eigen::Vector3d position;     // change since the last pose (without its velocity)
+	};
+
+private:
+	double maxPoseInterval_;
+	double velocityWindow_;
+	double gravity_;
+	bool gravityEstimated_;
+	std::map<double, Sample> samples_;
+
+	// Gravity estimation: the samples of the current window, and the gravity measured over
+	// the last quiet windows
+	struct StillSample
+	{
+		double stamp;
+		Eigen::Vector3d specificForce; // in the IMU frame
+		double angularVelocity;        // norm
+	};
+	std::vector<StillSample> stillWindow_;
+	std::vector<double> gravityWindows_;
+	size_t gravityWindowsCount_;
+	// Recent odometry poses, to estimate the velocity from (see velocityWindow)
+	std::map<double, rtabmap::Transform> poses_;
+
+	// The last odometry pose and the state predictions start from.
+	double poseStamp_;
+	rtabmap::Transform pose_;
+	Eigen::Vector3d velocity_;
+	// Rotation from the IMU's world frame to the odometry frame, at the last pose: the two
+	// are both gravity aligned, but their yaw differ.
+	Eigen::Quaterniond worldToOdom_;
+
+	// Built lazily by predict(), from the last pose to the newest sample; cleared when the
+	// pose or the samples it was built from change.
+	mutable std::map<double, Integrated> integrated_;
+	// Whether the acceleration at the last pose's stamp (the first entry) is final: it is
+	// held constant from the newest sample until a sample after that stamp is received.
+	mutable bool integratedStartIsFinal_;
+};
+
+}
+
+#endif /* RTABMAP_CORE_IMUMOTIONPREDICTOR_H_ */

corelib/include/rtabmap/core/Odometry.h

@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/Transform.h>
 #include <rtabmap/core/SensorData.h>
 #include <rtabmap/core/Parameters.h>
+#include <rtabmap/core/ImuMotionPredictor.h>
 
 namespace rtabmap {
 
@@ -189,6 +190,7 @@ private:
 	std::vector<StereoCameraModel> stereoModels_;
 	std::vector<CameraModel> models_;
 	std::map<double, Transform> imus_;
+	ImuMotionPredictor imuMotionPredictor_; // fed when IMU is received (motion guess and deskewing)
 };
 
 } /* namespace rtabmap */

corelib/include/rtabmap/core/Parameters.h

@@ -489,7 +489,7 @@ class RTABMAP_CORE_EXPORT Parameters
 
     RTABMAP_PARAM(Optimizer, Baseline,           double, 0.075,   "When doing bundle adjustment with RGB-D data (mono camera + depth), set a fake baseline (m) so the BA backend treats depth as stereo disparity. Applies to all BA-capable backends (g2o, GTSAM, Ceres). Set to 0 to keep the problem mono (depth observations are ignored). For real stereo data the baseline in the calibration (Tx) is used directly.");
     RTABMAP_PARAM(Optimizer, PixelVariance,     double, 1.0,     "Pixel variance used on the u/v axes of every bundle adjustment reprojection edge. Applies to all BA-capable backends (g2o, GTSAM, Ceres). Should approximate the squared 1-sigma keypoint localization error in pixels. Set higher (e.g. 4-9) if features are noisy (low texture, motion blur, low light, or large detector scale). Set lower (e.g. 0.01-0.1) if features are sub-pixel refined (Lucas-Kanade tracking, parabolic peak interpolation). Intuition: the lower the pixel variance, the more the optimizer trusts the keypoint positions.");
-    RTABMAP_PARAM(Optimizer, DisparityVariance, double, 1.0,     "Disparity variance used on the disparity axis (u - u_right) of stereo / RGB-D bundle adjustment edges. Applies to all BA-capable backends (g2o, GTSAM, Ceres). Defaults to the same value as PixelVariance for backward compatibility. Set higher (e.g. 2-4) if your depth source is noisier than your feature detector's u/v precision (typical for stereo block matchers / SGM at long range). Set lower (e.g. 0.01-0.1) if your depth source is more accurate than the u/v detector (typical for ToF / LiDAR-fused depth where range is measured directly rather than triangulated). Intuition: the lower the disparity variance, the more the optimizer trusts the depth measurements. Geometric note: wider baseline and/or higher image resolution improve a block matcher's effective disparity precision (larger disparity magnitudes and finer sub-pixel refinement), so wide-baseline high-resolution stereo pairs can usually afford a lower disparity variance (e.g. 0.1-0.5); narrow-baseline low-resolution pairs should keep it higher (e.g. 1-4).");
+    RTABMAP_PARAM(Optimizer, DisparityVariance, double, 1.0,     "Disparity variance used on the disparity axis (u - u_right) of stereo / RGB-D bundle adjustment edges (g2o, GTSAM, Ceres). Defaults to the same value as PixelVariance for backward compatibility. The lower it is, the more the optimizer trusts the depth. Set higher (e.g. 2-4) if the depth is noisier than the features' u/v precision (stereo block matchers / SGM at long range, narrow-baseline low-resolution stereo). Set lower (e.g. 0.01-0.1) if it is more accurate (ToF / LiDAR-fused depth, measured rather than triangulated), or around 0.1-0.5 for wide-baseline high-resolution stereo.");
     RTABMAP_PARAM(Optimizer, RobustKernelDelta, double, 8,       "Robust kernel delta used for bundle adjustment (0 means don't use robust kernel). Applies to all BA-capable backends (g2o, GTSAM, Ceres). Observations with chi2 over this threshold will be ignored in the second optimization pass.");
 
     RTABMAP_PARAM(GTSAM, Optimizer,       int, 1,          "0=Levenberg 1=GaussNewton 2=Dogleg");
@@ -512,13 +512,14 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(Odom, KalmanProcessNoise,     float, 0.001, "Process noise covariance value.");
     RTABMAP_PARAM(Odom, KalmanMeasurementNoise, float, 0.01,  "Process measurement covariance value.");
     RTABMAP_PARAM(Odom, GuessMotion,            bool, true,   "Guess next transformation from the last motion computed.");
-    RTABMAP_PARAM(Odom, GuessSmoothingDelay,    float, 0,     uFormat("Guess smoothing delay (s). Estimated velocity is averaged based on last transforms up to this maximum delay. This can help to get smoother velocity prediction. Last velocity computed is used directly if \"%s\" is set or the delay is below the odometry rate.", kOdomFilteringStrategy().c_str()));
+    RTABMAP_PARAM(Odom, ImuGravity,             float, 9.80665, uFormat("Gravity magnitude (m/s^2) removed from the IMU linear acceleration (used with \"%s\" > 0). Standard gravity by default. Set it to what the accelerometer reads at rest to compensate for its scale error, or for another gravity than Earth's. 0: estimated from the IMU while it is still (gyro and accelerometer quiet over 0.5 s), with standard gravity until then: the robot should then be perfectly still for a moment (e.g., at start), and a steady acceleration without rotation would be taken for gravity.", kOdomGuessSmoothingDelay().c_str()));
+    RTABMAP_PARAM(Odom, GuessSmoothingDelay,    float, 0,     uFormat("Guess smoothing delay (s). The velocity is averaged over the last transforms up to this delay, for a smoother velocity prediction. The last velocity is used directly if \"%s\" is set or the delay is below the odometry rate. With an IMU giving orientation and linear acceleration, a delay > 0 also enables the IMU acceleration: the velocity is the displacement over this delay corrected by the acceleration measured since, so it is not delayed, and the motion guess and lidar deskewing (\"%s\") integrate the acceleration from it (~0.5 s recommended). With 0, the IMU only gives the orientation. Without IMU, the average is delayed by half this delay: useful for platforms with inertia, high frame rates, or lidar deskewing, where the velocity noise would feed back into the next poses.", kOdomFilteringStrategy().c_str(), kOdomDeskewing().c_str()));
     RTABMAP_PARAM(Odom, KeyFrameThr,            float, 0.3,   "[Visual] Create a new keyframe when the number of inliers drops under this ratio of features in last frame. Setting the value to 0 means that a keyframe is created for each processed frame.");
     RTABMAP_PARAM(Odom, VisKeyFrameThr,         int, 150,     "[Visual] Create a new keyframe when the number of inliers drops under this threshold. Setting the value to 0 means that a keyframe is created for each processed frame.");
     RTABMAP_PARAM(Odom, ScanKeyFrameThr,        float, 0.9,   "[Geometry] Create a new keyframe when the number of ICP inliers drops under this ratio of points in last frame's scan. Setting the value to 0 means that a keyframe is created for each processed frame.");
     RTABMAP_PARAM(Odom, ImageDecimation,     unsigned int, 1, uFormat("Decimation of the RGB image before registration. If depth size is larger than decimated RGB size, depth is decimated to be always at most equal to RGB size. If %s is true and if depth is smaller than decimated RGB, depth may be interpolated to match RGB size for feature detection.", kVisDepthAsMask().c_str()));
     RTABMAP_PARAM(Odom, AlignWithGround,        bool, false,  "Align odometry with the ground on initialization.");
-    RTABMAP_PARAM(Odom, Deskewing,              bool, true,   "Lidar deskewing. If input lidar has time channel, it will be deskewed with a constant motion model (with IMU orientation and/or guess if provided).");
+    RTABMAP_PARAM(Odom, Deskewing,              bool, true,   uFormat("Lidar deskewing. If input lidar has time channel, it will be deskewed. With an IMU, the pose of every point is predicted from the previous frame: orientation from the IMU, translation from the velocity (with the IMU acceleration if \"%s\" > 0). Without IMU, with a constant motion model (or the guess if provided).", kOdomGuessSmoothingDelay().c_str()));
 
     // 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.");

corelib/include/rtabmap/core/util3d.h

@@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef UTIL3D_H_
 #define UTIL3D_H_
 
+#include <functional>
 #include "rtabmap/core/rtabmap_core_export.h"
 
 #include <pcl/point_cloud.h>
@@ -1295,6 +1296,27 @@ LaserScan RTABMAP_CORE_EXPORT deskew(
 		double inputStamp,
 		const rtabmap::Transform & velocity);
 
+/**
+ * @brief Deskews a lidar scan with a motion given by the caller.
+ *
+ * Same as the velocity overload, but the motion during the sweep comes from @p motion,
+ * for instance a pose predicted from an IMU.
+ *
+ * @param input      scan with a time channel (`kXYZIT` or `kXYZIRT`)
+ * @param inputStamp stamp of the scan, which the time channel is relative to
+ * @param motion     for a stamp (s) in the sweep, the pose of the scan's base frame at that
+ *                   stamp relative to the base frame at @p inputStamp; a null transform
+ *                   aborts deskewing
+ * @param slerp      call @p motion only for the first and last points and interpolate in
+ *                   between, instead of calling it for every time of the sweep
+ * @return the deskewed scan, empty on error
+ */
+LaserScan RTABMAP_CORE_EXPORT deskew(
+		const LaserScan & input,
+		double inputStamp,
+		const std::function<rtabmap::Transform(double stamp)> & motion,
+		bool slerp = false);
+
 } // namespace util3d
 } // namespace rtabmap