imu_integration.hpp

#pragma once
 
#include <deque>
#include <vector>
#include <Eigen/Core>
#include <gtsam/navigation/ImuFactor.h>
 
namespace glim {
 
struct IMUIntegrationParams {
  IMUIntegrationParams(const bool upright = true);
  ~IMUIntegrationParams();
 
  bool upright;       // If true, +Z = up
  double acc_noise;   // Linear acceleration noise
  double gyro_noise;  // Angular velocity noise
  double int_noise;   // Integration noise
};
 
class IMUIntegration {
public:
  IMUIntegration(const IMUIntegrationParams& params = IMUIntegrationParams());
  ~IMUIntegration();
 
  void insert_imu(double stamp, const Eigen::Vector3d& linear_acc, const Eigen::Vector3d& angular_vel);
 
  int integrate_imu(double start_time, double end_time, const gtsam::imuBias::ConstantBias& bias, int* num_integrated);
 
  int integrate_imu(
    double start_time,
    double end_time,
    const gtsam::NavState& state,
    const gtsam::imuBias::ConstantBias& bias,
    std::vector<double>& pred_times,
    std::vector<Eigen::Isometry3d>& pred_poses);
 
  int find_imu_data(double start_time, double end_time, std::vector<double>& delta_times, std::vector<Eigen::Matrix<double, 7, 1>>& imu_data);
 
  void erase_imu_data(int last);
 
  const gtsam::PreintegratedImuMeasurements& integrated_measurements() const;
 
  const std::deque<Eigen::Matrix<double, 7, 1>>& imu_data_in_queue() const;
 
private:
  std::shared_ptr<gtsam::PreintegratedImuMeasurements> imu_measurements;
  std::deque<Eigen::Matrix<double, 7, 1>> imu_queue;
};
}  // namespace glim