Added auto gravity estimation mode

corelib/include/rtabmap/core/ImuMotionPredictor.h

@@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <Eigen/Geometry>
 
 #include <map>
+#include <vector>
 
 namespace rtabmap {
 
@@ -60,6 +61,17 @@ namespace rtabmap {
  * ignored: its centripetal and tangential accelerations are small over the fraction of a
  * second this predicts.
  *
+ * The gravity removed can be estimated from the IMU itself (see the constructor): an
+ * accelerometer can read a few percent off at rest (scale or bias), and that error,
+ * integrated as an acceleration, would bias the predicted velocity along gravity. The
+ * accelerometer is averaged over the windows where it is quiet: with a gyro and an
+ * accelerometer that barely change, the IMU is not accelerating (at rest or at constant
+ * velocity), so it only measures gravity. The IMU must then be held still (or at constant
+ * velocity) for a moment, e.g., before moving: until then, standard gravity is used, and
+ * a steady acceleration without rotation would be taken for gravity. Only the error along
+ * gravity is corrected, which is all of it for an IMU that stays about level; one changing
+ * attitude would need its axes calibrated.
+ *
  * Not thread-safe: a caller sharing it between threads must lock around every call.
  */
 class RTABMAP_CORE_EXPORT ImuMotionPredictor
@@ -81,7 +93,9 @@ public:
 	 *                        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
+	 *                        given to addImu(), standard gravity by default. <= 0: estimated
+	 *                        from the IMU when it is still (see the class description), with
+	 *                        standard gravity until then.
 	 *
 	 * These are fixed for the life of the predictor (there is no setter): changing them
 	 * while it estimates would mix poses and samples taken under different settings.
@@ -93,7 +107,12 @@ public:
 
 	double maxPoseInterval() const {return maxPoseInterval_;}
 	double velocityWindow() const {return velocityWindow_;}
+	/// Gravity (m/s^2) removed from the specific force: the one given, or the estimate
 	double gravity() const {return gravity_;}
+	/// Whether the gravity is estimated from the IMU (see the constructor)
+	bool isGravityEstimated() const {return gravityEstimated_;}
+	/// Number of quiet IMU windows the gravity estimate is from (0: not estimated yet)
+	size_t gravityWindows() const {return gravityWindowsCount_;}
 
 	/**
 	 * @brief Adds an IMU measurement.
@@ -115,7 +134,8 @@ public:
 	 */
 	void addPose(double stamp, const rtabmap::Transform & pose);
 
-	/// Forgets the poses and the IMU samples.
+	/// Forgets the poses and the IMU samples. The gravity estimate is kept: it is the
+	/// sensor's.
 	void reset();
 
 	/**
@@ -140,13 +160,22 @@ public:
 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);
+	// specificForce: in that frame too, including the reaction to gravity (see
+	// hasAcceleration)
+	void addSample(double stamp, const Eigen::Quaterniond & orientation,
+			const Eigen::Vector3d & specificForce, bool hasAcceleration);
+	// Gravity estimation, from the IMU's own specific force and angular velocity norm
+	void estimateGravity(double stamp, const Eigen::Vector3d & specificForce, double angularVelocity);
 
 	struct Sample
 	{
 		Eigen::Quaterniond orientation;
-		Eigen::Vector3d acceleration;
+		// Kept with gravity, which is removed when used: the estimate can change
+		Eigen::Vector3d specificForce;
+		bool hasAcceleration;
 	};
+	// Acceleration of a sample, gravity removed (null without acceleration)
+	Eigen::Vector3d accelerationOf(const Sample & sample) const;
 	Eigen::Quaterniond orientationAt(double stamp) const;
 	Eigen::Vector3d accelerationAt(double stamp) const;
 	void integrate(double from, double to, const Eigen::Quaterniond & rotation,
@@ -166,7 +195,20 @@ private:
 	double maxPoseInterval_;
 	double velocityWindow_;
 	double gravity_;
+	bool gravityEstimated_;
 	std::map<double, Sample> samples_;
+
+	// Gravity estimation: the samples of the current window, and the gravity measured over
+	// the last quiet windows
+	struct StillSample
+	{
+		double stamp;
+		Eigen::Vector3d specificForce; // in the IMU frame
+		double angularVelocity;        // norm
+	};
+	std::vector<StillSample> stillWindow_;
+	std::vector<double> gravityWindows_;
+	size_t gravityWindowsCount_;
 	// Recent odometry poses, to estimate the velocity from (see velocityWindow)
 	std::map<double, rtabmap::Transform> poses_;
 

corelib/include/rtabmap/core/Parameters.h

@@ -512,7 +512,7 @@ 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, 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, ImuGravity,             float, 9.80665, uFormat("Gravity magnitude (m/s^2) removed from the IMU linear acceleration (used with \"%s\" > 0). Standard gravity by default. Set it to what the accelerometer reads at rest to compensate for its scale error, or for another gravity than Earth's. 0: estimated from the IMU while it is still (gyro and accelerometer quiet over 0.5 s), with standard gravity until then: the robot should then be perfectly still for a moment (e.g., at start), and a steady acceleration without rotation would be taken for gravity.", kOdomGuessSmoothingDelay().c_str()));
     RTABMAP_PARAM(Odom, GuessSmoothingDelay,    float, 0,     uFormat("Guess smoothing delay (s). 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.");

corelib/src/ImuMotionPredictor.cpp

@@ -26,7 +26,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
 #include <rtabmap/core/ImuMotionPredictor.h>
+#include <rtabmap/utilite/ULogger.h>
 
+#include <algorithm>
 #include <vector>
 
 namespace rtabmap {
@@ -35,14 +37,29 @@ namespace rtabmap {
 // bounded while no odometry pose comes to trim it.
 static const double kMaxBufferDuration = 10.0;
 
+// Gravity estimation (see the class description). The IMU is still over a window if its
+// angular velocity stays under kStillMaxAngularVelocity and the standard deviation of its
+// specific force, on every axis, under kStillMaxAccelerationStd. Those are above the noise
+// and vibrations of an IMU on a robot at rest (e.g., ~0.1 m/s^2 on a drone on the ground,
+// against ~1 m/s^2 in flight), and well under what any acceleration that matters would
+// cause. The gravity is the median over the last kGravityWindows quiet windows.
+static const double kStillWindow = 0.5;              // s
+static const double kStillMaxAngularVelocity = 0.05; // rad/s
+static const double kStillMaxAccelerationStd = 0.15; // m/s^2
+static const size_t kStillMinSamples = 5;
+static const size_t kGravityWindows = 100;
+static const double kStandardGravity = 9.80665;
+
 ImuMotionPredictor::ImuMotionPredictor(double maxPoseInterval, double velocityWindow, double gravity) :
 	maxPoseInterval_(maxPoseInterval),
 	velocityWindow_(velocityWindow),
-	gravity_(gravity),
-	integratedStartIsFinal_(false),
+	gravity_(gravity > 0.0 ? gravity : kStandardGravity),
+	gravityEstimated_(gravity <= 0.0),
+	gravityWindowsCount_(0),
 	poseStamp_(0.0),
 	velocity_(Eigen::Vector3d::Zero()),
-	worldToOdom_(Eigen::Quaterniond::Identity())
+	worldToOdom_(Eigen::Quaterniond::Identity()),
+	integratedStartIsFinal_(false)
 {
 }
 
@@ -63,21 +80,93 @@ void ImuMotionPredictor::addImu(double stamp, const IMU & imu)
 			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))
+	const bool hasAcceleration = (f[0] != 0.0 || f[1] != 0.0 || f[2] != 0.0) &&
+			(imu.linearAccelerationCovariance().empty() || imu.linearAccelerationCovariance().at<double>(0,0) != -1.0);
+	if(hasAcceleration && gravityEstimated_)
 	{
-		// 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_);
+		const cv::Vec3d & w = imu.angularVelocity();
+		estimateGravity(stamp, Eigen::Vector3d(f[0], f[1], f[2]), Eigen::Vector3d(w[0], w[1], w[2]).norm());
 	}
-	addSample(stamp, worldToImu * baseToImu.inverse(), acceleration);
+	// The specific force includes the reaction to gravity, up in the world frame: it is
+	// removed when used (accelerationOf())
+	addSample(stamp, worldToImu * baseToImu.inverse(),
+			hasAcceleration ? Eigen::Vector3d(worldToImu * Eigen::Vector3d(f[0], f[1], f[2])) : Eigen::Vector3d::Zero(),
+			hasAcceleration);
 }
 
-void ImuMotionPredictor::addSample(double stamp, const Eigen::Quaterniond & orientation, const Eigen::Vector3d & acceleration)
+void ImuMotionPredictor::estimateGravity(double stamp, const Eigen::Vector3d & specificForce, double angularVelocity)
+{
+	if(!stillWindow_.empty() && stamp < stillWindow_.back().stamp)
+	{
+		// Out of order
+		stillWindow_.clear();
+	}
+	if(!stillWindow_.empty() && stamp - stillWindow_.front().stamp >= kStillWindow)
+	{
+		// The window is complete (windows don't overlap: each sample counts once)
+		if(stillWindow_.size() >= kStillMinSamples)
+		{
+			Eigen::Vector3d mean = Eigen::Vector3d::Zero();
+			double maxAngularVelocity = 0.0;
+			for(size_t i=0; i<stillWindow_.size(); ++i)
+			{
+				mean += stillWindow_[i].specificForce;
+				maxAngularVelocity = std::max(maxAngularVelocity, stillWindow_[i].angularVelocity);
+			}
+			mean /= double(stillWindow_.size());
+			Eigen::Vector3d variance = Eigen::Vector3d::Zero();
+			for(size_t i=0; i<stillWindow_.size(); ++i)
+			{
+				variance += (stillWindow_[i].specificForce - mean).cwiseAbs2();
+			}
+			variance /= double(stillWindow_.size());
+			if(maxAngularVelocity < kStillMaxAngularVelocity &&
+			   variance.maxCoeff() < kStillMaxAccelerationStd*kStillMaxAccelerationStd)
+			{
+				// Not accelerating: the mean specific force is gravity, whatever the
+				// attitude (the norm of the mean, not the mean of the norms, which the
+				// noise would bias up)
+				gravityWindows_.push_back(mean.norm());
+				if(gravityWindows_.size() > kGravityWindows)
+				{
+					gravityWindows_.erase(gravityWindows_.begin());
+				}
+				++gravityWindowsCount_;
+				std::vector<double> sorted = gravityWindows_;
+				std::nth_element(sorted.begin(), sorted.begin() + sorted.size()/2, sorted.end());
+				const double gravity = sorted[sorted.size()/2];
+				if(gravityWindowsCount_ == 1)
+				{
+					UINFO("IMU gravity estimated at %f m/s^2 (the IMU is still)", gravity);
+				}
+				else
+				{
+					UDEBUG("IMU gravity estimated at %f m/s^2 (%d still windows)", gravity, (int)gravityWindowsCount_);
+				}
+				if(gravity != gravity_)
+				{
+					gravity_ = gravity;
+					// The accelerations integrated changed
+					integrated_.clear();
+				}
+			}
+		}
+		stillWindow_.clear();
+	}
+	StillSample sample;
+	sample.stamp = stamp;
+	sample.specificForce = specificForce;
+	sample.angularVelocity = angularVelocity;
+	stillWindow_.push_back(sample);
+}
+
+void ImuMotionPredictor::addSample(double stamp, const Eigen::Quaterniond & orientation,
+		const Eigen::Vector3d & specificForce, bool hasAcceleration)
 {
 	Sample sample;
 	sample.orientation = orientation.normalized();
-	sample.acceleration = acceleration;
+	sample.specificForce = specificForce;
+	sample.hasAcceleration = hasAcceleration;
 	samples_[stamp] = sample;
 	if(!integrated_.empty() && stamp <= integrated_.rbegin()->first)
 	{
@@ -172,6 +261,7 @@ void ImuMotionPredictor::addPose(double stamp, const rtabmap::Transform & pose)
 void ImuMotionPredictor::reset()
 {
 	samples_.clear();
+	stillWindow_.clear();
 	poses_.clear();
 	integrated_.clear();
 	pose_.setNull();
@@ -287,7 +377,7 @@ void ImuMotionPredictor::updateIntegration() const
 		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.acceleration = worldToOdom_ * accelerationOf(iter->second);
 		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;
@@ -311,20 +401,30 @@ Eigen::Quaterniond ImuMotionPredictor::orientationAt(double stamp) const
 	return before->second.orientation.slerp(ratio, after->second.orientation);
 }
 
+Eigen::Vector3d ImuMotionPredictor::accelerationOf(const Sample & sample) const
+{
+	if(!sample.hasAcceleration)
+	{
+		return Eigen::Vector3d::Zero();
+	}
+	return sample.specificForce - Eigen::Vector3d(0, 0, gravity_);
+}
+
 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;
+		return accelerationOf(samples_.rbegin()->second);
 	}
 	if(after == samples_.begin() || after->first == stamp)
 	{
-		return after->second.acceleration;
+		return accelerationOf(after->second);
 	}
 	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);
+	const Eigen::Vector3d accelerationBefore = accelerationOf(before->second);
+	return accelerationBefore + ratio * (accelerationOf(after->second) - accelerationBefore);
 }
 
 void ImuMotionPredictor::integrate(double from, double to, const Eigen::Quaterniond & rotation,

corelib/test/test_imumotionpredictor.cpp

@@ -375,3 +375,86 @@ TEST(ImuMotionPredictor, ignores_the_acceleration_without_a_velocity_window)
 	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);
 }
+
+namespace {
+
+/// An IMU tilted by 0.1 rad in pitch whose accelerometer reads `measuredGravity` at rest,
+/// with a vibration of the given amplitude (m/s^2) on every axis and the given angular
+/// velocity (rad/s, about z).
+rtabmap::IMU tiltedImu(double t, double measuredGravity, double vibration = 0.05, double angularVelocity = 0.0)
+{
+	const Eigen::Quaterniond worldToImu(Eigen::AngleAxisd(0.1, Eigen::Vector3d::UnitY()));
+	const Eigen::Vector3d f = worldToImu.inverse() * Eigen::Vector3d(0, 0, measuredGravity) +
+			vibration * Eigen::Vector3d(std::sin(t*331.0), std::sin(t*457.0), std::sin(t*563.0));
+	return rtabmap::IMU(cv::Vec4d(worldToImu.x(), worldToImu.y(), worldToImu.z(), worldToImu.w()), cv::Mat::eye(3,3,CV_64FC1),
+			cv::Vec3d(0, 0, angularVelocity), cv::Mat::eye(3,3,CV_64FC1),
+			cv::Vec3d(f.x(), f.y(), f.z()), cv::Mat::eye(3,3,CV_64FC1),
+			rtabmap::Transform::getIdentity());
+}
+
+}  // namespace
+
+TEST(ImuMotionPredictor, estimates_the_gravity_while_the_imu_is_still)
+{
+	// An accelerometer reading 9.55 m/s^2 at rest, as the netherdrone's does.
+	ImuMotionPredictor predictor(1.0, 0.5, 0.0);
+	EXPECT_TRUE(predictor.isGravityEstimated());
+	EXPECT_DOUBLE_EQ(predictor.gravity(), 9.80665) << "standard gravity until estimated";
+
+	for(int i=0; i<100; ++i) // 0.5 s at 200 Hz: the first window is not complete yet
+	{
+		predictor.addImu(i*0.005, tiltedImu(i*0.005, 9.55));
+	}
+	EXPECT_EQ(predictor.gravityWindows(), 0u);
+	EXPECT_DOUBLE_EQ(predictor.gravity(), 9.80665);
+
+	for(int i=100; i<=400; ++i)
+	{
+		predictor.addImu(i*0.005, tiltedImu(i*0.005, 9.55));
+	}
+	EXPECT_GE(predictor.gravityWindows(), 3u);
+	EXPECT_NEAR(predictor.gravity(), 9.55, 0.005) << "whatever the attitude";
+
+	// At rest, it doesn't look like falling anymore.
+	predictor.addPose(1.5, rtabmap::Transform::getIdentity());
+	predictor.addPose(2.0, rtabmap::Transform::getIdentity());
+	EXPECT_NEAR(predictor.predict(2.0).z(), 0.0, 1e-3);
+	for(int i=401; i<=460; ++i)
+	{
+		predictor.addImu(i*0.005, tiltedImu(i*0.005, 9.55));
+	}
+	EXPECT_NEAR(predictor.predict(2.3).z(), 0.0, 2e-3);
+
+	// The estimate is the sensor's: a reset keeps it.
+	predictor.reset();
+	EXPECT_NEAR(predictor.gravity(), 9.55, 0.005);
+}
+
+TEST(ImuMotionPredictor, does_not_take_a_moving_imu_for_a_still_one)
+{
+	// Turning (centripetal acceleration, gravity moving between the axes) or vibrating
+	// as in flight: not still, standard gravity is kept.
+	ImuMotionPredictor turning(1.0, 0.5, 0.0);
+	ImuMotionPredictor vibrating(1.0, 0.5, 0.0);
+	for(int i=0; i<=400; ++i)
+	{
+		turning.addImu(i*0.005, tiltedImu(i*0.005, 9.55, 0.05, 0.5));
+		vibrating.addImu(i*0.005, tiltedImu(i*0.005, 9.55, 1.0));
+	}
+	EXPECT_EQ(turning.gravityWindows(), 0u);
+	EXPECT_DOUBLE_EQ(turning.gravity(), 9.80665);
+	EXPECT_EQ(vibrating.gravityWindows(), 0u);
+	EXPECT_DOUBLE_EQ(vibrating.gravity(), 9.80665);
+}
+
+TEST(ImuMotionPredictor, keeps_the_gravity_it_is_given)
+{
+	ImuMotionPredictor predictor;
+	for(int i=0; i<=400; ++i)
+	{
+		predictor.addImu(i*0.005, tiltedImu(i*0.005, 9.55));
+	}
+	EXPECT_FALSE(predictor.isGravityEstimated());
+	EXPECT_EQ(predictor.gravityWindows(), 0u);
+	EXPECT_DOUBLE_EQ(predictor.gravity(), 9.80665);
+}