Imu motion predictor

corelib/include/rtabmap/core/ImuMotionPredictor.h

@@ -0,0 +1,187 @@
+/*
+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:
+	/**
+	 * Without 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). Over a single frame
+	 *                        interval, the noise of the odometry poses would be of the order
+	 *                        of the velocity itself; with the acceleration, a longer window
+	 *                        still gives the velocity at the last pose, not an average.
+	 * @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 {
 
@@ -159,6 +160,7 @@ private:
 	bool _force3DoF;
 	bool _holonomic;
 	bool guessFromMotion_;
+	bool guessImuAcceleration_;
 	float guessSmoothingDelay_;
 	int _filteringStrategy;
 	int _particleSize;
@@ -189,6 +191,7 @@ private:
 	std::vector<StereoCameraModel> stereoModels_;
 	std::vector<CameraModel> models_;
 	std::map<double, Transform> imus_;
+	ImuMotionPredictor imuMotionPredictor_; // used with Odom/GuessImuAcceleration only
 };
 
 } /* namespace rtabmap */

corelib/include/rtabmap/core/Parameters.h

@@ -512,7 +512,9 @@ 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, GuessImuAcceleration,   bool, false,  uFormat("With an IMU giving orientation and linear acceleration, predict the translation of the motion guess (with \"%s\") by integrating the IMU acceleration from the velocity at the previous frame, instead of with a constant velocity. That velocity is the average over \"%s\" (from the displacement over that delay), carried to the previous frame with the acceleration measured since: it is not delayed like the average alone. Set \"%s\" to about 0.5 s: over a single frame, the noise of the poses is of the order of the velocity itself. Ignored by odometry approaches processing the IMU themselves.", kOdomGuessMotion().c_str(), kOdomGuessSmoothingDelay().c_str(), kOdomGuessSmoothingDelay().c_str()));
+    RTABMAP_PARAM(Odom, ImuGravity,             float, 9.80665, uFormat("Gravity magnitude (m/s^2) removed from the IMU linear acceleration with \"%s\". 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.", kOdomGuessImuAcceleration().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. Recommended (~0.5 s) with \"%s\": the translational velocity is then the displacement over this delay corrected by the IMU acceleration, so that it is the velocity at the last frame rather than a delayed average. 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 (\"%s\"), where its noise would feed back into the next poses.", kOdomFilteringStrategy().c_str(), kOdomGuessImuAcceleration().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.");

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,389 @@
+/*
+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(!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 (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(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

@@ -134,6 +134,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 		_force3DoF(Parameters::defaultRegForce3DoF()),
 		_holonomic(Parameters::defaultOdomHolonomic()),
 		guessFromMotion_(Parameters::defaultOdomGuessMotion()),
+		guessImuAcceleration_(Parameters::defaultOdomGuessImuAcceleration()),
 		guessSmoothingDelay_(Parameters::defaultOdomGuessSmoothingDelay()),
 		_filteringStrategy(Parameters::defaultOdomFilteringStrategy()),
 		_particleSize(Parameters::defaultOdomParticleSize()),
@@ -160,7 +161,15 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 	Parameters::parse(parameters, Parameters::kRegForce3DoF(), _force3DoF);
 	Parameters::parse(parameters, Parameters::kOdomHolonomic(), _holonomic);
 	Parameters::parse(parameters, Parameters::kOdomGuessMotion(), guessFromMotion_);
+	Parameters::parse(parameters, Parameters::kOdomGuessImuAcceleration(), guessImuAcceleration_);
 	Parameters::parse(parameters, Parameters::kOdomGuessSmoothingDelay(), guessSmoothingDelay_);
+	if(guessImuAcceleration_)
+	{
+		float imuGravity = Parameters::defaultOdomImuGravity();
+		Parameters::parse(parameters, Parameters::kOdomImuGravity(), imuGravity);
+		// The velocity is estimated over the smoothing delay
+		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 +237,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;
@@ -340,6 +350,11 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 			{
 				imus_.erase(imus_.begin());
 			}
+
+			if(guessImuAcceleration_)
+			{
+				imuMotionPredictor_.addImu(data.stamp(), data.imu());
+			}
 		}
 		else
 		{
@@ -646,6 +661,16 @@ 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_ && guessImuAcceleration_ && imuMotionPredictor_.hasPose())
+			{
+				// Translation (and orientation) predicted from the previous pose with the
+				// IMU acceleration, instead of a constant velocity.
+				Transform predicted = imuMotionPredictor_.predict(data.stamp());
+				if(!predicted.isNull())
+				{
+					guess = _pose.inverse() * predicted;
+				}
+			}
 			if(_force3DoF)
 			{
 				guess = guess.to3DoF();
@@ -877,6 +902,12 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 		}
 	}
 
+	if(t.isNull() && guessImuAcceleration_)
+	{
+		// Lost: no velocity can be estimated across the reset that follows
+		imuMotionPredictor_.addPose(data.stamp(), Transform());
+	}
+
 	if(!t.isNull())
 	{
 		_resetCurrentCount = _resetCountdown;
@@ -1032,6 +1063,21 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 			velocityGuess_.setNull();
 		}
 
+		if(guessImuAcceleration_)
+		{
+			const Transform newPose = _pose * t;
+			imuMotionPredictor_.addPose(data.stamp(), newPose);
+			if(!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/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,354 @@
+#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);
+}