OdomF2M: Scan gravity optimization

CMakeLists.txt

@@ -22,7 +22,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 24)
-SET(RTABMAP_PATCH_VERSION 0)
+SET(RTABMAP_PATCH_VERSION 1)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
 

corelib/include/rtabmap/core/ImuMotionPredictor.h

@@ -0,0 +1,191 @@
+/*
+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>
+
+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.
+ *
+ * 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
+	 *
+	 * 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_;}
+	double gravity() const {return gravity_;}
+
+	/**
+	 * @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.
+	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
+	void addSample(double stamp, const Eigen::Quaterniond & orientation, const Eigen::Vector3d & acceleration);
+
+	struct Sample
+	{
+		Eigen::Quaterniond orientation;
+		Eigen::Vector3d acceleration;
+	};
+	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_;
+	std::map<double, Sample> samples_;
+	// 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

@@ -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.", kOdomGuessSmoothingDelay().c_str()));
+    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. With an IMU giving orientation and linear acceleration, a delay > 0 also enables the IMU acceleration: the velocity is then the displacement over this delay corrected by the acceleration measured since, so that it is the velocity at the last frame rather than a delayed average, and the motion guess and lidar deskewing (\"%s\") integrate the acceleration from it. Recommended (~0.5 s) with an IMU. With 0, the IMU only gives the orientation. Without IMU, the average is delayed by half this delay: it can make sense for platforms with inertia (e.g., cars at high speed, where one bad frame would otherwise change the predicted velocity a lot), for high frame rates, or when the velocity is used for lidar deskewing, where its 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.");
@@ -528,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/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
 

corelib/src/CMakeLists.txt

@@ -88,6 +88,7 @@ SET(SRC_FILES
     RegistrationVis.cpp
     
     Odometry.cpp
+    ImuMotionPredictor.cpp
     OdometryThread.cpp
 	OdometryInfo.cpp
     odometry/OdometryF2M.cpp

corelib/src/ImuMotionPredictor.cpp

@@ -0,0 +1,394 @@
+/*
+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.
+*/
+
+#include <rtabmap/core/ImuMotionPredictor.h>
+
+#include <vector>
+
+namespace rtabmap {
+
+// Samples older than this (s) behind the newest one are dropped, so the buffer stays
+// bounded while no odometry pose comes to trim it.
+static const double kMaxBufferDuration = 10.0;
+
+ImuMotionPredictor::ImuMotionPredictor(double maxPoseInterval, double velocityWindow, double gravity) :
+	maxPoseInterval_(maxPoseInterval),
+	velocityWindow_(velocityWindow),
+	gravity_(gravity),
+	integratedStartIsFinal_(false),
+	poseStamp_(0.0),
+	velocity_(Eigen::Vector3d::Zero()),
+	worldToOdom_(Eigen::Quaterniond::Identity())
+{
+}
+
+void ImuMotionPredictor::addImu(double stamp, const IMU & imu)
+{
+	const cv::Vec4d & o = imu.orientation();
+	const Eigen::Quaterniond imuOrientation(o[3], o[0], o[1], o[2]);
+	if(imu.empty() ||
+	   imuOrientation.norm() < 0.5 ||
+	   (!imu.orientationCovariance().empty() && imu.orientationCovariance().at<double>(0,0) == -1.0))
+	{
+		// No orientation
+		return;
+	}
+	const Eigen::Quaterniond worldToImu = imuOrientation.normalized();
+	const Eigen::Quaterniond baseToImu = imu.localTransform().isNull()?
+			Eigen::Quaterniond::Identity():
+			imu.localTransform().getQuaterniond();
+
+	const cv::Vec3d & f = imu.linearAcceleration();
+	Eigen::Vector3d acceleration = Eigen::Vector3d::Zero();
+	if((f[0] != 0.0 || f[1] != 0.0 || f[2] != 0.0) &&
+	   (imu.linearAccelerationCovariance().empty() || imu.linearAccelerationCovariance().at<double>(0,0) != -1.0))
+	{
+		// The specific force includes the reaction to gravity, up in the world frame
+		acceleration = worldToImu * Eigen::Vector3d(f[0], f[1], f[2]) - Eigen::Vector3d(0, 0, gravity_);
+	}
+	addSample(stamp, worldToImu * baseToImu.inverse(), acceleration);
+}
+
+void ImuMotionPredictor::addSample(double stamp, const Eigen::Quaterniond & orientation, const Eigen::Vector3d & acceleration)
+{
+	Sample sample;
+	sample.orientation = orientation.normalized();
+	sample.acceleration = acceleration;
+	samples_[stamp] = sample;
+	if(!integrated_.empty() && stamp <= integrated_.rbegin()->first)
+	{
+		// Out of order: what was integrated after it changes
+		integrated_.clear();
+	}
+	while(samples_.size() > 2 && samples_.begin()->first < stamp - kMaxBufferDuration)
+	{
+		samples_.erase(samples_.begin());
+		integrated_.clear();
+	}
+}
+
+void ImuMotionPredictor::addPose(double stamp, const rtabmap::Transform & pose)
+{
+	integrated_.clear();
+	if(pose.isNull())
+	{
+		pose_.setNull();
+		poseStamp_ = 0.0;
+		velocity_.setZero();
+		poses_.clear();
+		return;
+	}
+
+	Eigen::Vector3d velocity = Eigen::Vector3d::Zero();
+	Eigen::Quaterniond worldToOdom = Eigen::Quaterniond::Identity();
+	if(!samples_.empty())
+	{
+		// The odometry and the IMU agree on the orientation of the base at that time,
+		// whatever the yaw of their frames.
+		worldToOdom = (pose.getQuaterniond() * orientationAt(stamp).inverse()).normalized();
+	}
+
+	// The velocity is estimated from the displacement since a previous pose. Not the
+	// last one: odometry's own noise, divided by a frame interval, would be of the order of
+	// the velocity itself, and since the prediction deskews the next scans, that noise would
+	// feed back into the next poses. The newest pose at least velocityWindow_ old is used
+	// instead (or the oldest kept if there is none yet), not older than maxPoseInterval_.
+	poses_.erase(poses_.lower_bound(stamp), poses_.end());
+	poses_.erase(poses_.begin(), poses_.lower_bound(stamp - maxPoseInterval_));
+	std::map<double, rtabmap::Transform>::const_iterator reference = poses_.begin();
+	for(std::map<double, rtabmap::Transform>::const_iterator iter = poses_.begin();
+		iter != poses_.end() && iter->first <= stamp - velocityWindow_; ++iter)
+	{
+		reference = iter;
+	}
+	if(reference != poses_.end())
+	{
+		const double interval = stamp - reference->first;
+		const Eigen::Vector3d displacement(
+				pose.x() - reference->second.x(),
+				pose.y() - reference->second.y(),
+				pose.z() - reference->second.z());
+
+		if(velocityWindow_ > 0.0 &&
+		   !samples_.empty() &&
+		   samples_.begin()->first <= reference->first &&
+		   samples_.rbegin()->first >= stamp)
+		{
+			// displacement = v0*T + D, with D the double integral of the acceleration
+			// over the interval: solve for v0, the velocity at the reference pose, then
+			// carry it to this pose with the single integral V.
+			Eigen::Vector3d deltaVelocity;
+			Eigen::Vector3d deltaPosition;
+			integrate(reference->first, stamp, worldToOdom, deltaVelocity, deltaPosition);
+			velocity = (displacement - deltaPosition) / interval + deltaVelocity;
+		}
+		else
+		{
+			// Average velocity over the interval (no window, or the samples don't cover it)
+			velocity = displacement / interval;
+		}
+	}
+
+	pose_ = pose;
+	poseStamp_ = stamp;
+	velocity_ = velocity;
+	worldToOdom_ = worldToOdom;
+	poses_[stamp] = pose;
+
+	// The next velocities are estimated from the poses kept: keep the samples from the
+	// oldest one, including the last sample before it to interpolate at its stamp.
+	std::map<double, Sample>::iterator iter = samples_.upper_bound(poses_.begin()->first);
+	if(iter != samples_.begin())
+	{
+		--iter;
+		samples_.erase(samples_.begin(), iter);
+	}
+}
+
+void ImuMotionPredictor::reset()
+{
+	samples_.clear();
+	poses_.clear();
+	integrated_.clear();
+	pose_.setNull();
+	poseStamp_ = 0.0;
+	velocity_.setZero();
+	worldToOdom_.setIdentity();
+}
+
+rtabmap::Transform ImuMotionPredictor::predict(double stamp) const
+{
+	if(samples_.empty())
+	{
+		return rtabmap::Transform();
+	}
+	if(pose_.isNull())
+	{
+		// No pose yet (or lost): only the orientation is known, in the IMU's world frame.
+		const Eigen::Quaterniond orientation = orientationAt(stamp);
+		return rtabmap::Transform(0, 0, 0, orientation.x(), orientation.y(), orientation.z(), orientation.w());
+	}
+
+	const Eigen::Quaterniond orientation = (worldToOdom_ * orientationAt(stamp)).normalized();
+
+	// With t0 the stamp of the last pose, p0 its position and v0 the velocity there, and
+	// a(u) the acceleration (gravity removed, in the odometry frame), the position at t is:
+	//
+	//   p(t) = p0 + v0*(t-t0) + D(t),  with D(t) = integral_t0^t integral_t0^s a(u) du ds
+	//
+	// D(t) is the displacement due to the change of velocity since t0, V(s) = integral_t0^s a(u) du.
+	Eigen::Vector3d position(pose_.x(), pose_.y(), pose_.z()); // p0
+	position += velocity_ * (stamp - poseStamp_);               // + v0*(t-t0)
+	if(velocityWindow_ <= 0.0)
+	{
+		// No acceleration without a velocity window: constant velocity
+	}
+	else if(stamp >= poseStamp_)
+	{
+		// + D(t). D and V are kept at every sample stamp since t0 (integrated_), so only
+		// the part from the last sample ti <= t is integrated here, with dt = t-ti:
+		//
+		//   D(t) = D(ti) + V(ti)*dt + integral_ti^t integral_ti^s a(u) du ds
+		//
+		// Between samples the acceleration is linear, from a(ti) to a(t), for which that
+		// last double integral is exactly dt^2*(2*a(ti) + a(t))/6. Those are the same
+		// segments integrate() would go through, without redoing all those before ti.
+		updateIntegration();
+		std::map<double, Integrated>::const_iterator from = integrated_.upper_bound(stamp);
+		--from; // ti: the first entry is at t0, so there is one
+		const double dt = stamp - from->first;
+		const Eigen::Vector3d accelerationB = worldToOdom_ * accelerationAt(stamp); // a(t)
+		position += from->second.position +                       // D(ti)
+				from->second.velocity * dt +                          // V(ti)*dt
+				dt * dt * (2.0 * from->second.acceleration + accelerationB) / 6.0; // a(ti) to a(t)
+	}
+	else
+	{
+		// + D(t), backward from t0: only for points stamped before the last pose, rare
+		Eigen::Vector3d deltaVelocity;
+		Eigen::Vector3d deltaPosition;
+		integrate(poseStamp_, stamp, worldToOdom_, deltaVelocity, deltaPosition);
+		position += deltaPosition;
+	}
+
+	return rtabmap::Transform(position.x(), position.y(), position.z(),
+			orientation.x(), orientation.y(), orientation.z(), orientation.w());
+}
+
+bool ImuMotionPredictor::hasPose() const
+{
+	return !pose_.isNull();
+}
+
+double ImuMotionPredictor::lastPoseStamp() const
+{
+	return poseStamp_;
+}
+
+Eigen::Vector3d ImuMotionPredictor::velocity() const
+{
+	return velocity_;
+}
+
+size_t ImuMotionPredictor::samples() const
+{
+	return samples_.size();
+}
+
+// samples_ must not be empty for the four below.
+
+void ImuMotionPredictor::updateIntegration() const
+{
+	if(!integrated_.empty() && !integratedStartIsFinal_ && samples_.rbegin()->first >= poseStamp_)
+	{
+		// The acceleration at the pose was held from the newest sample, which is no
+		// longer the newest
+		integrated_.clear();
+	}
+	if(integrated_.empty())
+	{
+		Integrated start;
+		start.acceleration = worldToOdom_ * accelerationAt(poseStamp_);
+		start.velocity.setZero();
+		start.position.setZero();
+		integrated_[poseStamp_] = start;
+		integratedStartIsFinal_ = samples_.rbegin()->first >= poseStamp_;
+	}
+	// Extend to the samples received since
+	for(std::map<double, Sample>::const_iterator iter = samples_.upper_bound(integrated_.rbegin()->first);
+		iter != samples_.end(); ++iter)
+	{
+		const std::pair<const double, Integrated> & previous = *integrated_.rbegin();
+		const double dt = iter->first - previous.first;
+		Integrated next;
+		// Same segment as in integrate(), from the previous sample ti to this one:
+		// D(ti+1) = D(ti) + V(ti)*dt + dt^2*(2*a(ti) + a(ti+1))/6, V(ti+1) = V(ti) + dt*(a(ti) + a(ti+1))/2
+		next.acceleration = worldToOdom_ * iter->second.acceleration;
+		next.position = previous.second.position + previous.second.velocity * dt +
+				dt * dt * (2.0 * previous.second.acceleration + next.acceleration) / 6.0;
+		next.velocity = previous.second.velocity + dt * (previous.second.acceleration + next.acceleration) / 2.0;
+		integrated_.insert(integrated_.end(), std::make_pair(iter->first, next));
+	}
+}
+
+Eigen::Quaterniond ImuMotionPredictor::orientationAt(double stamp) const
+{
+	std::map<double, Sample>::const_iterator after = samples_.lower_bound(stamp);
+	if(after == samples_.end())
+	{
+		return samples_.rbegin()->second.orientation;
+	}
+	if(after == samples_.begin() || after->first == stamp)
+	{
+		return after->second.orientation;
+	}
+	std::map<double, Sample>::const_iterator before = std::prev(after);
+	const double ratio = (stamp - before->first) / (after->first - before->first);
+	return before->second.orientation.slerp(ratio, after->second.orientation);
+}
+
+Eigen::Vector3d ImuMotionPredictor::accelerationAt(double stamp) const
+{
+	std::map<double, Sample>::const_iterator after = samples_.lower_bound(stamp);
+	if(after == samples_.end())
+	{
+		return samples_.rbegin()->second.acceleration;
+	}
+	if(after == samples_.begin() || after->first == stamp)
+	{
+		return after->second.acceleration;
+	}
+	std::map<double, Sample>::const_iterator before = std::prev(after);
+	const double ratio = (stamp - before->first) / (after->first - before->first);
+	return before->second.acceleration + ratio * (after->second.acceleration - before->second.acceleration);
+}
+
+void ImuMotionPredictor::integrate(double from, double to, const Eigen::Quaterniond & rotation,
+		Eigen::Vector3d & velocity, Eigen::Vector3d & position) const
+{
+	velocity.setZero();
+	position.setZero();
+	if(from == to)
+	{
+		return;
+	}
+
+	// Breakpoints: the bounds and every sample in between, in the direction of
+	// integration (backward if "to" is before "from"). Between two of them the
+	// acceleration is linear, which the segment update below integrates exactly; before
+	// the first sample and after the last one, it is held constant.
+	std::vector<double> stamps;
+	stamps.push_back(from);
+	if(from < to)
+	{
+		for(std::map<double, Sample>::const_iterator iter = samples_.upper_bound(from);
+			iter != samples_.end() && iter->first < to; ++iter)
+		{
+			stamps.push_back(iter->first);
+		}
+	}
+	else
+	{
+		std::map<double, Sample>::const_iterator iter = samples_.lower_bound(from);
+		while(iter != samples_.begin())
+		{
+			--iter;
+			if(iter->first <= to)
+			{
+				break;
+			}
+			stamps.push_back(iter->first);
+		}
+	}
+	stamps.push_back(to);
+
+	// Returned: with a(u) the acceleration rotated by "rotation", from "from" (t0) to "to" (t),
+	//
+	//   velocity = V(t) = integral_t0^t a(u) du
+	//   position = D(t) = integral_t0^t integral_t0^s a(u) du ds
+	//
+	// that is, the change of velocity and the displacement it causes (from a velocity null
+	// at t0: the caller adds v0*(t-t0)). They are accumulated breakpoint by breakpoint: from
+	// ti to the next one ti+1, with dt = ti+1 - ti and a(u) linear from a(ti) to a(ti+1),
+	//
+	//   D(ti+1) = D(ti) + V(ti)*dt + dt^2*(2*a(ti) + a(ti+1))/6
+	//   V(ti+1) = V(ti) + dt*(a(ti) + a(ti+1))/2
+	//
+	// both exact for a linear acceleration (dt is negative backward, the same formulas hold).
+	Eigen::Vector3d accelerationA = rotation * accelerationAt(stamps[0]); // a(t0)
+	for(size_t i=1; i<stamps.size(); ++i)
+	{
+		const double dt = stamps[i] - stamps[i-1];
+		const Eigen::Vector3d accelerationB = rotation * accelerationAt(stamps[i]); // a(ti+1)
+		position += velocity * dt +                                     // V(ti)*dt
+				dt * dt * (2.0 * accelerationA + accelerationB) / 6.0;  // a(ti) to a(ti+1)
+		velocity += dt * (accelerationA + accelerationB) / 2.0;         // trapezoid of a
+		accelerationA = accelerationB;
+	}
+}
+
+}

