mirror of
https://github.com/introlab/rtabmap.git
synced 2026-10-12 21:09:50 +08:00
Imu motion predictor
This commit is contained in:
matlabbe
@@ -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_ */
|
||||
@@ -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 */
|
||||
|
||||
@@ -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.");
|
||||
|
||||
Reference in New Issue
Block a user