rtabmap::ImuMotionPredictor Class Reference

Predicts the pose of the base frame from the last odometry pose and the IMU. More...

#include <ImuMotionPredictor.h>

Public Member Functions
	ImuMotionPredictor (double maxPoseInterval=1.0, double velocityWindow=0.5, double gravity=9.80665)
double	maxPoseInterval () const
double	velocityWindow () const
double	gravity () const
void	addImu (double stamp, const IMU &imu)
	Adds an IMU measurement.
void	addPose (double stamp, const rtabmap::Transform &pose)
	Adds an odometry pose, from which the next poses are predicted.
void	reset ()
	Forgets the poses and the IMU samples.
rtabmap::Transform	predict (double stamp) const
	Predicts the pose of the base frame in the odometry frame.
bool	hasPose () const
	Whether there is a pose to predict from (none yet, or the last one was null).
double	lastPoseStamp () const
	Stamp of the last pose added, 0 if there is none.
Eigen::Vector3d	velocity () const
	Velocity (m/s) of the base in the odometry frame at the last pose.
size_t	samples () const
	Number of IMU samples kept.

Detailed Description

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.

Definition at line 65 of file ImuMotionPredictor.h.

Constructor & Destructor Documentation

ImuMotionPredictor()

rtabmap::ImuMotionPredictor::ImuMotionPredictor ( double  maxPoseInterval = 1.0, double  velocityWindow = 0.5, double  gravity = 9.80665  )

explicit

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).

Parameters

maxPoseInterval	odometry poses older (s) than this are not used to estimate the velocity, which is null without one
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.
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.

Member Function Documentation

maxPoseInterval()

double rtabmap::ImuMotionPredictor::maxPoseInterval ( ) const

inline

Definition at line 94 of file ImuMotionPredictor.h.

velocityWindow()

double rtabmap::ImuMotionPredictor::velocityWindow ( ) const

inline

Definition at line 95 of file ImuMotionPredictor.h.

gravity()

double rtabmap::ImuMotionPredictor::gravity ( ) const

inline

Definition at line 96 of file ImuMotionPredictor.h.

addImu()

void rtabmap::ImuMotionPredictor::addImu	(	double	stamp,
		const IMU &	imu
	)

Adds an IMU measurement.

Parameters

stamp	time of the measurement (s)
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.

addPose()

void rtabmap::ImuMotionPredictor::addPose	(	double	stamp,
		const rtabmap::Transform &	pose
	)

Adds an odometry pose, from which the next poses are predicted.

Parameters

stamp	time of the pose (s)
pose	pose of the base frame in the odometry frame; a null pose (odometry lost) resets the prediction until the next valid pose

predict()

rtabmap::Transform rtabmap::ImuMotionPredictor::predict

(

double

stamp

)

const

Predicts the pose of the base frame in the odometry frame.

Parameters

stamp

time (s) of the prediction, normally after the last pose

Returns

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.

---

The documentation for this class was generated from the following file:

• corelib/include/rtabmap/core/ImuMotionPredictor.h