corelib/src/Odometry.cpp

@@ -161,6 +161,13 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 	Parameters::parse(parameters, Parameters::kOdomHolonomic(), _holonomic);
 	Parameters::parse(parameters, Parameters::kOdomGuessMotion(), guessFromMotion_);
 	Parameters::parse(parameters, Parameters::kOdomGuessSmoothingDelay(), guessSmoothingDelay_);
+	{
+		float imuGravity = Parameters::defaultOdomImuGravity();
+		Parameters::parse(parameters, Parameters::kOdomImuGravity(), imuGravity);
+		// The velocity is estimated over the smoothing delay, and the IMU acceleration used
+		// only with one (> 0)
+		imuMotionPredictor_ = ImuMotionPredictor(1.0, guessSmoothingDelay_, imuGravity);
+	}
 	Parameters::parse(parameters, Parameters::kOdomFillInfoData(), _fillInfoData);
 	Parameters::parse(parameters, Parameters::kOdomFilteringStrategy(), _filteringStrategy);
 	Parameters::parse(parameters, Parameters::kOdomParticleSize(), _particleSize);
@@ -228,6 +235,7 @@ void Odometry::reset(const Transform & initialPose)
 	framesProcessed_ = 0;
 	imuLastTransform_.setNull();
 	imus_.clear();
+	imuMotionPredictor_.reset();
 	if(_force3DoF || particleFilters_.size())
 	{
 		float x,y,z, roll,pitch,yaw;
@@ -312,38 +320,47 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 {
 	UASSERT_MSG(data.id() >= 0, uFormat("Input data should have ID greater or equal than 0 (id=%d)!", data.id()).c_str());
 
-	// cache imu data
-	if(!data.imu().empty() && !this->canProcessAsyncIMU())
+	if(!data.imu().empty())
 	{
-		if(!(data.imu().orientation()[0] == 0.0 && data.imu().orientation()[1] == 0.0 && data.imu().orientation()[2] == 0.0))
+		if(!this->canProcessAsyncIMU())
 		{
-			Transform orientation(0,0,0, data.imu().orientation()[0], data.imu().orientation()[1], data.imu().orientation()[2], data.imu().orientation()[3]);
-			// orientation includes roll and pitch but not yaw in local transform
-			Transform imuT = Transform(data.imu().localTransform().x(),data.imu().localTransform().y(),data.imu().localTransform().z(), 0,0,data.imu().localTransform().theta()) *
-					orientation*
-					data.imu().localTransform().rotation().inverse();
-
-			if(	this->getPose().r11() == 1.0f && this->getPose().r22() == 1.0f && this->getPose().r33() == 1.0f &&
-				this->framesProcessed() == 0)
+			// cache imu data
+			if(!(data.imu().orientation()[0] == 0.0 && data.imu().orientation()[1] == 0.0 && data.imu().orientation()[2] == 0.0))
 			{
-				Eigen::Quaterniond imuQuat = imuT.getQuaterniond();
-				Transform previous = this->getPose();
-				Transform newFramePose = Transform(previous.x(), previous.y(), previous.z(), imuQuat.x(), imuQuat.y(), imuQuat.z(), imuQuat.w());
-				UWARN("Updated initial pose from %s to %s with IMU orientation", previous.prettyPrint().c_str(), newFramePose.prettyPrint().c_str());
-				std::map<double, rtabmap::Transform> imus = imus_;
-				this->reset(newFramePose);
-				imus_ = imus;
+				Transform orientation(0,0,0, data.imu().orientation()[0], data.imu().orientation()[1], data.imu().orientation()[2], data.imu().orientation()[3]);
+				// orientation includes roll and pitch but not yaw in local transform
+				Transform imuT = Transform(data.imu().localTransform().x(),data.imu().localTransform().y(),data.imu().localTransform().z(), 0,0,data.imu().localTransform().theta()) *
+						orientation*
+						data.imu().localTransform().rotation().inverse();
+
+				imus_.insert(std::make_pair(data.stamp(), imuT));
+				if(imus_.size() > 1000)
+				{
+					imus_.erase(imus_.begin());
+				}
+
+				imuMotionPredictor_.addImu(data.stamp(), data.imu());
 			}
-
-			imus_.insert(std::make_pair(data.stamp(), imuT));
-			if(imus_.size() > 1000)
+			else
 			{
-				imus_.erase(imus_.begin());
+				UWARN("Received IMU doesn't have orientation set! It is ignored.");
 			}
 		}
-		else
+
+		// IMU-only update: nothing more to do once the IMU is cached, except for approaches
+		// processing it themselves. A frame that brings its own features carries no image,
+		// and a frame whose scene was empty carries no feature either, so neither says
+		// whether there is a frame at all. The calibration does: it is there when a camera
+		// produced this data.
+		if(data.imageRaw().empty() && data.imageCompressed().empty() &&
+		   data.laserScanRaw().isEmpty() && data.laserScanCompressed().isEmpty() &&
+		   data.cameraModels().empty() && data.stereoCameraModels().empty())
 		{
-			UWARN("Received IMU doesn't have orientation set! It is ignored.");
+			if(this->canProcessAsyncIMU())
+			{
+				this->computeTransform(data, Transform(), info);
+			}
+			return Transform(); // Return null on IMU-only updates
 		}
 	}
 
@@ -588,12 +605,40 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 		}
 	}
 
+	// Initial orientation from the IMU at the first frame's own stamp (unless an initial pose
+	// with a rotation was given). Not from the first IMU sample received, which can be much
+	// older (e.g., IMU buffered while waiting for the first frame), nor from the newest one,
+	// which can be after the stamp (lidar deskewing needs IMU up to the end of the sweep).
+	if(this->framesProcessed() == 0 && !imus_.empty() &&
+	   this->getPose().r11() == 1.0f && this->getPose().r22() == 1.0f && this->getPose().r33() == 1.0f)
+	{
+		// Interpolated at the stamp, or the closest sample if the IMU doesn't cover it (e.g.,
+		// the only sample received is just after the frame)
+		Transform imuAtStamp = Transform::getTransform(imus_, data.stamp());
+		if(imuAtStamp.isNull())
+		{
+			imuAtStamp = data.stamp() < imus_.begin()->first?imus_.begin()->second:imus_.rbegin()->second;
+		}
+		if(!imuAtStamp.isNull())
+		{
+			const Eigen::Quaterniond q = imuAtStamp.getQuaterniond();
+			const Transform previous = this->getPose();
+			const Transform initialPose(previous.x(), previous.y(), previous.z(), q.x(), q.y(), q.z(), q.w());
+			UWARN("Updated initial pose from %s to %s with IMU orientation", previous.prettyPrint().c_str(), initialPose.prettyPrint().c_str());
+			std::map<double, rtabmap::Transform> imus = imus_;
+			ImuMotionPredictor imuMotionPredictor = imuMotionPredictor_;
+			this->reset(initialPose);
+			imus_ = imus;
+			imuMotionPredictor_ = imuMotionPredictor;
+		}
+	}
+
 	// KITTI datasets start with stamp=0
 	double dt = previousStamp_>0.0f || (previousStamp_==0.0f && framesProcessed()==1)?data.stamp() - previousStamp_:0.0;
 	Transform guess = dt>0.0 && guessFromMotion_ && !velocityGuess_.isNull()?Transform::getIdentity():Transform();
 	if(!(dt>0.0 || (dt == 0.0 && velocityGuess_.isNull())))
 	{
-		if(guessFromMotion_ && (!data.imageRaw().empty() || !data.laserScanRaw().isEmpty()))
+		if(guessFromMotion_)
 		{
 			UERROR("Guess from motion is set but dt is invalid! Odometry is then computed without guess. (dt=%f previous transform=%s)", dt, velocityGuess_.prettyPrint().c_str());
 		}
@@ -646,6 +691,17 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 					orientation.r11(), orientation.r12(), orientation.r13(), guess.x(),
 					orientation.r21(), orientation.r22(), orientation.r23(), guess.y(),
 					orientation.r31(), orientation.r32(), orientation.r33(), guess.z());
+			if(guessFromMotion_ && guessSmoothingDelay_ > 0.0f && imuMotionPredictor_.hasPose())
+			{
+				// Translation (and orientation) predicted from the previous pose with the
+				// IMU acceleration, instead of a constant velocity: the velocity is the one
+				// over the smoothing delay, carried to the previous frame with the IMU.
+				Transform predicted = imuMotionPredictor_.predict(data.stamp());
+				if(!predicted.isNull())
+				{
+					guess = _pose.inverse() * predicted;
+				}
+			}
 			if(_force3DoF)
 			{
 				guess = guess.to3DoF();
@@ -657,21 +713,54 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 			UWARN("Could not find imu transform at %f", data.stamp());
 		}
 	}
-	else if(!guess.isNull() && (!data.imageRaw().empty() || !data.laserScanRaw().isEmpty())) {
+	else if(!guess.isNull()) {
 		UDEBUG("Using guess from motion %s", guess.prettyPrint().c_str());
 	}
 
 	UTimer time;
 
-	// Deskewing lidar
+	// Deskewing lidar, if the scan has a time spread (not already deskewed: deskewing zeroes
+	// the time channel)
+	const bool scanHasTimeSpread =
+			!data.laserScanRaw().empty() &&
+			data.laserScanRaw().hasTime() &&
+			data.laserScanRaw().data().ptr<float>(0, data.laserScanRaw().size()-1)[data.laserScanRaw().getTimeOffset()] !=
+			data.laserScanRaw().data().ptr<float>(0, 0)[data.laserScanRaw().getTimeOffset()];
 	if( _deskewing &&
-		!data.laserScanRaw().empty() &&
-		data.laserScanRaw().hasTime() &&
+		scanHasTimeSpread &&
+		!imus_.empty() &&
+		!imuMotionPredictor_.predict(data.stamp()).isNull())
+	{
+		UDEBUG("Deskewing with IMU begin");
+		// Every point's pose predicted with the IMU since the previous frame: orientation
+		// from the IMU, translation from the velocity (carried with the IMU acceleration
+		// with a smoothing delay). Before the first pose, only the orientation.
+		const Transform referenceInverse = imuMotionPredictor_.predict(data.stamp()).inverse();
+		auto motion = [&](double stamp)
+		{
+			Transform pose = imuMotionPredictor_.predict(stamp);
+			if(pose.isNull())
+			{
+				return pose;
+			}
+			pose = referenceInverse * pose;
+			return _force3DoF?pose.to3DoF():pose;
+		};
+		LaserScan scanDeskewed = util3d::deskew(data.laserScanRaw(), data.stamp(), motion);
+		if(!scanDeskewed.isEmpty())
+		{
+			data.setLaserScan(scanDeskewed);
+		}
+		info->timeDeskewing = time.ticks();
+		UDEBUG("Deskewing end");
+	}
+	else if( _deskewing &&
+		scanHasTimeSpread &&
 		dt > 0 &&
 		!guess.isNull())
 	{
 		UDEBUG("Deskewing begin");
-		// Recompute velocity
+		// Constant velocity
 		float vx,vy,vz, vroll,vpitch,vyaw;
 		guess.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw);
 
@@ -683,38 +772,6 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 		vpitch /= dt;
 		vyaw /= dt;
 
-		if(!imus_.empty())
-		{
-			float scanTime =
-				data.laserScanRaw().data().ptr<float>(0, data.laserScanRaw().size()-1)[data.laserScanRaw().getTimeOffset()] -
-				data.laserScanRaw().data().ptr<float>(0, 0)[data.laserScanRaw().getTimeOffset()];
-
-			// replace orientation velocity based on IMU (if available)
-			Transform imuFirstScan = Transform::getTransform(imus_,
-					data.stamp() +
-					data.laserScanRaw().data().ptr<float>(0, 0)[data.laserScanRaw().getTimeOffset()]);
-			Transform imuLastScan = Transform::getTransform(imus_,
-					data.stamp() +
-					data.laserScanRaw().data().ptr<float>(0, data.laserScanRaw().size()-1)[data.laserScanRaw().getTimeOffset()]);
-			if(!imuFirstScan.isNull() && !imuLastScan.isNull())
-			{
-				Transform orientation = imuFirstScan.inverse() * imuLastScan;
-				orientation.getEulerAngles(vroll, vpitch, vyaw);
-				if(_force3DoF)
-				{
-					vroll=0;
-					vpitch=0;
-					vyaw /= scanTime;
-				}
-				else
-				{
-					vroll /= scanTime;
-					vpitch /= scanTime;
-					vyaw /= scanTime;
-				}
-			}
-		}
-
 		Transform velocity(vx,vy,vz,vroll,vpitch,vyaw);
 		LaserScan scanDeskewed = util3d::deskew(data.laserScanRaw(), data.stamp(), velocity);
 		if(!scanDeskewed.isEmpty())
@@ -837,23 +894,11 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 			}
 		}
 	}
-	// A frame that brings its own features carries no image, and a frame whose scene was
-	// empty carries no feature either, so neither says whether there is a frame at all.
-	// The calibration does: it is there when a camera produced this data.
-	else if(!data.imageRaw().empty() ||
-			!data.cameraModels().empty() ||
-			!data.stereoCameraModels().empty() ||
-			!data.laserScanRaw().isEmpty() ||
-			(this->canProcessAsyncIMU() && !data.imu().empty()))
+	else
 	{
 		t = this->computeTransform(data, guess, info);
 	}
 
-	if(data.imageRaw().empty() && data.laserScanRaw().isEmpty() && !data.imu().empty())
-	{
-		return Transform(); // Return null on IMU-only updates
-	}
-
 	if(info)
 	{
 		info->timeEstimation = time.ticks();
@@ -877,6 +922,12 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 		}
 	}
 
+	if(t.isNull())
+	{
+		// Lost: no velocity can be estimated across the reset that follows
+		imuMotionPredictor_.addPose(data.stamp(), Transform());
+	}
+
 	if(!t.isNull())
 	{
 		_resetCurrentCount = _resetCountdown;
@@ -1032,6 +1083,21 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 			velocityGuess_.setNull();
 		}
 
