Merge pull request #39 from koide3/imu

Re-integrated IMU factor

CMakeLists.txt

@@ -164,6 +164,7 @@ add_library(gtsam_points SHARED
   src/gtsam_points/factors/integrated_ct_gicp_factor.cpp
   src/gtsam_points/factors/bundle_adjustment_factor_evm.cpp
   src/gtsam_points/factors/bundle_adjustment_factor_lsq.cpp
+  src/gtsam_points/factors/reintegrated_imu_factor.cpp
   # experimental
   src/gtsam_points/factors/experimental/continuous_time_icp_factor.cpp
   # optimizers

include/gtsam_points/factors/reintegrated_imu_factor.hpp

@@ -0,0 +1,50 @@
+// SPDX-License-Identifier: MIT
+// Copyright (c) 2024  Kenji Koide ([email protected])
+#include <gtsam/navigation/ImuFactor.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+
+namespace gtsam_points {
+/**
+ * @brief IMU measurements to be re-integrated.
+ *       This class stores IMU measurements along with the pre-integrated IMU measurements.
+ */
+class ReintegratedImuMeasurements : public gtsam::PreintegratedImuMeasurements {
+public:
+  friend class ReintegratedImuFactor;
+
+  ReintegratedImuMeasurements(const boost::shared_ptr<gtsam::PreintegrationParams>& p, const gtsam::imuBias::ConstantBias& biasHat = gtsam::imuBias::ConstantBias());
+  ~ReintegratedImuMeasurements() override;
+
+  void resetIntegration() override;
+  void integrateMeasurement(const gtsam::Vector3& measuredAcc, const gtsam::Vector3& measuredOmega, double dt) override;
+
+  const Eigen::Vector3d mean_acc() const;
+  const Eigen::Vector3d mean_gyro() const;
+
+public:
+  std::vector<gtsam::Vector7> imu_data;  // [a, w, dt]
+};
+/**
+ * @brief "Re"-integrated IMU factor (IMU factor without pre-integration).
+ *        This factor re-integrates IMU measurements every linearization to better estimate the IMU bias.
+ */
+class ReintegratedImuFactor : public gtsam::NonlinearFactor {
+public:
+  ReintegratedImuFactor(gtsam::Key pose_i, gtsam::Key vel_i, gtsam::Key pose_j, gtsam::Key vel_j, gtsam::Key bias, const ReintegratedImuMeasurements& imu_measurements);
+  ~ReintegratedImuFactor() override;
+
+  size_t dim() const override { return 9; }
+
+  boost::shared_ptr<gtsam::GaussianFactor> linearize(const gtsam::Values& values) const override;
+  double error(const gtsam::Values& values) const override;
+
+  const ReintegratedImuMeasurements& measurements() const { return imu_measurements; }
+
+private:
+  boost::shared_ptr<gtsam::ImuFactor> create_imu_factor(const gtsam::imuBias::ConstantBias& bias) const;
+
+private:
+  const ReintegratedImuMeasurements imu_measurements;
+  mutable boost::shared_ptr<gtsam::ImuFactor> imu_factor;
+};
+}  // namespace gtsam_points

src/gtsam_points/factors/reintegrated_imu_factor.cpp

@@ -0,0 +1,77 @@
+// SPDX-License-Identifier: MIT
+// Copyright (c) 2024  Kenji Koide ([email protected])
+#include <gtsam_points/factors/reintegrated_imu_factor.hpp>
+
+namespace gtsam_points {
+
+ReintegratedImuMeasurements::ReintegratedImuMeasurements(const boost::shared_ptr<gtsam::PreintegrationParams>& p, const gtsam::imuBias::ConstantBias& biasHat)
+: gtsam::PreintegratedImuMeasurements(p, biasHat) {}
+
+ReintegratedImuMeasurements::~ReintegratedImuMeasurements() {}
+
+void ReintegratedImuMeasurements::resetIntegration() {
+  gtsam::PreintegratedImuMeasurements::resetIntegration();
+  imu_data.clear();
+}
+
+void ReintegratedImuMeasurements::integrateMeasurement(const gtsam::Vector3& measuredAcc, const gtsam::Vector3& measuredOmega, double dt) {
+  gtsam::PreintegratedImuMeasurements::integrateMeasurement(measuredAcc, measuredOmega, dt);
+  gtsam::Vector7 imu;
+  imu << measuredAcc, measuredOmega, dt;
+  imu_data.emplace_back(imu);
+}
+
+const Eigen::Vector3d ReintegratedImuMeasurements::mean_acc() const {
+  Eigen::Vector3d sum_acc = Eigen::Vector3d::Zero();
+  for (const auto& imu : imu_data) {
+    sum_acc += imu.block<3, 1>(0, 0);
+  }
+  return sum_acc / imu_data.size();
+}
+
+const Eigen::Vector3d ReintegratedImuMeasurements::mean_gyro() const {
+  Eigen::Vector3d sum_gyro = Eigen::Vector3d::Zero();
+  for (const auto& imu : imu_data) {
+    sum_gyro += imu.block<3, 1>(3, 0);
+  }
+  return sum_gyro / imu_data.size();
+}
+
+ReintegratedImuFactor::ReintegratedImuFactor(
+  gtsam::Key pose_i,
+  gtsam::Key vel_i,
+  gtsam::Key pose_j,
+  gtsam::Key vel_j,
+  gtsam::Key bias,
+  const ReintegratedImuMeasurements& imu_measurements)
+: gtsam::NonlinearFactor(gtsam::KeyVector{pose_i, vel_i, pose_j, vel_j, bias}),
+  imu_measurements(imu_measurements) {}
+
+ReintegratedImuFactor::~ReintegratedImuFactor() {}
+
+boost::shared_ptr<gtsam::GaussianFactor> ReintegratedImuFactor::linearize(const gtsam::Values& values) const {
+  imu_factor = create_imu_factor(values.at<gtsam::imuBias::ConstantBias>(keys()[4]));
+  return imu_factor->linearize(values);
+}
+
+double ReintegratedImuFactor::error(const gtsam::Values& values) const {
+  if (!imu_factor) {
+    imu_factor = create_imu_factor(values.at<gtsam::imuBias::ConstantBias>(keys()[4]));
+  }
+
+  return imu_factor->error(values);
+}
+
+boost::shared_ptr<gtsam::ImuFactor> ReintegratedImuFactor::create_imu_factor(const gtsam::imuBias::ConstantBias& bias) const {
+  gtsam::PreintegratedImuMeasurements pim(imu_measurements.params(), bias);
+  for (const auto& imu : imu_measurements.imu_data) {
+    const gtsam::Vector3 acc = imu.block<3, 1>(0, 0);
+    const gtsam::Vector3 gyro = imu.block<3, 1>(3, 0);
+    const double dt = imu[6];
+    pim.integrateMeasurement(acc, gyro, dt);
+  }
+
+  return gtsam::make_shared<gtsam::ImuFactor>(keys()[0], keys()[1], keys()[2], keys()[3], keys()[4], pim);
+}
+
+}  // namespace gtsam_points