i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-12 12:59:51 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added auto gravity estimation mode

Browse Source
This commit is contained in:
matlabbe
2026-10-11 11:36:44 -07:00
parent 5d447d9bb0
commit 526c422643
5 changed files with 249 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/ImuMotionPredictor.h
+46 -4
View File
@@ -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
+1 -1
View File
@@ -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.");