+		{
+			const Transform newPose = _pose * t;
+			imuMotionPredictor_.addPose(data.stamp(), newPose);
+			if(guessSmoothingDelay_ > 0.0f && !imus_.empty() &&
+			   !velocityGuess_.isNull() && _filteringStrategy != 1 && particleFilters_.empty())
+			{
+				// The translational velocity over the smoothing delay, carried to this
+				// frame with the IMU acceleration (see ImuMotionPredictor), in this frame.
+				const Eigen::Vector3d v = newPose.getQuaterniond().inverse() * imuMotionPredictor_.velocity();
+				float vx,vy,vz, vroll,vpitch,vyaw;
+				velocityGuess_.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw);
+				velocityGuess_ = Transform(v.x(), v.y(), v.z(), vroll, vpitch, vyaw);
+			}
+		}
+
 		if(info)
 		{
 			distanceTravelled_ += t.getNorm();

corelib/src/OdometryThread.cpp

@@ -34,6 +34,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/utilite/UTimer.h"
 
+#include <algorithm>
+
 namespace rtabmap {
 
 OdometryThread::OdometryThread(Odometry * odometry, unsigned int dataBufferMaxSize) :
@@ -236,13 +238,24 @@ bool OdometryThread::getData(SensorEvent & event)
 	{
 		if(!_dataBuffer.empty())
 		{
-			// Send IMU up to stamp greater than image (OpenVINS needs this).
+			// Send IMU up to stamp greater than image (OpenVINS needs this). For a lidar
+			// scan with a time channel, up to the end of its sweep: deskewing predicts the
+			// pose of every point with the IMU (approaches processing the IMU themselves
+			// get it as before).
+			double imuUntil = _dataBuffer.front().data().stamp();
+			const LaserScan & scan = _dataBuffer.front().data().laserScanRaw();
+			if(!_odometry->canProcessAsyncIMU() && !scan.isEmpty() && scan.hasTime())
+			{
+				imuUntil += std::max(0.0f, std::max(
+						scan.data().ptr<float>(0, 0)[scan.getTimeOffset()],
+						scan.data().ptr<float>(0, scan.size()-1)[scan.getTimeOffset()]));
+			}
 			while(!_imuBuffer.empty())
 			{
 				_odometry->process(_imuBuffer.front());
 				double stamp =_imuBuffer.front().stamp();
 				_imuBuffer.pop_front();
-				if(stamp > _dataBuffer.front().data().stamp()) {
+				if(stamp > imuUntil) {
 					break;
 				}
 			}

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/src/util3d.cpp

@@ -3822,17 +3822,12 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr loadCloud(
 	return util3d::transformPointCloud(cloud, transform);
 }
 
-LaserScan deskew(
+static LaserScan deskewImpl(
 		const LaserScan & input,
 		double inputStamp,
-		const rtabmap::Transform & velocity)
+		const std::function<rtabmap::Transform(double)> & motion,
+		bool slerp)
 {
-	if(velocity.isNull())
-	{
-		UERROR("velocity should be valid!");
-		return LaserScan();
-	}
-
 	if(!input.hasTime())
 	{
 		UERROR("input scan doesn't have a \"time\" channel! Supported formats: \"%s\", \"%s\".",
@@ -3861,33 +3856,28 @@ LaserScan deskew(
 		return LaserScan();
 	}
 
+	// With slerp, the poses of the base frame at the first and last stamps (relative to
+	// the base frame at inputStamp), interpolated in between
 	rtabmap::Transform firstPose;
 	rtabmap::Transform lastPose;
-
-	float vx,vy,vz, vroll,vpitch,vyaw;
-	velocity.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw);
-
-	//  1- The pose of base frame in odom frame at first stamp
-	//  2- The pose of base frame in odom frame at last stamp
-	double dt1 = firstStamp - inputStamp;
-	double dt2 = lastStamp - inputStamp;
-
-	firstPose = rtabmap::Transform(vx*dt1, vy*dt1, vz*dt1, vroll*dt1, vpitch*dt1, vyaw*dt1);
-	lastPose = rtabmap::Transform(vx*dt2, vy*dt2, vz*dt2, vroll*dt2, vpitch*dt2, vyaw*dt2);
-
-	if(firstPose.isNull())
+	if(slerp)
 	{
-		UERROR("Could not get transform between stamps %f and %f!",
-				firstStamp,
-				inputStamp);
-		return LaserScan();
-	}
-	if(lastPose.isNull())
-	{
-		UERROR("Could not get transform between stamps %f and %f!",
-				lastStamp,
-				inputStamp);
-		return LaserScan();
+		firstPose = motion(firstStamp);
+		lastPose = motion(lastStamp);
+		if(firstPose.isNull())
+		{
+			UERROR("Could not get transform between stamps %f and %f!",
+					firstStamp,
+					inputStamp);
+			return LaserScan();
+		}
+		if(lastPose.isNull())
+		{
+			UERROR("Could not get transform between stamps %f and %f!",
+					lastStamp,
+					inputStamp);
+			return LaserScan();
+		}
 	}
 
 	double stamp;
@@ -3918,7 +3908,12 @@ LaserScan deskew(
 		{
 			const float * inputPtr = input.data().ptr<float>(0, u);
 			stamp = inputStamp + inputPtr[offsetTime];
-			rtabmap::Transform transform = firstPose.interpolate((stamp-firstStamp) / scanTime, lastPose);
+			rtabmap::Transform transform = slerp?firstPose.interpolate((stamp-firstStamp) / scanTime, lastPose):motion(stamp);
+			if(transform.isNull())
+			{
+				UERROR("Could not get transform between stamps %f and %f!", stamp, inputStamp);
+				return LaserScan();
+			}
 
 			for(int v=0; v<input.data().rows; ++v)
 			{
@@ -3960,7 +3955,12 @@ LaserScan deskew(
 		{
 			const float * inputPtr = input.data().ptr<float>(v, 0);
 			stamp = inputStamp + inputPtr[offsetTime];
-			rtabmap::Transform transform = firstPose.interpolate((stamp-firstStamp) / scanTime, lastPose);
+			rtabmap::Transform transform = slerp?firstPose.interpolate((stamp-firstStamp) / scanTime, lastPose):motion(stamp);
+			if(transform.isNull())
+			{
+				UERROR("Could not get transform between stamps %f and %f!", stamp, inputStamp);
+				return LaserScan();
+			}
 
 			for(int u=0; u<input.data().cols; ++u)
 			{
@@ -3996,6 +3996,42 @@ LaserScan deskew(
 	return LaserScan(output, input.maxPoints(), input.rangeMax(), outputFormat, input.localTransform());
 }
 
+LaserScan deskew(
+		const LaserScan & input,
+		double inputStamp,
+		const rtabmap::Transform & velocity)
+{
+	if(velocity.isNull())
+	{
+		UERROR("velocity should be valid!");
+		return LaserScan();
+	}
+	float vx,vy,vz, vroll,vpitch,vyaw;
+	velocity.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw);
+	// The pose of the base frame at a stamp relative to the one at inputStamp, with a
+	// constant velocity: computed at the first and last stamps, interpolated in between
+	auto motion = [&](double stamp)
+	{
+		const double dt = stamp - inputStamp;
+		return rtabmap::Transform(vx*dt, vy*dt, vz*dt, vroll*dt, vpitch*dt, vyaw*dt);
+	};
+	return deskewImpl(input, inputStamp, motion, true);
+}
+
+LaserScan deskew(
+		const LaserScan & input,
+		double inputStamp,
+		const std::function<rtabmap::Transform(double stamp)> & motion,
+		bool slerp)
+{
+	if(!motion)
+	{
+		UERROR("motion should be set!");
+		return LaserScan();
+	}
+	return deskewImpl(input, inputStamp, motion, slerp);
+}
+
 
 }
 

corelib/test/CMakeLists.txt

@@ -44,6 +44,7 @@ set(corelib_test_sources
 	test_gps.cpp                        #GPS.h
 	test_imu.cpp                        #IMU.h
 	test_imufilter.cpp                  #IMUFilter.h
+	test_imumotionpredictor.cpp         #ImuMotionPredictor.h
 	test_imuthread.cpp                  #IMUThread.h
 	test_landmark.cpp                   #Landmark.h
 	test_localgrid.cpp                  #LocalGrid.h

corelib/test/test_imumotionpredictor.cpp

@@ -0,0 +1,369 @@
+#include <gtest/gtest.h>
+
+#include <cmath>
+#include <algorithm>
+#include <functional>
+
+#include <rtabmap/core/ImuMotionPredictor.h>
+#include <rtabmap/core/IMU.h>
+
+using rtabmap::ImuMotionPredictor;
+
+namespace {
+
+Eigen::Quaterniond yaw(double angle)
+{
+	return Eigen::Quaterniond(Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitZ()));
+}
+
+rtabmap::Transform pose(const Eigen::Vector3d & position, double angle)
+{
+	return rtabmap::Transform(position.x(), position.y(), position.z(), 0, 0, angle);
+}
+
+/// An IMU at rest or accelerating, as measured: orientation of the IMU in its world
+/// frame, and the specific force (acceleration minus gravity) in the IMU frame.
+rtabmap::IMU measuredImu(const Eigen::Quaterniond & worldToImu,
+		const Eigen::Vector3d & accelerationInWorld, double gravity,
+		const rtabmap::Transform & baseToImu)
+{
+	const Eigen::Vector3d f = worldToImu.inverse() * (accelerationInWorld + Eigen::Vector3d(0, 0, gravity));
+	const Eigen::Quaterniond q = worldToImu.normalized();
+	return rtabmap::IMU(cv::Vec4d(q.x(), q.y(), q.z(), q.w()), cv::Mat::eye(3,3,CV_64FC1),
+			cv::Vec3d(0,0,0), cv::Mat::eye(3,3,CV_64FC1),
+			cv::Vec3d(f.x(), f.y(), f.z()), cv::Mat::eye(3,3,CV_64FC1),
+			baseToImu);
+}
+
+/// IMU measurements at 200 Hz over [from, to] of an IMU at the base origin, oriented and
+/// accelerating (in its world frame) as given. Without accelerometer, it measures no
+/// linear acceleration at all.
+void addSamples(ImuMotionPredictor & predictor, double from, double to,
+		const std::function<Eigen::Quaterniond(double)> & orientation,
+		const std::function<Eigen::Vector3d(double)> & acceleration,
+		bool withAccelerometer = true)
+{
+	for(int i=0; from + i*0.005 <= to + 1e-9; ++i)
+	{
+		const double t = from + i*0.005;
+		rtabmap::IMU imu = measuredImu(orientation(t), acceleration(t), predictor.gravity(), rtabmap::Transform::getIdentity());
+		if(!withAccelerometer)
+		{
+			imu = rtabmap::IMU(imu.orientation(), imu.orientationCovariance(),
+					imu.angularVelocity(), imu.angularVelocityCovariance(),
+					cv::Vec3d(0,0,0), imu.linearAccelerationCovariance(),
+					imu.localTransform());
+		}
+		predictor.addImu(t, imu);
+	}
+}
+
+}  // namespace
+
+TEST(ImuMotionPredictor, predicts_only_the_orientation_without_a_pose)
+{
+	ImuMotionPredictor predictor;
+	EXPECT_TRUE(predictor.predict(1.0).isNull());
+
+	predictor.addImu(1.0, measuredImu(yaw(0.3), Eigen::Vector3d(1, 0, 0), 9.80665, rtabmap::Transform::getIdentity()));
+	predictor.addImu(1.1, measuredImu(yaw(0.5), Eigen::Vector3d(1, 0, 0), 9.80665, rtabmap::Transform::getIdentity()));
+	const rtabmap::Transform predicted = predictor.predict(1.05);
+	ASSERT_FALSE(predicted.isNull()) << "the orientation is known without a pose";
+	EXPECT_NEAR(predicted.theta(), 0.4, 1e-5);
+	EXPECT_NEAR(predicted.x(), 0.0, 1e-9) << "no position without a pose";
+
+	ImuMotionPredictor withoutImu;
+	withoutImu.addPose(1.0, rtabmap::Transform::getIdentity());
+	EXPECT_TRUE(withoutImu.predict(1.0).isNull()) << "no imu yet";
+}
+
+TEST(ImuMotionPredictor, follows_a_constant_velocity)
+{
+	ImuMotionPredictor predictor;
+	const Eigen::Vector3d velocity(1.0, -0.5, 0.2);
+	addSamples(predictor, 0.0, 0.3,
+			[](double) { return Eigen::Quaterniond::Identity(); },
+			[](double) { return Eigen::Vector3d::Zero(); });
+	predictor.addPose(0.0, pose(Eigen::Vector3d::Zero(), 0));
+	EXPECT_TRUE(predictor.velocity().isZero()) << "a single pose has no velocity";
+	predictor.addPose(0.1, pose(velocity * 0.1, 0));
+
+	EXPECT_TRUE(predictor.velocity().isApprox(velocity, 1e-6));
+	const rtabmap::Transform predicted = predictor.predict(0.25);
+	ASSERT_FALSE(predicted.isNull());
+	EXPECT_NEAR(predicted.x(), velocity.x() * 0.25, 1e-6);
+	EXPECT_NEAR(predicted.y(), velocity.y() * 0.25, 1e-6);
+	EXPECT_NEAR(predicted.z(), velocity.z() * 0.25, 1e-6);
+}
+
+TEST(ImuMotionPredictor, integrates_the_acceleration)
+{
+	// From rest at t=0 with a constant 2 m/s^2: p = t^2, v = 2t. The velocity at the
+	// second pose is the instantaneous one, not the average over the interval, and the
+	// prediction keeps accelerating. An IMU without accelerometer gives a constant
+	// velocity instead.
+	const double a = 2.0;
+	for(bool withAccelerometer : {true, false})
+	{
+		ImuMotionPredictor predictor;
+		addSamples(predictor, -0.05, 0.3,
+				[](double) { return Eigen::Quaterniond::Identity(); },
+				[&](double t) { return Eigen::Vector3d(t < 0.0 ? 0.0 : a, 0, 0); },
+				withAccelerometer);
+		predictor.addPose(0.0, pose(Eigen::Vector3d::Zero(), 0));
+		predictor.addPose(0.1, pose(Eigen::Vector3d(0.5*a*0.01, 0, 0), 0));
+
+		const double t = 0.2;
+		const rtabmap::Transform predicted = predictor.predict(t);
+		ASSERT_FALSE(predicted.isNull());
+		if(withAccelerometer)
+		{
+			EXPECT_NEAR(predictor.velocity().x(), a*0.1, 1e-6);
+			EXPECT_NEAR(predicted.x(), 0.5*a*t*t, 1e-6);
+		}
+		else
+		{
+			// Constant velocity model: the average velocity over the last interval.
+			EXPECT_NEAR(predictor.velocity().x(), 0.5*a*0.1, 1e-6);
+			EXPECT_NEAR(predicted.x(), 0.5*a*0.01 + 0.5*a*0.1*(t-0.1), 1e-6);
+		}
+	}
+}
+
+TEST(ImuMotionPredictor, expresses_the_imu_in_the_odometry_frame)
+{
+	// The IMU's world frame and the odometry frame differ by 90 degrees of yaw. The base
+	// turns at 1 rad/s and accelerates along the IMU world's x, which is the odometry's y.
+	const double rate = 1.0;
+	const double offset = M_PI/2.0;
+	const double a = 3.0;
+	ImuMotionPredictor predictor;
+	addSamples(predictor, 0.0, 0.3,
+			[&](double t) { return yaw(rate*t); },
+			[&](double) { return Eigen::Vector3d(a, 0, 0); });
+	predictor.addPose(0.0, pose(Eigen::Vector3d::Zero(), offset));
+	predictor.addPose(0.1, pose(Eigen::Vector3d(0, 0.5*a*0.01, 0), offset + rate*0.1));
+
+	const double t = 0.25;
+	const rtabmap::Transform predicted = predictor.predict(t);
+	ASSERT_FALSE(predicted.isNull());
+	EXPECT_NEAR(predicted.x(), 0.0, 1e-6);
+	EXPECT_NEAR(predicted.y(), 0.5*a*t*t, 1e-6);
+	EXPECT_NEAR(predicted.theta(), offset + rate*t, 1e-5);
+}
+
+TEST(ImuMotionPredictor, a_lost_pose_resets_the_prediction)
+{
+	ImuMotionPredictor predictor;
+	addSamples(predictor, 0.0, 0.5,
+			[](double) { return Eigen::Quaterniond::Identity(); },
+			[](double) { return Eigen::Vector3d::Zero(); });
+	predictor.addPose(0.0, pose(Eigen::Vector3d::Zero(), 0));
+	predictor.addPose(0.1, pose(Eigen::Vector3d(0.1, 0, 0), 0));
+	ASSERT_FALSE(predictor.velocity().isZero());
+
+	predictor.addPose(0.2, rtabmap::Transform());
+	EXPECT_TRUE(predictor.predict(0.25).isIdentity()) << "orientation only, which is constant here";
+
+	// After a reset of the odometry, the pose jumps: no velocity across it.
+	predictor.addPose(0.3, pose(Eigen::Vector3d(10, 0, 0), 0));
+	EXPECT_TRUE(predictor.velocity().isZero());
+	EXPECT_NEAR(predictor.predict(0.4).x(), 10.0, 1e-6);
+}
+
+TEST(ImuMotionPredictor, poses_too_far_apart_give_no_velocity)
+{
+	ImuMotionPredictor predictor(0.5);
+	addSamples(predictor, 0.0, 1.5,
+			[](double) { return Eigen::Quaterniond::Identity(); },
+			[](double) { return Eigen::Vector3d::Zero(); });
+	predictor.addPose(0.0, pose(Eigen::Vector3d::Zero(), 0));
+	predictor.addPose(1.0, pose(Eigen::Vector3d(1, 0, 0), 0));
+	EXPECT_TRUE(predictor.velocity().isZero());
+}
+
+TEST(ImuMotionPredictor, a_longer_window_averages_out_the_pose_noise)
+{
+	// 1 m/s along x, with odometry poses alternating 1 cm on each side of the truth: the
+	// worst case for a velocity differenced over one frame, which sees 0.2 m/s of noise.
+	for(double window : {0.0, 0.5})
+	{
+		ImuMotionPredictor predictor(1.0, window);
+		addSamples(predictor, 0.0, 2.0,
+				[](double) { return Eigen::Quaterniond::Identity(); },
+				[](double) { return Eigen::Vector3d::Zero(); });
+		double maxError = 0.0;
+		for(int i=0; i<=15; ++i)
+		{
+			const double t = i*0.1;
+			predictor.addPose(t, pose(Eigen::Vector3d(t + (i%2?0.01:-0.01), 0, 0), 0));
+			if(i >= 10)
+			{
+				maxError = std::max(maxError, std::fabs(predictor.velocity().x() - 1.0));
+			}
+		}
+		if(window == 0.0)
+		{
+			EXPECT_NEAR(maxError, 0.2, 1e-6) << "differenced over one frame";
+		}
+		else
+		{
+			EXPECT_LT(maxError, 0.05) << "differenced over half a second";
+		}
+	}
+}
+
+TEST(ImuMotionPredictor, keeps_only_the_samples_since_the_last_pose)
+{
+	ImuMotionPredictor predictor;
+	addSamples(predictor, 0.0, 0.2,
+			[](double) { return Eigen::Quaterniond::Identity(); },
+			[](double) { return Eigen::Vector3d::Zero(); });
+	ASSERT_EQ(predictor.samples(), 41u);
+	predictor.addPose(0.1025, pose(Eigen::Vector3d::Zero(), 0));
+	// 0.100 (the last one before the pose, to interpolate at its stamp) .. 0.200
+	EXPECT_EQ(predictor.samples(), 21u);
+
+	predictor.reset();
+	EXPECT_EQ(predictor.samples(), 0u);
+	EXPECT_TRUE(predictor.predict(0.2).isNull());
+}
+
+
+TEST(ImuMotionPredictor, removes_gravity_from_what_the_imu_measures)
+{
+	// The IMU is mounted rolled by 90 degrees on a level base at rest: it measures gravity
+	// along its own y. Once removed, nothing moves, and the base stays level.
+	const rtabmap::Transform baseToImu(0, 0, 0, M_PI/2.0, 0, 0);
+	const Eigen::Quaterniond worldToImu = baseToImu.getQuaterniond();
+	ImuMotionPredictor predictor;
+	for(int i=0; i<=60; ++i)
+	{
+		predictor.addImu(i*0.005, measuredImu(worldToImu, Eigen::Vector3d::Zero(), 9.80665, baseToImu));
+	}
+	predictor.addPose(0.0, rtabmap::Transform::getIdentity());
+	const rtabmap::Transform predicted = predictor.predict(0.3);
+	ASSERT_FALSE(predicted.isNull());
+	EXPECT_NEAR(predicted.getNorm(), 0.0, 1e-6) << "gravity was not removed";
+	EXPECT_TRUE(predicted.getQuaterniond().isApprox(Eigen::Quaterniond::Identity(), 1e-6)) << "the base orientation is the imu's, unmounted";
+}
+
+TEST(ImuMotionPredictor, removes_the_gravity_it_is_given)
+{
+	// On the Moon, at rest: the IMU measures 1.62 m/s^2 up.
+	ImuMotionPredictor predictor(1.0, 0.5, 1.62);
+	for(int i=0; i<=60; ++i)
+	{
+		predictor.addImu(i*0.005, measuredImu(Eigen::Quaterniond::Identity(), Eigen::Vector3d::Zero(), 1.62,
+				rtabmap::Transform::getIdentity()));
+	}
+	predictor.addPose(0.0, rtabmap::Transform::getIdentity());
+	EXPECT_NEAR(predictor.predict(0.3).z(), 0.0, 1e-6);
+
+	// Earth's gravity removed from the same measurement: it looks like falling.
+	ImuMotionPredictor earth;
+	for(int i=0; i<=60; ++i)
+	{
+		earth.addImu(i*0.005, measuredImu(Eigen::Quaterniond::Identity(), Eigen::Vector3d::Zero(), 1.62,
+				rtabmap::Transform::getIdentity()));
+	}
+	earth.addPose(0.0, rtabmap::Transform::getIdentity());
+	EXPECT_NEAR(earth.predict(0.2).z(), 0.5*(1.62-9.80665)*0.04, 1e-6);
+}
+
+TEST(ImuMotionPredictor, an_imu_without_acceleration_is_not_a_free_fall)
+{
+	ImuMotionPredictor predictor;
+	const Eigen::Quaterniond q(Eigen::AngleAxisd(0.3, Eigen::Vector3d::UnitZ()));
+	for(int i=0; i<=60; ++i)
+	{
+		predictor.addImu(i*0.005, rtabmap::IMU(cv::Vec4d(q.x(), q.y(), q.z(), q.w()), cv::Mat::eye(3,3,CV_64FC1),
+				cv::Vec3d(0,0,0), cv::Mat::eye(3,3,CV_64FC1),
+				cv::Vec3d(0,0,0), cv::Mat::eye(3,3,CV_64FC1),
+				rtabmap::Transform::getIdentity()));
+	}
+	predictor.addPose(0.0, rtabmap::Transform::getIdentity());
+	EXPECT_NEAR(predictor.predict(0.3).getNorm(), 0.0, 1e-6);
+
+	// And one without orientation is ignored altogether.
+	ImuMotionPredictor noOrientation;
+	noOrientation.addImu(0.0, rtabmap::IMU(cv::Vec4d(0,0,0,0), cv::Mat(),
+			cv::Vec3d(0,0,0), cv::Mat(), cv::Vec3d(0,0,9.8), cv::Mat(), rtabmap::Transform::getIdentity()));
+	EXPECT_EQ(noOrientation.samples(), 0u);
+}
+
+TEST(ImuMotionPredictor, predicts_the_same_whatever_the_order_samples_and_predictions_come_in)
+{
+	// The integration since the last pose is kept between predictions: it must not go
+	// stale when samples arrive after a prediction, or out of order. Compared with a
+	// predictor given every sample before predicting anything.
+	auto acceleration = [](double t) { return Eigen::Vector3d(std::sin(20*t), std::cos(15*t), 0.3*t); };
+	auto orientation = [](double t) { return yaw(0.5*t); };
+	auto imu = [&](double t) { return measuredImu(orientation(t), acceleration(t), 9.80665, rtabmap::Transform::getIdentity()); };
+
+	ImuMotionPredictor reference;
+	for(int i=0; i<=80; ++i) reference.addImu(i*0.005, imu(i*0.005));
+	reference.addPose(0.0, rtabmap::Transform::getIdentity());
+	reference.addPose(0.1, pose(Eigen::Vector3d(0.05, 0, 0), 0.05));
+
+	ImuMotionPredictor incremental;
+	for(int i=0; i<=40; ++i) if(i != 30) incremental.addImu(i*0.005, imu(i*0.005));
+	incremental.addPose(0.0, rtabmap::Transform::getIdentity());
+	incremental.addPose(0.1, pose(Eigen::Vector3d(0.05, 0, 0), 0.05)); // velocity needs up to 0.1: covered
+	incremental.predict(0.12);   // integrates without the sample at 0.15
+	incremental.predict(0.3);    // beyond the newest sample (0.2)
+	incremental.addImu(0.15, imu(0.15));   // out of order
+	for(int i=41; i<=80; ++i)
+	{
+		incremental.addImu(i*0.005, imu(i*0.005));
+		if(i % 7 == 0) incremental.predict(i*0.005 - 0.0012);
+	}
+
+	for(double t : {0.1, 0.1013, 0.15, 0.2337, 0.4, 0.45})
+	{
+		const rtabmap::Transform a = reference.predict(t);
+		const rtabmap::Transform b = incremental.predict(t);
+		ASSERT_FALSE(a.isNull());
+		ASSERT_FALSE(b.isNull());
+		EXPECT_NEAR(a.x(), b.x(), 1e-6) << "t=" << t;
+		EXPECT_NEAR(a.y(), b.y(), 1e-6) << "t=" << t;
+		EXPECT_NEAR(a.z(), b.z(), 1e-6) << "t=" << t;
+	}
+}
+
+TEST(ImuMotionPredictor, holds_the_acceleration_at_the_pose_until_a_newer_sample)
+{
+	// The pose comes after the newest sample: the acceleration there is held from that
+	// sample, until a newer one says otherwise.
+	ImuMotionPredictor reference;
+	ImuMotionPredictor incremental;
+	auto imu = [](double a) { return measuredImu(Eigen::Quaterniond::Identity(), Eigen::Vector3d(a, 0, 0), 9.80665, rtabmap::Transform::getIdentity()); };
+	for(ImuMotionPredictor * p : {&reference, &incremental})
+	{
+		p->addImu(0.0, imu(0.0));
+		p->addImu(0.1, imu(0.0));
+		p->addPose(0.15, rtabmap::Transform::getIdentity());
+	}
+	incremental.predict(0.2);
+	for(ImuMotionPredictor * p : {&reference, &incremental})
+	{
+		p->addImu(0.2, imu(4.0));
+		p->addImu(0.3, imu(4.0));
+	}
+	EXPECT_NEAR(reference.predict(0.3).x(), incremental.predict(0.3).x(), 1e-9);
+}
+
+TEST(ImuMotionPredictor, ignores_the_acceleration_without_a_velocity_window)
+{
+	// From rest with a constant 2 m/s^2: with a window of 0, the velocity is the one of the
+	// last interval and is kept constant, as without IMU.
+	const double a = 2.0;
+	ImuMotionPredictor predictor(1.0, 0.0);
+	addSamples(predictor, -0.05, 0.3,
+			[](double) { return Eigen::Quaterniond::Identity(); },
+			[&](double t) { return Eigen::Vector3d(t < 0.0 ? 0.0 : a, 0, 0); });
+	predictor.addPose(0.0, pose(Eigen::Vector3d::Zero(), 0));
+	predictor.addPose(0.1, pose(Eigen::Vector3d(0.5*a*0.01, 0, 0), 0));
+	EXPECT_NEAR(predictor.velocity().x(), 0.5*a*0.1, 1e-6);
+	EXPECT_NEAR(predictor.predict(0.2).x(), 0.5*a*0.01 + 0.5*a*0.1*0.1, 1e-6);
+}

corelib/test/test_odometry.cpp

@@ -11,6 +11,11 @@
 #include <opencv2/core.hpp>
 #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>
 #include <string>
 
 using namespace rtabmap;
@@ -584,3 +589,261 @@ TEST(OdometryTest, RefusesAFirstScanTooSmallForTheCorrespondenceRatio)
 	EXPECT_FALSE(unchecked->process(uncheckedData).isNull())
 			<< "a scan of unknown sweep size was refused";
 }
+
+// ---------------------------------------------------------------------------
+// Lidar deskewing with an IMU (Odom/Deskewing): a sensor moving in a box-shaped room,
+// whose scans are generated point by point from where the sensor was at each point's
+// time, as a spinning lidar measures them.
+// ---------------------------------------------------------------------------
+
+namespace {
+
+const Eigen::Vector3d kRoomMin(-6.0, -4.0, -1.5);
+const Eigen::Vector3d kRoomMax(7.0, 5.0, 2.5);
+const double kSweep = 0.1;     // s, first column to last
+const int kRings = 16;
+const int kColumns = 512;
+const double kGravity = 9.80665;
+
+/// Pose of the sensor at time t, and its acceleration (world frame).
+struct Trajectory
+{
+	double yaw = 0.0;              // rad, heading at t=0
+	double yawRate = 0.0;          // rad/s
+	double acceleration = 0.0;     // m/s^2 along x, from rest at t=0
+	Transform pose(double t) const
+	{
+		return Transform(float(0.5*acceleration*t*t), 0, 0, 0, 0, float(yaw + yawRate*t));
+	}
+	Eigen::Vector3d linearAcceleration() const { return Eigen::Vector3d(acceleration, 0, 0); }
+};
+
+/// The scan of a sweep starting at @p stamp: organized (rings x columns), time on columns.
+LaserScan makeSweep(const Trajectory & trajectory, double stamp)
+{
+	cv::Mat data(kRings, kColumns, CV_32FC(5));
+	for(int u=0; u<kColumns; ++u)
+	{
+		const double dt = kSweep * double(u) / double(kColumns-1);
+		const Transform pose = trajectory.pose(stamp + dt);
+		const Eigen::Matrix3d rotation = pose.toEigen3d().linear();
+		const Eigen::Vector3d origin(pose.x(), pose.y(), pose.z());
+		const double azimuth = 2.0*M_PI*double(u)/double(kColumns);
+		for(int v=0; v<kRings; ++v)
+		{
+			const double elevation = (-15.0 + 30.0*double(v)/double(kRings-1)) * M_PI / 180.0;
+			const Eigen::Vector3d direction(std::cos(elevation)*std::cos(azimuth), std::cos(elevation)*std::sin(azimuth), std::sin(elevation));
+			const Eigen::Vector3d world = rotation * direction;
+			// Distance to the wall the ray hits
+			double range = std::numeric_limits<double>::max();
+			for(int i=0; i<3; ++i)
+			{
+				if(world[i] > 1e-9) range = std::min(range, (kRoomMax[i] - origin[i]) / world[i]);
+				else if(world[i] < -1e-9) range = std::min(range, (kRoomMin[i] - origin[i]) / world[i]);
+			}
+			float * p = data.ptr<float>(v, u);
+			p[0] = float(range*direction.x());
+			p[1] = float(range*direction.y());
+			p[2] = float(range*direction.z());
+			p[3] = 1.0f;
+			p[4] = float(dt);
+		}
+	}
+	return LaserScan(data, kRings*kColumns, 0.0f, LaserScan::kXYZIT, Transform::getIdentity());
+}
+
+/// What the IMU, at the sensor's origin, measures at time t.
+IMU makeImu(const Trajectory & trajectory, double t)
+{
+	const Eigen::Quaterniond q = trajectory.pose(t).getQuaterniond();
+	const Eigen::Vector3d f = q.inverse() * (trajectory.linearAcceleration() + Eigen::Vector3d(0, 0, kGravity));
+	return IMU(cv::Vec4d(q.x(), q.y(), q.z(), q.w()), cv::Mat::eye(3,3,CV_64FC1),
+			cv::Vec3d(0, 0, trajectory.yawRate), cv::Mat::eye(3,3,CV_64FC1),
+			cv::Vec3d(f.x(), f.y(), f.z()), cv::Mat::eye(3,3,CV_64FC1),
+			Transform::getIdentity());
+}
+
+/// RMS distance (m) of the scan's points to the nearest wall, with the sensor at @p pose.
+double wallDistance(const LaserScan & scan, const Transform & pose)
+{
+	double sum = 0.0;
+	int n = 0;
+	for(int i=0; i<scan.size(); ++i)
+	{
+		const float * p = scan.data().ptr<float>(0, i);
+		const Transform world = pose * Transform(p[0], p[1], p[2], 0, 0, 0);
+		const Eigen::Vector3d w(world.x(), world.y(), world.z());
+		double d = std::numeric_limits<double>::max();
+		for(int k=0; k<3; ++k)
+		{
+			d = std::min(d, std::fabs(w[k] - kRoomMin[k]));
+			d = std::min(d, std::fabs(w[k] - kRoomMax[k]));
+		}
+		sum += d*d;
+		++n;
+	}
+	return n?std::sqrt(sum/n):0.0;
+}
+
+/**
+ * Feeds @p frames sweeps, one every 0.1 s from t=0.1, with the IMU at 200 Hz up to the end
+ * of each sweep (as OdometryThread does), and returns the RMS wall distance of the last
+ * sweep as odometry left it in the frame (deskewed or not).
+ */
+double deskewedWallDistance(const Trajectory & trajectory, int frames, const ParametersMap & extra)
+{
+	ParametersMap parameters;
+	parameters.insert(ParametersPair(Parameters::kRegStrategy(), "1"));
+	parameters.insert(ParametersPair(Parameters::kIcpVoxelSize(), "0"));
+	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"));
+	for(const ParametersPair & p : extra) uInsert(parameters, p);
+	std::unique_ptr<Odometry> odometry(Odometry::create(parameters));
+
+	double imuStamp = 0.0;
+	double distance = -1.0;
+	for(int i=1; i<=frames; ++i)
+	{
+		const double stamp = 0.1*i;
+		for(; imuStamp <= stamp + kSweep + 0.005; imuStamp += 0.005)
+		{
+			SensorData imu(makeImu(trajectory, imuStamp), 0, imuStamp);
+			odometry->process(imu);
+		}
+		SensorData data(makeSweep(trajectory, stamp), cv::Mat(), cv::Mat(), CameraModel(), i, stamp);
+		OdometryInfo info;
+		const Transform pose = odometry->process(data, &info);
+		EXPECT_FALSE(pose.isNull()) << "frame " << i << " not registered";
+		distance = wallDistance(data.laserScanRaw(), trajectory.pose(stamp));
+	}
+	return distance;
+}
+
+}  // namespace
+
+TEST(OdometryTest, DeskewsWithTheImuOrientation)
+{
+	// Turning at 2 rad/s: 11 degrees over a sweep, so the far walls are smeared by about a
+	// metre. On the first frame, before any pose, only the IMU orientation is used.
+	Trajectory turning;
+	turning.yawRate = 2.0;
+	ParametersMap off;
+	off.insert(ParametersPair(Parameters::kOdomDeskewing(), "false"));
+	const double skewed = deskewedWallDistance(turning, 1, off);
+	const double deskewed = deskewedWallDistance(turning, 1, ParametersMap());
+	EXPECT_GT(skewed, 0.1) << "the scan should be smeared without deskewing";
+	EXPECT_LT(deskewed, 0.01) << "the points should be back on the walls";
+}
+
+TEST(OdometryTest, DeskewsWithTheImuAccelerationOverTheSmoothingDelay)
+{
+	// Accelerating at 2 m/s^2 from rest. With Odom/GuessSmoothingDelay, the velocity is
+	// carried to each frame with the IMU acceleration, and the prediction integrates it
+	// over the sweep. At 0, the acceleration is not used: the velocity of the last
+	// interval lags behind, and the sweep is still skewed.
+	Trajectory accelerating;
+	accelerating.acceleration = 2.0;
+	// Not facing exactly along x: an IMU orientation whose x, y and z are all zero (the
+	// identity) is read by RTAB-Map's odometry as "not set", and the IMU ignored.
+	accelerating.yaw = 0.3;
+	ParametersMap noDelay;
+	noDelay.insert(ParametersPair(Parameters::kOdomGuessSmoothingDelay(), "0"));
+	ParametersMap delay;
+	delay.insert(ParametersPair(Parameters::kOdomGuessSmoothingDelay(), "0.5"));
+	const double withoutAcceleration = deskewedWallDistance(accelerating, 12, noDelay);
+	const double withAcceleration = deskewedWallDistance(accelerating, 12, delay);
+	EXPECT_LT(withAcceleration, 0.005) << "the points should be back on the walls";
+	EXPECT_GT(withoutAcceleration, withAcceleration * 3.0);
+}
+
+TEST(OdometryTest, InitialOrientationIsTheImuOrientationAtTheFirstFrame)
+{
+	// IMU received for 2 s before the first frame, while the sensor turns at 1 rad/s: the
+	// first frame must start from the orientation at its own stamp, not at the first IMU
+	// sample (2 rad earlier).
+	Trajectory turning;
+	turning.yaw = 0.3;
+	turning.yawRate = 1.0;
+	ParametersMap parameters;
+	parameters.insert(ParametersPair(Parameters::kRegStrategy(), "1"));
+	parameters.insert(ParametersPair(Parameters::kIcpVoxelSize(), "0"));
+	std::unique_ptr<Odometry> odometry(Odometry::create(parameters));
+	const double stamp = 2.0;
+	for(double t = 0.0; t <= stamp + kSweep + 0.005; t += 0.005)
+	{
+		SensorData imu(makeImu(turning, t), 0, t);
+		odometry->process(imu);
+	}
+	SensorData data(makeSweep(turning, stamp), cv::Mat(), cv::Mat(), CameraModel(), 1, stamp);
+	OdometryInfo info;
+	const Transform pose = odometry->process(data, &info);
+	ASSERT_FALSE(pose.isNull());
+	// The IMU was fed up to the end of the sweep (as OdometryThread does), but the orientation
+	// is interpolated at the frame's stamp
+	EXPECT_NEAR(pose.theta(), turning.yaw + turning.yawRate*stamp, 0.002);
+
+	// An initial pose given with a rotation is kept
+	std::unique_ptr<Odometry> given(Odometry::create(parameters));
+	given->reset(Transform(0, 0, 0, 0, 0, 1.0f));
+	for(double t = 0.0; t <= stamp; t += 0.005)
+	{
+		SensorData imu(makeImu(turning, t), 0, t);
+		given->process(imu);
+	}
+	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";
+}

corelib/test/test_util3d.cpp

@@ -2114,3 +2114,41 @@ TEST(Util3dTest, DeskewValidScan) {
     // empty scan
     EXPECT_TRUE(util3d::deskew(LaserScan(), inputStamp, velocity).empty());
 }
+
+TEST(Util3dTest, DeskewWithMotion) {
+    // Three points measured at y=0, 1 s before, at and 1 s after the scan stamp, while the
+    // base moves along y as (t-stamp)^2: a motion no constant velocity describes.
+    cv::Mat data = cv::Mat::zeros(1, 3, CV_32FC(5));
+    float * dataPtr = (float*)data.data;
+    dataPtr[0] = 1;  dataPtr[4] = -1;
+    dataPtr[5] = 1;  dataPtr[9] = 0;
+    dataPtr[10] = 1; dataPtr[14] = 1;
+    LaserScan scan(data, 3, 10.0f, LaserScan::kXYZIT);
+    const double inputStamp = 1000.0;
+
+    std::vector<double> stamps;
+    auto motion = [&](double stamp) {
+        stamps.push_back(stamp);
+        const double dt = stamp - inputStamp;
+        return Transform(0.0, dt*dt, 0.0, 0.0, 0.0, 0.0);
+    };
+
+    // Asked for every point time, each point is moved by its own pose
+    LaserScan result = util3d::deskew(scan, inputStamp, motion);
+    ASSERT_EQ(result.size(), 3);
+    EXPECT_EQ(stamps.size(), 3u);
+    EXPECT_FLOAT_EQ(result.field(0, 1), 1.0f);
+    EXPECT_FLOAT_EQ(result.field(1, 1), 0.0f);
+    EXPECT_FLOAT_EQ(result.field(2, 1), 1.0f);
+
+    // With slerp, only the ends are asked for, the middle point is interpolated between them
+    stamps.clear();
+    result = util3d::deskew(scan, inputStamp, motion, true);
+    ASSERT_EQ(result.size(), 3);
+    EXPECT_EQ(stamps.size(), 2u);
+    EXPECT_FLOAT_EQ(result.field(1, 1), 1.0f);
+
+    // A failing motion, or none
+    EXPECT_TRUE(util3d::deskew(scan, inputStamp, [](double) { return Transform(); }).empty());
+    EXPECT_TRUE(util3d::deskew(scan, inputStamp, std::function<Transform(double)>()).empty());
+}

guilib/src/PreferencesDialog.cpp

@@ -1504,6 +1504,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->odom_flow_scanKeyframeThr->setObjectName(Parameters::kOdomScanKeyFrameThr().c_str());
 	_ui->odom_flow_guessMotion->setObjectName(Parameters::kOdomGuessMotion().c_str());
 	_ui->odom_guess_smoothing_delay->setObjectName(Parameters::kOdomGuessSmoothingDelay().c_str());
+	_ui->odom_imu_gravity->setObjectName(Parameters::kOdomImuGravity().c_str());
 	_ui->odom_imageDecimation->setObjectName(Parameters::kOdomImageDecimation().c_str());
 	_ui->odom_alignWithGround->setObjectName(Parameters::kOdomAlignWithGround().c_str());
 	_ui->odom_lidar_deskewing->setObjectName(Parameters::kOdomDeskewing().c_str());
@@ -1516,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

@@ -16427,7 +16427,7 @@ With <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_232">
                         <property name="text">
                          <string>Data buffer size (0 means inf).</string>
@@ -16570,7 +16570,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                         </property>
                        </widget>
                       </item>
-                      <item row="12" column="0">
+                      <item row="13" column="0">
                        <widget class="QDoubleSpinBox" name="odom_flow_keyframeThr">
                         <property name="maximum">
                          <double>1.000000000000000</double>
@@ -16590,7 +16590,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="QSpinBox" name="odom_VisKeyFrameThr">
                         <property name="maximum">
                          <number>9999</number>
@@ -16600,7 +16600,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_246">
                         <property name="text">
                          <string>[Geometry] Create a new keyframe when the number of inliers drops under this threshold. Setting value to 0 means that a keyframe is created for each processed frame.</string>
@@ -16613,7 +16613,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_248">
                         <property name="text">
                          <string>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 Visual Registration -&gt; Visual Feature -&gt; Depth as Mask is true and if depth is smaller than decimated RGB, depth may be interpolated to match RGB size for feature detection.</string>
@@ -16709,7 +16709,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                         </property>
                        </widget>
                       </item>
-                      <item row="15" column="0">
+                      <item row="16" column="0">
                        <widget class="QPushButton" name="pushButton_testOdometry">
                         <property name="text">
                          <string>Test odometry</string>
@@ -16729,14 +16729,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="QSpinBox" name="odom_dataBufferSize">
                         <property name="maximum">
                          <number>999999</number>
                         </property>
                        </widget>
                       </item>
-                      <item row="12" column="1">
+                      <item row="13" column="1">
                        <widget class="QLabel" name="label_196">
                         <property name="text">
                          <string>[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.</string>
@@ -16749,7 +16749,7 @@ 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_imageDecimation">
                         <property name="minimum">
                          <number>1</number>
@@ -16778,7 +16778,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="QDoubleSpinBox" name="odom_flow_scanKeyframeThr">
                         <property name="maximum">
                          <double>1.000000000000000</double>
@@ -16794,7 +16794,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                       <item row="8" column="1">
                        <widget class="QLabel" name="label_520">
                         <property name="text">
-                         <string>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 a filtering strategy is set or the delay is below the odometry rate.</string>
+                         <string>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 a filtering strategy is set or the delay is below the odometry rate. With an IMU giving orientation and linear acceleration, a delay &gt; 0 also enables the IMU acceleration: the velocity is then corrected by the acceleration measured since, so that it is the velocity at the last frame rather than a delayed average, and the motion guess and lidar deskewing integrate the acceleration from it. Recommended (~0.5 s) with an IMU; with 0, the IMU only gives the orientation. Without IMU, the average is delayed by half this delay: it can make sense for platforms with inertia (e.g., cars at high speed, where one bad frame would otherwise change the predicted velocity a lot), for high frame rates, or when the velocity is used for lidar deskewing, where its noise would feed back into the next poses.</string>
                         </property>
                         <property name="wordWrap">
                          <bool>true</bool>
@@ -16831,7 +16831,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_354">
                         <property name="text">
                          <string>[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.</string>
@@ -16844,10 +16844,10 @@ With &lt;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_7461">
                         <property name="text">
-                         <string>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).</string>
+                         <string>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 the guess smoothing delay is &gt; 0). Without IMU, with a constant motion model (or the guess if provided).</string>
                         </property>
                         <property name="wordWrap">
                          <bool>true</bool>
@@ -16857,13 +16857,45 @@ 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="QCheckBox" name="odom_lidar_deskewing">
                         <property name="text">
                          <string/>
                         </property>
                        </widget>
                       </item>
+                      <item row="9" column="0">
+                       <widget class="QDoubleSpinBox" name="odom_imu_gravity">
+                        <property name="suffix">
+                         <string> m/s²</string>
+                        </property>
+                        <property name="decimals">
+                         <number>5</number>
+                        </property>
+                        <property name="maximum">
+                         <double>100.000000000000000</double>
+                        </property>
+                        <property name="singleStep">
+                         <double>0.010000000000000</double>
+                        </property>
+                        <property name="value">
+                         <double>9.806650000000000</double>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="9" column="1">
+                       <widget class="QLabel" name="label_odom_imu_gravity">
+                        <property name="text">
+                         <string>Gravity magnitude removed from the IMU linear acceleration (used with a guess smoothing delay &gt; 0 above). 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.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
                      </layout>
                     </item>
                     <item>
@@ -16920,7 +16952,7 @@ With &lt;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>
@@ -16933,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>
@@ -16962,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>
@@ -16975,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>
@@ -17044,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/>
@@ -17066,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>
@@ -17111,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>
@@ -17124,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>
@@ -17137,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>
@@ -17176,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/>
@@ -17208,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>
@@ -17247,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>
@@ -17260,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>
@@ -17300,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/>
@@ -17322,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>

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>rtabmap</name>
-  <version>0.24.0</version>
+  <version>0.24.1</version>
   <description>RTAB-Map's standalone library. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
   <maintainer email="[email protected]">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>

tools/CMakeLists.txt

@@ -17,7 +17,6 @@ ADD_SUBDIRECTORY( Info )
 ADD_SUBDIRECTORY( CleanupLocalGrids )
 ADD_SUBDIRECTORY( GlobalBundleAdjustment )
 ADD_SUBDIRECTORY( ReduceGraph )
-ADD_SUBDIRECTORY( LidarCameraCalibration )
 
 IF(OPENCV_NONFREE_FOUND)
 ADD_SUBDIRECTORY( VocabularyComparison )

tools/LidarCameraCalibration/CMakeLists.txt

@@ -1,33 +0,0 @@
-
-ADD_EXECUTABLE(lidarCameraCalibration
-  main.cpp
-  CalibrationProblem.cpp
-  CorrectionSolver.cpp
-  PatternSearchSolver.cpp
-  DownhillSimplexSolver.cpp
-  G2oSolver.cpp
-  GtsamSolver.cpp)
-  
-TARGET_LINK_LIBRARIES(lidarCameraCalibration rtabmap_core)
-
-# For the g2o solver: rtabmap_core links g2o privately.
-IF(G2O_FOUND)
-  IF(g2o_FOUND)
-    TARGET_LINK_LIBRARIES(lidarCameraCalibration g2o::core g2o::solver_eigen)
-  ELSE()
-    TARGET_INCLUDE_DIRECTORIES(lidarCameraCalibration PRIVATE ${G2O_INCLUDE_DIRS})
-    TARGET_LINK_LIBRARIES(lidarCameraCalibration ${G2O_LIBRARIES})
-  ENDIF()
-ENDIF(G2O_FOUND)
-
-# For the GTSAM solver: rtabmap_core links GTSAM privately.
-IF(GTSAM_FOUND)
-  TARGET_LINK_LIBRARIES(lidarCameraCalibration gtsam)
-ENDIF(GTSAM_FOUND)
-
-SET_TARGET_PROPERTIES( lidarCameraCalibration 
-  PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-lidarCameraCalibration)
-
-INSTALL(TARGETS lidarCameraCalibration
-		RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
-		BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime)

tools/LidarCameraCalibration/CalibrationProblem.cpp

@@ -1,130 +0,0 @@
-/*
-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.
-*/
-
-#include "CalibrationProblem.h"
-
-#include <rtabmap/core/util3d_transforms.h>
-
-#include <algorithm>
-#include <cmath>
-
-using namespace rtabmap;
-
-Transform correctionFrom(const double p[6])
-{
-	return Transform(p[0], p[1], p[2], p[3]*M_PI/180.0, p[4]*M_PI/180.0, p[5]*M_PI/180.0);
-}
-
-Eigen::Isometry3d correctionFromDouble(const double p[6])
-{
-	// As Transform(x, y, z, roll, pitch, yaw): yaw * pitch * roll.
-	Eigen::Isometry3d C = Eigen::Isometry3d::Identity();
-	C.translation() = Eigen::Vector3d(p[0], p[1], p[2]);
-	C.linear() = (Eigen::AngleAxisd(p[5]*M_PI/180.0, Eigen::Vector3d::UnitZ()) *
-			Eigen::AngleAxisd(p[4]*M_PI/180.0, Eigen::Vector3d::UnitY()) *
-			Eigen::AngleAxisd(p[3]*M_PI/180.0, Eigen::Vector3d::UnitX())).toRotationMatrix();
-	return C;
-}
-
-CalibrationProblem::CalibrationProblem(const std::vector<Frame> & frames, const std::vector<int> & nodes, float minDepth) :
-	frames_(frames), nodes_(nodes), minDepth_(minDepth), evaluations_(0)
-{
-}
-
-void CalibrationProblem::project(const Transform & C,
-		const std::function<void(const Frame &, size_t, float, float)> & visit) const
-{
-	for(int k : nodes_)
-	{
-		const Frame & f = frames_[k];
-		const Transform scanInCam = (f.scanToCam * C).inverse();
-		const double fx = f.K.at<double>(0,0), fy = f.K.at<double>(1,1);
-		const double cx = f.K.at<double>(0,2), cy = f.K.at<double>(1,2);
-		for(size_t i = 0; i < f.edgePoints.size(); ++i)
-		{
-			const cv::Point3f pc = util3d::transformPoint(f.edgePoints[i], scanInCam);
-			if(pc.z < minDepth_)
-			{
-				continue;
-			}
-			const float u = fx * pc.x / pc.z + cx, v = fy * pc.y / pc.z + cy;
-			if(u < 0 || v < 0 || u >= f.gray.cols - 1 || v >= f.gray.rows - 1)
-			{
-				continue;
-			}
-			visit(f, i, u, v);
-		}
-	}
-}
-
-double CalibrationProblem::score(const Transform & C) const
-{
-	++evaluations_;
-	double sum = 0.0;
-	project(C, [&](const Frame & f, size_t i, float u, float v) {
-		const int u0 = int(u), v0 = int(v);
-		const float a = u - u0, b = v - v0;
-		const float s =
-				(1-a)*(1-b)*f.edgeScore.at<float>(v0, u0) + a*(1-b)*f.edgeScore.at<float>(v0, u0+1) +
-				(1-a)*b*f.edgeScore.at<float>(v0+1, u0) + a*b*f.edgeScore.at<float>(v0+1, u0+1);
-		sum += f.edgeWeights[i] * s;
-	});
-	// The frames' edge points are selected again between passes: not cached.
-	double weights = 0.0;
-	for(int k : nodes_)
-	{
-		for(float w : frames_[k].edgeWeights)
-		{
-			weights += w;
-		}
-	}
-	return weights > 0.0 ? sum / weights : 0.0;
-}
-
-double CalibrationProblem::edgeDistance(const Frame & f, size_t i, const double p[6], double cap) const
-{
-	const cv::Point3f & pt = f.edgePoints[i];
-	const Eigen::Vector3d pc =
-			(f.scanToCam.toEigen3d() * correctionFromDouble(p)).inverse() * Eigen::Vector3d(pt.x, pt.y, pt.z);
-	if(pc.z() < minDepth_)
-	{
-		return cap;
-	}
-	const double u = f.K.at<double>(0,0) * pc.x() / pc.z() + f.K.at<double>(0,2);
-	const double v = f.K.at<double>(1,1) * pc.y() / pc.z() + f.K.at<double>(1,2);
-	if(u < 0 || v < 0 || u >= f.edgeDistance.cols - 1 || v >= f.edgeDistance.rows - 1)
-	{
-		return cap;
-	}
-	const int u0 = int(u), v0 = int(v);
-	const double a = u - u0, b = v - v0;
-	const cv::Mat & d = f.edgeDistance;
-	const double distance =
-			(1-a)*(1-b)*d.at<float>(v0, u0) + a*(1-b)*d.at<float>(v0, u0+1) +
-			(1-a)*b*d.at<float>(v0+1, u0) + a*b*d.at<float>(v0+1, u0+1);
-	return std::min(distance, cap);
-}

tools/LidarCameraCalibration/CalibrationProblem.h

@@ -1,113 +0,0 @@
-/*
-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 LIDARCAMERACALIBRATION_CALIBRATIONPROBLEM_H_
-#define LIDARCAMERACALIBRATION_CALIBRATIONPROBLEM_H_
-
-#include <rtabmap/core/Transform.h>
-
-#include <opencv2/core/core.hpp>
-#include <Eigen/Geometry>
-
-#include <functional>
-#include <vector>
-
-// What makes a lidar point an edge point, in order of precedence.
-enum EdgeType
-{
-	kEdgeDepth = 0,      // in front of a depth discontinuity
-	kEdgeCrease = 1,     // where the surface's orientation changes (e.g., wall and floor)
-	kEdgeIntensity = 2   // where the surface's reflectance changes
-};
-
-// One node of the database: its image, its lidar scan, and the lidar's edges.
-struct Frame
-{
-	int id;
-	cv::Mat gray;          // as stored, matching the camera model
-	cv::Mat edges;         // CV_8U: the image's edges (Canny), non-zero on an edge
-	cv::Mat edgeDistance;  // CV_32F: distance (pixels) to the image's nearest edge
-	cv::Mat edgeScore;     // CV_32F: 1 on the image's edges, decaying with the distance to them
-	cv::Mat K;             // CV_64F 3x3
-	rtabmap::Transform scanToCam;  // camera pose in the scan frame, before correction
-	cv::Mat cloud;         // Nx4 CV_32F, scan frame: x y z intensity (0 without intensity)
-	cv::Mat normals;       // Mx6 CV_32F, scan frame: voxelized points and their normals (for creases)
-	bool hasIntensity;
-	// The robot's speeds (m/s, deg/s; negative if unknown): instantaneous, odometry's when the
-	// node was added, and mean along odometry over the meanWindow (s) since the previous
-	// node, which is when an assembled scan was taken.
-	float linearSpeed = -1.0f, angularSpeed = -1.0f;
-	float meanLinearSpeed = -1.0f, meanAngularSpeed = -1.0f, meanWindow = 0.0f;
-	int meanPoses = 0;  // odometry steps the mean speed is from: more than 1 with intermediate nodes
-	std::vector<cv::Point3f> edgePoints;  // depth and intensity edge points, scan frame
-	std::vector<float> edgeWeights;
-	std::vector<unsigned char> edgeTypes; // EdgeType of each edge point
-};
-
-// The correction of parameters p = tx ty tz (m), rx ry rz (deg), in the camera frame.
-rtabmap::Transform correctionFrom(const double p[6]);
-// The same, in double precision: rtabmap's Transform is float, too coarse for the small
-// steps of numerical derivatives.
-Eigen::Isometry3d correctionFromDouble(const double p[6]);
-
-// The lidar edge points of some nodes, and how well a candidate correction C lines them
-// up with their images' edges. This is all a solver sees of the calibration.
-class CalibrationProblem
-{
-public:
-	// The frames' edge points are read at each call, so they can be selected again after
-	// the problem is made. minDepth: lidar points closer than this to the camera (m) are
-	// ignored.
-	CalibrationProblem(const std::vector<Frame> & frames, const std::vector<int> & nodes, float minDepth);
-
-	const std::vector<Frame> & frames() const {return frames_;}
-	const std::vector<int> & nodes() const {return nodes_;}
-	float minDepth() const {return minDepth_;}
-
-	// Calls visit(frame, point index, u, v) for each edge point that C projects in its
-	// image, at full resolution.
-	void project(const rtabmap::Transform & C,
-			const std::function<void(const Frame &, size_t, float, float)> & visit) const;
-
-	// What the direct solvers maximize: the weighted mean of the image edges' score under
-	// the projected lidar edge points, in [0, 1]. A point out of its image counts as 0.
-	double score(const rtabmap::Transform & C) const;
-	int evaluations() const {return evaluations_;}
-
-	// What the least-squares solvers minimize, per point: the distance (pixels) from
-	// where p projects edge point i of f to f's nearest image edge, capped; the cap where
-	// it does not project in the image. In double precision, for numerical derivatives.
-	double edgeDistance(const Frame & f, size_t i, const double p[6], double cap) const;
-
-private:
-	const std::vector<Frame> & frames_;
-	std::vector<int> nodes_;
-	float minDepth_;
-	mutable int evaluations_;
-};
-
-#endif /* LIDARCAMERACALIBRATION_CALIBRATIONPROBLEM_H_ */

tools/LidarCameraCalibration/CorrectionSolver.cpp

@@ -1,74 +0,0 @@
-/*
-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.
-*/
-
-#include "CorrectionSolver.h"
-#include "DownhillSimplexSolver.h"
-#include "G2oSolver.h"
-#include "GtsamSolver.h"
-#include "PatternSearchSolver.h"
-
-#include <rtabmap/core/Version.h>
-
-const double kInitialSteps[6] = {0.04, 0.04, 0.04, 2.0, 2.0, 2.0};
-
-std::unique_ptr<CorrectionSolver> createSolver(const std::string & name, float sigma)
-{
-	(void)sigma;  // used only by the least-squares solvers
-	if(name == "pattern")
-	{
-		return std::unique_ptr<CorrectionSolver>(new PatternSearchSolver());
-	}
-	if(name == "simplex")
-	{
-		return std::unique_ptr<CorrectionSolver>(new DownhillSimplexSolver());
-	}
-#ifdef RTABMAP_G2O
-	if(name == "g2o")
-	{
-		return std::unique_ptr<CorrectionSolver>(new G2oSolver(sigma));
-	}
-#endif
-#ifdef RTABMAP_GTSAM
-	if(name == "gtsam")
-	{
-		return std::unique_ptr<CorrectionSolver>(new GtsamSolver(sigma));
-	}
-#endif
-	return std::unique_ptr<CorrectionSolver>();
-}
-
-std::vector<std::string> availableSolvers()
-{
-	std::vector<std::string> names = {"simplex", "pattern"};
-#ifdef RTABMAP_G2O
-	names.push_back("g2o");
-#endif
-#ifdef RTABMAP_GTSAM
-	names.push_back("gtsam");
-#endif
-	return names;
-}

tools/LidarCameraCalibration/CorrectionSolver.h

@@ -1,58 +0,0 @@
-/*
-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 LIDARCAMERACALIBRATION_CORRECTIONSOLVER_H_
-#define LIDARCAMERACALIBRATION_CORRECTIONSOLVER_H_
-
-#include "CalibrationProblem.h"
-
-#include <memory>
-#include <string>
-#include <vector>
-
-// Parameters: p = tx ty tz (m), rx ry rz (deg), in the camera frame (see correctionFrom()).
-// Initial search steps, also the scale of each parameter.
-extern const double kInitialSteps[6];
-
-// Finds the correction best aligning the problem's lidar edges with its images' edges.
-// A new solver implements this and is added to createSolver().
-class CorrectionSolver
-{
-public:
-	virtual ~CorrectionSolver() {}
-	virtual const char * name() const = 0;
-	// Improves p from its value. Without estimateTranslation, p's translation is left as is.
-	virtual void solve(const CalibrationProblem & problem, bool estimateTranslation, double p[6]) const = 0;
-};
-
-// The solver named "pattern", "simplex", "g2o" or "gtsam" (the last two if rtabmap was
-// built with them), or null. sigma: the image edges' fall off (pixels).
-std::unique_ptr<CorrectionSolver> createSolver(const std::string & name, float sigma);
-// The names createSolver() knows in this build.
-std::vector<std::string> availableSolvers();
-
-#endif /* LIDARCAMERACALIBRATION_CORRECTIONSOLVER_H_ */

tools/LidarCameraCalibration/DownhillSimplexSolver.cpp

@@ -1,95 +0,0 @@
-/*
-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.
-*/
-
-#include "DownhillSimplexSolver.h"
-
-#include <opencv2/core/optim.hpp>
-
-#include <algorithm>
-#include <vector>
-
-namespace {
-
-// The score to minimize, as a function of the solved parameters x; the others stay as
-// in p.
-class NegativeScore : public cv::MinProblemSolver::Function
-{
-public:
-	NegativeScore(const CalibrationProblem & problem, const std::vector<int> & solved, const double p[6]) :
-		problem_(problem), solved_(solved)
-	{
-		std::copy(p, p + 6, p_);
-	}
-	int getDims() const override {return (int)solved_.size();}
-	double calc(const double * x) const override
-	{
-		double q[6];
-		std::copy(p_, p_ + 6, q);
-		for(size_t i = 0; i < solved_.size(); ++i)
-		{
-			q[solved_[i]] = x[i];
-		}
-		return -problem_.score(correctionFrom(q));
-	}
-private:
-	const CalibrationProblem & problem_;
-	std::vector<int> solved_;
-	double p_[6];
-};
-
-}  // namespace
-
-void DownhillSimplexSolver::solve(const CalibrationProblem & problem, bool estimateTranslation, double p[6]) const
-{
-	std::vector<int> solved;
-	for(int k = estimateTranslation ? 0 : 3; k < 6; ++k)
-	{
-		solved.push_back(k);
-	}
-
-	cv::Ptr<cv::DownhillSolver> solver = cv::DownhillSolver::create(
-			cv::makePtr<NegativeScore>(problem, solved, p),
-			cv::noArray(),
-			cv::TermCriteria(cv::TermCriteria::MAX_ITER + cv::TermCriteria::EPS, 5000, 1e-9));
-	cv::Mat x(1, (int)solved.size(), CV_64F), step(1, (int)solved.size(), CV_64F);
-	for(size_t i = 0; i < solved.size(); ++i)
-	{
-		x.at<double>(0, i) = p[solved[i]];
-		step.at<double>(0, i) = kInitialSteps[solved[i]];
-	}
-	// Started again from its result: a simplex can shrink before it reaches the
-	// optimum, the restart gives it its full size back.
-	for(int restart = 0; restart < 2; ++restart)
-	{
-		solver->setInitStep(step);
-		solver->minimize(x);
-	}
-	for(size_t i = 0; i < solved.size(); ++i)
-	{
-		p[solved[i]] = x.at<double>(0, i);
-	}
-}

tools/LidarCameraCalibration/DownhillSimplexSolver.h

@@ -1,42 +0,0 @@
-/*
-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 LIDARCAMERACALIBRATION_DOWNHILLSIMPLEXSOLVER_H_
-#define LIDARCAMERACALIBRATION_DOWNHILLSIMPLEXSOLVER_H_
-
-#include "CorrectionSolver.h"
-
-// OpenCV's Nelder-Mead simplex: moves all the parameters together, so it follows
-// directions in which they are coupled.
-class DownhillSimplexSolver : public CorrectionSolver
-{
-public:
-	const char * name() const override {return "simplex";}
-	void solve(const CalibrationProblem & problem, bool estimateTranslation, double p[6]) const override;
-};
-
-#endif /* LIDARCAMERACALIBRATION_DOWNHILLSIMPLEXSOLVER_H_ */

tools/LidarCameraCalibration/G2oSolver.cpp

@@ -1,188 +0,0 @@
-/*
-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.
-*/
-
-#include "G2oSolver.h"
-
-#ifdef RTABMAP_G2O
-
-#include <g2o/core/base_unary_edge.h>
-#include <g2o/core/base_vertex.h>
-#include <g2o/core/block_solver.h>
-#include <g2o/core/optimization_algorithm_levenberg.h>
-#include <g2o/core/robust_kernel_impl.h>
-#include <g2o/core/sparse_optimizer.h>
-#include <g2o/solvers/eigen/linear_solver_eigen.h>
-
-#include <algorithm>
-
-namespace {
-
-// The solved parameters of p (the rotation, and the translation if estimated).
-template<int D>
-class VertexCorrection : public g2o::BaseVertex<D, Eigen::Matrix<double, D, 1> >
-{
-public:
-	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-	void setToOriginImpl() override {this->_estimate.setZero();}
-	void oplusImpl(const double * update) override  // as rtabmap's own g2o vertices
-	{
-		for(int i = 0; i < D; ++i)
-		{
-			this->_estimate[i] += update[i];
-		}
-	}
-	bool read(std::istream &) override {return false;}
-	bool write(std::ostream &) const override {return true;}
-};
-
-// One lidar edge point: CalibrationProblem::edgeDistance() for it.
-template<int D>
-class EdgeToImageEdge : public g2o::BaseUnaryEdge<1, double, VertexCorrection<D> >
-{
-public:
-	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-	EdgeToImageEdge(const CalibrationProblem & problem, const Frame & frame, size_t point,
-			const std::vector<int> & solved, const double p[6], double cap) :
-		problem_(problem), frame_(frame), point_(point), solved_(solved), cap_(cap)
-	{
-		std::copy(p, p + 6, p_);
-	}
-
-	void computeError() override
-	{
-		this->_error[0] = residual(static_cast<const VertexCorrection<D> *>(this->_vertices[0])->estimate());
-	}
-
-	// Central differences, with steps of the precision the projection needs rather than
-	// g2o's default (1e-9), lost in the image's distance map.
-	void linearizeOplus() override
-	{
-		const Eigen::Matrix<double, D, 1> x = static_cast<const VertexCorrection<D> *>(this->_vertices[0])->estimate();
-		for(int k = 0; k < D; ++k)
-		{
-			const double h = solved_[k] < 3 ? 1e-4 : 1e-3;  // m, deg
-			Eigen::Matrix<double, D, 1> plus = x, minus = x;
-			plus[k] += h;
-			minus[k] -= h;
-			this->_jacobianOplusXi(0, k) = (residual(plus) - residual(minus)) / (2.0 * h);
-		}
-	}
-
-	bool read(std::istream &) override {return false;}
-	bool write(std::ostream &) const override {return true;}
-
-private:
-	double residual(const Eigen::Matrix<double, D, 1> & x) const
-	{
-		double q[6];
-		std::copy(p_, p_ + 6, q);
-		for(int k = 0; k < D; ++k)
-		{
-			q[solved_[k]] = x[k];
-		}
-		return problem_.edgeDistance(frame_, point_, q, cap_);
-	}
-
-	const CalibrationProblem & problem_;
-	const Frame & frame_;
-	size_t point_;
-	std::vector<int> solved_;
-	double p_[6];
-	double cap_;
-};
-
-}  // namespace
-
-void G2oSolver::solve(const CalibrationProblem & problem, bool estimateTranslation, double p[6]) const
-{
-	coarse_.solve(problem, estimateTranslation, p);
-	if(estimateTranslation)
-	{
-		solveWith<6>(problem, {0, 1, 2, 3, 4, 5}, p);
-	}
-	else
-	{
-		solveWith<3>(problem, {3, 4, 5}, p);
-	}
-}
-
-template<int D>
-void G2oSolver::solveWith(const CalibrationProblem & problem, const std::vector<int> & solved, double p[6]) const
-{
-	typedef g2o::BlockSolver<g2o::BlockSolverTraits<D, 1> > Block;
-	typedef g2o::LinearSolverEigen<typename Block::PoseMatrixType> Linear;
-
-	Eigen::Matrix<double, D, 1> x;
-	for(int k = 0; k < D; ++k)
-	{
-		x[k] = p[solved[k]];
-	}
-	for(double scale : {9.0, 3.0, 1.0})
-	{
-		const double delta = scale * sigma_;
-		g2o::SparseOptimizer optimizer;
-#ifdef RTABMAP_G2O_CPP11
-		optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(
-				std::unique_ptr<Block>(new Block(std::unique_ptr<Linear>(new Linear())))));
-#else
-		optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(new Block(new Linear())));
-#endif
-		VertexCorrection<D> * vertex = new VertexCorrection<D>();
-		vertex->setEstimate(x);
-		vertex->setId(0);
-		optimizer.addVertex(vertex);
-
-		// Beyond a few times the kernel's scale, a point does not count anyway.
-		const double cap = 10.0 * delta;
-		int id = 1;
-		for(int k : problem.nodes())
-		{
-			const Frame & f = problem.frames()[k];
-			for(size_t i = 0; i < f.edgePoints.size(); ++i)
-			{
-				EdgeToImageEdge<D> * edge = new EdgeToImageEdge<D>(problem, f, i, solved, p, cap);
-				edge->setId(id++);
-				edge->setVertex(0, vertex);
-				edge->setMeasurement(0.0);
-				edge->setInformation(Eigen::Matrix<double, 1, 1>::Constant(f.edgeWeights[i]));
-				g2o::RobustKernelWelsch * kernel = new g2o::RobustKernelWelsch();
-				kernel->setDelta(delta);
-				edge->setRobustKernel(kernel);
-				optimizer.addEdge(edge);
-			}
-		}
-		optimizer.initializeOptimization();
-		optimizer.optimize(10);  // close already, after the coarse search
-		x = vertex->estimate();
-	}
-	for(int k = 0; k < D; ++k)
-	{
-		p[solved[k]] = x[k];
-	}
-}
-
-#endif

tools/LidarCameraCalibration/G2oSolver.h

@@ -1,65 +0,0 @@
-/*
-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 LIDARCAMERACALIBRATION_G2OSOLVER_H_
-#define LIDARCAMERACALIBRATION_G2OSOLVER_H_
-
-#include "CorrectionSolver.h"
-#include "PatternSearchSolver.h"
-
-#include <rtabmap/core/Version.h>
-
-#ifdef RTABMAP_G2O
-
-// Least squares with g2o: Levenberg-Marquardt on the distances of the lidar edge points
-// to the images' edges (chamfer matching), weighted by the points' weights.
-//
-// - A redescending robust kernel (Welsch) makes a point far from any image edge count for
-//   nothing, as the score does: many lidar edges have no counterpart in the image
-//   (speckle, surfaces the camera does not see the same way).
-// - Levenberg-Marquardt only follows the local slope, and each point is pulled toward its
-//   nearest image edge, often not its own when far from the solution: from a few degrees
-//   away, it stops in a local minimum. So a coarse pattern search gets close first, then
-//   the kernel's scale goes from wide to narrow (9, 3, then 1 x sigma).
-class G2oSolver : public CorrectionSolver
-{
-public:
-	G2oSolver(double sigma) : sigma_(sigma), coarse_(4) {}
-	const char * name() const override {return "g2o";}
-	void solve(const CalibrationProblem & problem, bool estimateTranslation, double p[6]) const override;
-
-private:
-	template<int D>
-	void solveWith(const CalibrationProblem & problem, const std::vector<int> & solved, double p[6]) const;
-
-	double sigma_;
-	PatternSearchSolver coarse_;  // steps of 2 down to 0.25 deg
-};
-
-#endif
-
-#endif /* LIDARCAMERACALIBRATION_G2OSOLVER_H_ */

tools/LidarCameraCalibration/GtsamSolver.cpp

@@ -1,159 +0,0 @@
-/*
-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.
-*/
-
-#include "GtsamSolver.h"
-
-#ifdef RTABMAP_GTSAM
-
-#include <gtsam/linear/NoiseModel.h>
-#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
-#include <gtsam/nonlinear/NonlinearFactor.h>
-#include <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <gtsam/nonlinear/Values.h>
-
-#include <algorithm>
-#include <cmath>
-
-namespace {
-
-// One lidar edge point: CalibrationProblem::edgeDistance() for it, as a function of the
-// solved parameters x of p.
-template<int D>
-class EdgeDistanceFactor : public gtsam::NoiseModelFactor1<Eigen::Matrix<double, D, 1> >
-{
-	typedef Eigen::Matrix<double, D, 1> X;
-public:
-	EdgeDistanceFactor(gtsam::Key key, const gtsam::SharedNoiseModel & model,
-			const CalibrationProblem & problem, const Frame & frame, size_t point,
-			const std::vector<int> & solved, const double p[6], double cap) :
-		gtsam::NoiseModelFactor1<X>(model, key),
-		problem_(problem), frame_(frame), point_(point), solved_(solved), cap_(cap)
-	{
-		std::copy(p, p + 6, p_);
-	}
-
-	gtsam::Vector evaluateError(const X & x,
-#if GTSAM_VERSION_NUMERIC >= 40300
-			gtsam::OptionalMatrixType H = OptionalNone) const override
-#else
-			boost::optional<gtsam::Matrix &> H = boost::none) const override
-#endif
-	{
-		if(H)
-		{
-			// Central differences, with steps of the precision the projection needs.
-			gtsam::Matrix J(1, D);
-			for(int k = 0; k < D; ++k)
-			{
-				const double h = solved_[k] < 3 ? 1e-4 : 1e-3;  // m, deg
-				X plus = x, minus = x;
-				plus[k] += h;
-				minus[k] -= h;
-				J(0, k) = (residual(plus) - residual(minus)) / (2.0 * h);
-			}
-			*H = J;
-		}
-		return gtsam::Vector1(residual(x));
-	}
-
-private:
-	double residual(const X & x) const
-	{
-		double q[6];
-		std::copy(p_, p_ + 6, q);
-		for(int k = 0; k < D; ++k)
-		{
-			q[solved_[k]] = x[k];
-		}
-		return problem_.edgeDistance(frame_, point_, q, cap_);
-	}
-
-	const CalibrationProblem & problem_;
-	const Frame & frame_;
-	size_t point_;
-	std::vector<int> solved_;
-	double p_[6];
-	double cap_;
-};
-
-}  // namespace
-
-void GtsamSolver::solve(const CalibrationProblem & problem, bool estimateTranslation, double p[6]) const
-{
-	coarse_.solve(problem, estimateTranslation, p);
-	if(estimateTranslation)
-	{
-		solveWith<6>(problem, {0, 1, 2, 3, 4, 5}, p);
-	}
-	else
-	{
-		solveWith<3>(problem, {3, 4, 5}, p);
-	}
-}
-
-template<int D>
-void GtsamSolver::solveWith(const CalibrationProblem & problem, const std::vector<int> & solved, double p[6]) const
-{
-	typedef Eigen::Matrix<double, D, 1> X;
-	const gtsam::Key key = 0;
-
-	X x;
-	for(int k = 0; k < D; ++k)
-	{
-		x[k] = p[solved[k]];
-	}
-	for(double scale : {9.0, 3.0, 1.0})
-	{
-		const double delta = scale * sigma_;
-		const double cap = 10.0 * delta;  // beyond a few times the scale, a point does not count anyway
-		const gtsam::noiseModel::mEstimator::Welsch::shared_ptr welsch =
-				gtsam::noiseModel::mEstimator::Welsch::Create(delta);
-		gtsam::NonlinearFactorGraph graph;
-		for(int k : problem.nodes())
-		{
-			const Frame & f = problem.frames()[k];
-			for(size_t i = 0; i < f.edgePoints.size(); ++i)
-			{
-				// Information = the point's weight, as with g2o.
-				const gtsam::SharedNoiseModel model = gtsam::noiseModel::Robust::Create(
-						welsch, gtsam::noiseModel::Isotropic::Sigma(1, 1.0 / std::sqrt(f.edgeWeights[i])));
-				graph.emplace_shared<EdgeDistanceFactor<D> >(key, model, problem, f, i, solved, p, cap);
-			}
-		}
-		gtsam::Values initial;
-		initial.insert(key, x);
-		gtsam::LevenbergMarquardtParams params;
-		params.setMaxIterations(10);  // close already, after the coarse search
-		x = gtsam::LevenbergMarquardtOptimizer(graph, initial, params).optimize().template at<X>(key);
-	}
-	for(int k = 0; k < D; ++k)
-	{
-		p[solved[k]] = x[k];
-	}
-}
-
-#endif

tools/LidarCameraCalibration/GtsamSolver.h

@@ -1,58 +0,0 @@
-/*
-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 LIDARCAMERACALIBRATION_GTSAMSOLVER_H_
-#define LIDARCAMERACALIBRATION_GTSAMSOLVER_H_
-
-#include "CorrectionSolver.h"
-#include "PatternSearchSolver.h"
-
-#include <rtabmap/core/Version.h>
-
-#ifdef RTABMAP_GTSAM
-
-// The same least squares as G2oSolver (see there), with GTSAM: one factor per lidar edge
-// point on the correction's parameters, a Welsch robust noise model, Levenberg-Marquardt
-// after a coarse pattern search, with the robust scale going from wide to narrow.
-class GtsamSolver : public CorrectionSolver
-{
-public:
-	GtsamSolver(double sigma) : sigma_(sigma), coarse_(4) {}
-	const char * name() const override {return "gtsam";}
-	void solve(const CalibrationProblem & problem, bool estimateTranslation, double p[6]) const override;
-
-private:
-	template<int D>
-	void solveWith(const CalibrationProblem & problem, const std::vector<int> & solved, double p[6]) const;
-
-	double sigma_;
-	PatternSearchSolver coarse_;  // steps of 2 down to 0.25 deg
-};
-
-#endif
-
-#endif /* LIDARCAMERACALIBRATION_GTSAMSOLVER_H_ */

tools/LidarCameraCalibration/PatternSearchSolver.cpp

@@ -1,73 +0,0 @@
-/*
-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.
-*/
-
-#include "PatternSearchSolver.h"
-
-#include <algorithm>
-
-void PatternSearchSolver::solve(const CalibrationProblem & problem, bool estimateTranslation, double p[6]) const
-{
-	double steps[6];
-	std::copy(kInitialSteps, kInitialSteps + 6, steps);
-	if(!estimateTranslation)
-	{
-		steps[0] = steps[1] = steps[2] = 0.0;
-	}
-	double best = problem.score(correctionFrom(p));
-	for(int level = 0; level < levels_; ++level)
-	{
-		bool improved = true;
-		while(improved)
-		{
-			improved = false;
-			for(int k = 0; k < 6; ++k)
-			{
-				if(steps[k] == 0.0)
-				{
-					continue;
-				}
-				for(int sign = -1; sign <= 1; sign += 2)
-				{
-					double q[6];
-					std::copy(p, p + 6, q);
-					q[k] += sign * steps[k];
-					const double s = problem.score(correctionFrom(q));
-					if(s > best + 1e-7)
-					{
-						best = s;
-						std::copy(q, q + 6, p);
-						improved = true;
-					}
-				}
-			}
-		}
-		for(double & s : steps)
-		{
-			s /= 2.0;
-		}
-	}
-}

tools/LidarCameraCalibration/PatternSearchSolver.h

@@ -1,47 +0,0 @@
-/*
-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 LIDARCAMERACALIBRATION_PATTERNSEARCHSOLVER_H_
-#define LIDARCAMERACALIBRATION_PATTERNSEARCHSOLVER_H_
-
-#include "CorrectionSolver.h"
-
-// One parameter at a time, trying a step each way, until no step improves, then with
-// half the steps. Simple and robust, but can only move along the parameters' axes.
-class PatternSearchSolver : public CorrectionSolver
-{
-public:
-	// levels: how many times the steps are halved, from 2 deg (7: down to about 0.03 deg)
-	PatternSearchSolver(int levels = 7) : levels_(levels) {}
-	const char * name() const override {return "pattern";}
-	void solve(const CalibrationProblem & problem, bool estimateTranslation, double p[6]) const override;
-
-private:
-	int levels_;
-};
-
-#endif /* LIDARCAMERACALIBRATION_PATTERNSEARCHSOLVER_H_ */

tools/LidarCameraCalibration/README.md

@@ -1,137 +0,0 @@
-# rtabmap-lidarCameraCalibration
-
-Refines the rotation between a camera and a lidar from a mapping session, without a calibration target. It aligns the edges the lidar sees (depth discontinuities, creases and reflectance changes) with the edges in the camera images, over all the nodes of an RTAB-Map database at once.
-
-```bash
-rtabmap-lidarCameraCalibration [options] map.db
-```
-
-The database is opened read-only. The tool prints a correction to apply to the camera's transform; it does not modify anything.
-
-## What the database needs
-
-Each node must have an image from one camera and a lidar scan taken at about the same time, with the camera's and the lidar's local transforms (from TF when mapping with ROS).
-
-What helps:
-
-- **Dense scans.** Edges are found where the scan, projected in the image, leaves no holes. An assembled cloud (a few lidar sweeps per node) at full resolution, or with a small voxel (2 cm), works better than a sparse one.
-- **Images and scans taken together.** The tool assumes that the camera's and the lidar's clocks are synchronized: it does not estimate a time offset between them, which would look like a rotation while the robot turns. With `odom_sensor_sync`, the camera's transform stored in each node already compensates the motion between the image's and the scan's stamps. The tool estimates its correction on top of that.
-- **Many nodes from varied viewpoints.** The correction is the one that fits all the nodes; a single view constrains it much less.
-- **Intensity in the scans** (format `XYZI`). It is used when there; see below.
-
-## How it works
-
-For every node:
-
-1. **Image edges.** Canny edges of the image, turned into a score with a distance transform: 1 on an edge, decreasing with the distance to it (`exp(-d / sigma)`, `--sigma`, default 3 pixels). The score is smooth enough for a search to follow it toward the edges.
-2. **Lidar edges.** The scan is projected in the camera at a coarse resolution (`--decimation`, default 1/4 of the image), where the projected points are dense, keeping the nearest point per cell so that surfaces hidden from the camera do not count. A cell's point is a lidar edge point when:
-   - it is in front of a depth discontinuity: a neighboring cell is at least 15% farther (`--jump`) than where this cell's surface would continue, as at the border of an object in front of a farther background. The continuation is predicted from the opposite neighbor (on a plane, inverse depth is linear in the image), so that a surface seen at a grazing angle, such as the floor ahead of a low camera, is not taken for a discontinuity: its depth changes fast from one cell to the next, but as its continuation predicts; or
-   - it is on an intensity edge: the lidar's intensity changes by more than 40% to a neighboring cell (`--intensity_jump`), as at a change of paint or material. A cell's intensity is the mean (of the logarithm) over all the points of the surface it sees, not its nearest point's: a lidar's beams do not return the same intensity from the same surface, and a node's scan assembles several sweeps, so cells seen by different beams would otherwise differ and make false edges along the beams' traces, even on a flat uniform wall. Then it is median filtered against what speckle remains. As for creases (below), the cells only say where there is an intensity edge: where exactly is found at the image's full resolution. This is what finds edges on flat surfaces, where the depth does not jump: holds on a climbing wall, window frames, panels. `--no_intensity` turns it off; or
-   - it is on a crease: the surface normals of neighboring cells differ by more than 45 degrees (`--crease_angle`, 0 disables), as between a wall and the floor, where the depth does not jump. Normals are computed on the scan voxelized (`--crease_voxel`, 10 cm), smoother than at full resolution; each voxel's normal is spread over the cells it covers, and averaged per cell like intensity. As the normals then change over a band of a few cells across a crease (and intensity across an intensity edge), where exactly it is is found at the image's full resolution, as Canny finds edges: the normals (or intensity) are interpolated and smoothed, and the edge is where their change is the largest across it, to a fraction of a pixel. Its depth is interpolated there (these edges are on continuous surfaces), and the point is put back in 3D, one per cell. Taking the nearest lidar point of each cell instead would put them anywhere in a band a few cells wide on both sides of the edge. They help most where surfaces all reflect alike, with few intensity edges.
-
-   Each point is weighted by the size of its jump. The edge points are kept in 3D, in the scan's frame, so that they can then be projected at full resolution.
-
-Then a solver finds the correction `C` of the camera's transform that best lines the lidar edge points up with the images' edges, over all the nodes, starting from no correction (`--solver`). The direct solvers maximize a score: the weighted average, over all the lidar edge points, of the image edge score where the point projects at full resolution (interpolated).
-
-- `simplex` (default): OpenCV's Nelder-Mead simplex (`cv::DownhillSolver`), started again once from its result. It moves all the parameters together, so it can follow directions in which they are coupled (which is likely between translation and rotation).
-- `pattern`: one parameter at a time, a step each way, until no step improves, then with half the steps (2 degrees down to about 0.03 degree). The fastest, but it moves only along the parameters' axes.
-
-The least-squares solvers (if RTAB-Map is built with g2o or GTSAM) minimize instead, for each lidar edge point, its distance in pixels to the nearest image edge where it projects (chamfer matching), weighted by the point's weight:
-
-- `g2o`, `gtsam`: Levenberg-Marquardt, with a Welsch robust kernel, which makes a point far from any image edge count for nothing, as the score does: many lidar edges have no counterpart in the image. Levenberg-Marquardt only follows the local slope, and each point is pulled toward its nearest image edge, often not its own when far from the solution, so on its own it stops in a local minimum a few degrees away. A coarse `pattern` search (2 down to 0.25 degree) gets close first, then the kernel's scale goes from wide to narrow (9, 3, then 1 x `--sigma`). Several times longer than `simplex`, and about 2 GB of memory for 500k lidar edge points.
-
-The lidar edge points are then selected again with the result, and the solver runs a second time from there.
-
-### Code
-
-- `main.cpp`: loading the database, the image edges, the lidar edge points (`selectEdgePoints()`), the passes, the checks and the report.
-- `CalibrationProblem.h/.cpp`: what a solver sees: the nodes' lidar edge points, where a correction projects them (`project()`), the score (`score()`), and the distance to the image edges for least squares (`edgeDistance()`).
-- `CorrectionSolver.h/.cpp`: the solvers' interface and `createSolver()`.
-- `PatternSearchSolver`, `DownhillSimplexSolver`, `G2oSolver`, `GtsamSolver` (`.h/.cpp`): the solvers. The last two compile to nothing when RTAB-Map is built without g2o or GTSAM.
-
-A new solver implements `CorrectionSolver` and is added to `createSolver()` and `availableSolvers()`.
-
-This is the approach of Levinson and Thrun (see [Reference](#reference)), with intensity edges added.
-
-## The result
-
-The tool prints a correction `X` of the camera's mount, in the camera's body frame (x forward, y left, z up): the robot base to camera transform `B` (e.g., `base_link -> camera_link`) becomes `B * X`. It is a multiplication of transforms, not a sum of angles. Its roll is about the camera's viewing axis, its pitch is the camera's tilt and its yaw its pan. It prints it in radians and degrees, and as a quaternion. In TF, `X` can also be inserted after `B`, leaving the other transforms as they are: `base_link -[B]-> camera_link_measured -[X]-> camera_link`.
-
-Internally, the lidar is projected in the camera's optical frame (x right, y down, z forward), under the body frame through the optical rotation `R` (roll -pi/2, pitch 0, yaw -pi/2): the solvers search for the same correction there, `C = R^-1 * X * R`, each node's camera local transform `T` becoming `T * C`.
-
-### Rotation only, by default
-
-Only the rotation is estimated unless `--translation` is given. The translation between a camera and a lidar is usually a few centimeters, which moves the edges in the images very little when the scene is several meters away: it is not observable, and estimating it anyway gives values that change from one subset of the nodes to another. Measure it instead, or estimate it with `--translation` only on data with close surfaces, and check it as below.
-
-### Checking it
-
-The tool prints, for each kind of lidar edge, the share of its points within 2 pixels of an image edge once corrected: how much each brings, and how much of it is noise. Then:
-
-- **Each half of the nodes on its own.** The even and the odd nodes are calibrated separately. If they agree, the result is supported by the data; if they differ, the difference is about how much the result can be trusted.
-- **The sensitivity**, with `--verbose`: how much the score drops, in percent, with the result off by 1 degree (roll, pitch, yaw) or 2 cm (x, y, z) along or about each axis of the camera, the mean of both directions. Think of the score as a valley with the result at its bottom: the sensitivity is how steep its sides are along each axis.
-  - A large drop (several percent or more for 1 degree) means the data determines that axis well: a small error on it would misalign many edges, so the solver cannot be far off, and a correction on that axis can be trusted, however large.
-  - Almost none (around 1% or less) means the axis is not observable from this data: the edges hardly move with it, so any value the solver finds on it, large or small, is not reliable.
-  - It says how sure the result is, not how far off the camera was: that is the correction itself. It depends on the scene and the sensors, not on the error: e.g., many vertical and horizontal edges make yaw and pitch steep, roll (about the viewing axis) moves edges little near the image center so it is usually less steep, and translation is flat unless surfaces are close (a 2 cm shift moves edges a fraction of a pixel at several meters).
-
-With `--images dir`, the image of every node is saved darkened, with the image edges the alignment uses (Canny) in green and the lidar edge points projected over them, depth discontinuities in red, creases in blue and intensity edges in yellow, before (`<id>_edges_1_before.png`, from where the search started: the database's camera transform, with `--initial_rotation` if given) and after (`<id>_edges_2_after.png`) the correction, with the node's id and speeds in the top left corner: the mean since the previous node (over which an assembled scan is taken) and the instantaneous one when the node was added. After, the lidar points should lie on green wherever both sensors see an edge. Points away from any green, and green edges without points, are edges only one of the sensors sees (e.g., lidar intensity through glass, or shadows in the image).
-
-For example, a node of an indoor climbing gym, before (left) and after (right) the correction: before, the creases along the floor and the climbing holds' outlines are off their green edges; after, they lie on them.
-
-| Before (`<id>_edges_1_before.png`) | After (`<id>_edges_2_after.png`) |
-|---|---|
-| ![before](images/1621_edges_1_before.jpg) | ![after](images/1621_edges_2_after.jpg) |
-
-With each of them, two images show the maps the lidar edges are found from, at the decimated resolution, half transparent over the image, with the image's edges in green and the map's own edges (Canny, as for the image) in blue:
-
-- `<id>_intensity_1_before.png`, `<id>_intensity_2_after.png`: the lidar's intensity per cell, as used (mean log-intensity, median filtered), from red (dark) to yellow (bright), with the contrast stretched for each image. The intensity edges are where it changes; they should line up with green where the image shows the same change of material.
-- `<id>_normals_1_before.png`, `<id>_normals_2_after.png`: how each cell's surface faces the camera, from yellow (facing it) to red (seen edge on, at a grazing angle). Surfaces at a grazing angle are where depth changes fast without a discontinuity, and where the intensity drops. The normals are computed on the scans voxelized at `--crease_voxel`, so this image is made whether or not creases are used.
-
-Before (left) and after (right) the correction: the intensity of the same node, where the holds stand out in yellow, and the normals of another, where the climbing wall seen at a grazing angle is red against the walls facing the camera in yellow. Once corrected, the map's edges (blue) lie on the image's (green).
-
-| Before | After |
-|---|---|
-| ![intensity before](images/1621_intensity_1_before.jpg) | ![intensity after](images/1621_intensity_2_after.jpg) |
-| ![normals before](images/2093_normals_1_before.jpg) | ![normals after](images/2093_normals_2_after.jpg) |
-
-It is also worth running it again with other values of `--decimation`, `--intensity_jump` or `--voxel` (which voxel filters the scans first, to compare densities): a result that does not move with them is more trustworthy than one that does.
-
-## Options
-
-| Option | Default | Description |
-|---|---|---|
-| `--solver "name"` | `simplex` | `simplex`, `pattern`, `g2o` or `gtsam`, see above. `--help` lists those in this build. |
-| `--verbose` | off | Also print the sensitivity (see above). |
-| `--translation` | off | Also estimate the translation (see above). |
-| `--images "dir"` | | Save the image of every node with its edges (green) and the lidar edge points (depth: red, crease: blue, intensity: yellow), before and after the correction, and the lidar's intensity and surface orientation (see above). |
-| `--decimation #` | `4` | Image decimation at which the lidar edges are found. Lower is finer, but needs denser scans. |
-| `--jump #.#` | `0.15` | Relative depth jump for a depth discontinuity, as a fraction of the point's depth: 0.15 means a neighbor at least 15% of its depth farther than where its surface would continue (e.g., 0.6 m behind a point at 4 m). |
-| `--intensity_jump #.#` | `0.4` | Relative intensity change for an intensity edge, as a fraction: 0.4 means a neighbor at least 1.4 times brighter or darker (compared on log-intensity). Lower finds more edges, and more speckle. |
-| `--intensity_weight #.#` | `1` | Weight of the intensity edges relative to the depth discontinuities. Lower it where intensity is less reliable than geometry (e.g., much glass). |
-| `--crease_angle #.#` | `45` | Creases: where the surface normals differ by this angle (deg). 0: off. |
-| `--crease_voxel #.#` | `0.1` | Voxel size (m) of the scans on which the normals are computed. |
-| `--no_intensity` | | Use only depth discontinuities. |
-| `--sigma #.#` | `3` | Fall off, in pixels, of the image edge score. |
-| `--min_depth #.#` | `0.5` | Ignore lidar points closer than this to the camera (m). |
-| `--initial_rotation #.# #.# #.#` | `0 0 0` | Start from this correction (roll, pitch, yaw in deg, in the camera's body frame) instead of none. The correction printed stays relative to the database's camera transform (the same as without this option if the result is found again); the one relative to the starting point is printed too: to see from how far off the result is found again, or to start closer when the stored transform is known to be far off. The two halves of the nodes start from it too. |
-| `--max_angular_speed #.#` | `0` | Skip the nodes rotating faster than this (deg/s): the mean since the previous node, from their odometry poses, over which an assembled scan is taken (else the instantaneous odometry velocity stored in the node). 0: keep all. |
-| `--max_linear_speed #.#` | `0` | Skip the nodes moving faster than this (m/s). 0: keep all. |
-| `--voxel #.#` | `0` | Voxel filter the scans first (m), to compare the result at several lidar densities. |
-
-## Limits
-
-- **Time offset.** A delay between the camera's and the lidar's clocks looks like a rotation while the robot turns. It is not estimated: use well synchronized sensors, or data where the robot turns slowly (`--max_angular_speed` skips the nodes where it turns fast).
-- **Motion within a node.** An assembled cloud spans a few sweeps; the points from the earlier ones are moved by odometry, whose error adds noise.
-- **Rolling shutter.** It is not modeled; fast rotations distort the images.
-- **Intrinsics.** The camera's calibration (focal lengths, center, distortion) is assumed right; an error there biases the rotation.
-- **One camera per node.** Nodes with several cameras are skipped.
-
-## Reference
-
-J. Levinson and S. Thrun, "Automatic Online Calibration of Cameras and Lasers", *Robotics: Science and Systems IX* (RSS), 2013. [PDF](http://www.roboticsproceedings.org/rss09/p29.pdf)
-
-What this tool takes from it: lidar points at depth discontinuities as the lidar's edges, the image's edges spread out by a distance transform so that the alignment score is smooth, and that score maximized over many frames at once. What it does differently:
-
-- The depth discontinuities are found in the scan projected in the camera, at a coarse resolution, rather than between consecutive points of a scan line: the scans of a node can be an assembly of several sweeps, voxel filtered, without scan lines. They are measured against the continuation of the surface, so that surfaces seen at a grazing angle do not make false ones.
-- Intensity edges and creases are added to the depth discontinuities, for surfaces without depth jumps.
-- The search is local, from the current transform, with a choice of solvers; the rotation only by default.
-- The result is checked on two halves of the nodes estimated separately.