ImuMotionPredictor.h

/*
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>
#include <vector>
 
namespace rtabmap {
 
class RTABMAP_CORE_EXPORT ImuMotionPredictor
{
public:
  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_;}
  bool isGravityEstimated() const {return gravityEstimated_;}
  size_t gravityWindows() const {return gravityWindowsCount_;}
 
  void addImu(double stamp, const IMU & imu);
 
  void addPose(double stamp, const rtabmap::Transform & pose);
 
  void reset();
 
  rtabmap::Transform predict(double stamp) const;
 
  bool hasPose() const;
  double lastPoseStamp() const;
  Eigen::Vector3d velocity() const;
  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
  // 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;
    // 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,
      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_;
  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_;
 
  // 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_ */