IMUFactor.h

#pragma once

#include <gtsam/base/numericalDerivative.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam_unstable/dynamics/PoseRTV.h>

namespace gtsam {

template<class POSE>
class IMUFactor : public NoiseModelFactor2<POSE, POSE> {
public:
  typedef NoiseModelFactor2<POSE, POSE> Base;
  typedef IMUFactor<POSE> This;

protected:

  Vector3 accel_, gyro_;
  double dt_; 

public:

  IMUFactor(const Vector3& accel, const Vector3& gyro,
      double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model)
  : Base(model, key1, key2), accel_(accel), gyro_(gyro), dt_(dt) {}

  IMUFactor(const Vector6& imu_vector,
      double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model)
  : Base(model, key1, key2), accel_(imu_vector.head(3)), gyro_(imu_vector.tail(3)), dt_(dt) {}

  virtual ~IMUFactor() {}

  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
    return boost::static_pointer_cast<gtsam::NonlinearFactor>(
        gtsam::NonlinearFactor::shared_ptr(new This(*this))); }

  virtual bool equals(const NonlinearFactor& e, double tol = 1e-9) const {
    const This* const f = dynamic_cast<const This*>(&e);
    return f && Base::equals(e) &&
        equal_with_abs_tol(accel_, f->accel_, tol) &&
        equal_with_abs_tol(gyro_, f->gyro_, tol) &&
        fabs(dt_ - f->dt_) < tol;
  }

  void print(const std::string& s="", const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const {
    std::string a = "IMUFactor: " + s;
    Base::print(a, formatter);
    gtsam::print((Vector)accel_, "accel");
    gtsam::print((Vector)gyro_, "gyro");
    std::cout << "dt: " << dt_ << std::endl;
  }

  // access
  const Vector3& gyro() const { return gyro_; }
  const Vector3& accel() const { return accel_; }
  Vector6 z() const { return (Vector6() << accel_, gyro_).finished(); }

  virtual Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2,
      boost::optional<Matrix&> H1 = boost::none,
      boost::optional<Matrix&> H2 = boost::none) const {
    const Vector6 meas = z();
    if (H1) *H1 = numericalDerivative21<Vector6, PoseRTV, PoseRTV>(
        boost::bind(This::predict_proxy, _1, _2, dt_, meas), x1, x2, 1e-5);
    if (H2) *H2 = numericalDerivative22<Vector6, PoseRTV, PoseRTV>(
        boost::bind(This::predict_proxy, _1, _2, dt_, meas), x1, x2, 1e-5);
    return predict_proxy(x1, x2, dt_, meas);
  }

  virtual Vector evaluateError(const Pose3& x1, const Pose3& x2,
      boost::optional<Matrix&> H1 = boost::none,
      boost::optional<Matrix&> H2 = boost::none) const {
    assert(false); // no corresponding factor here
    return Vector6::Zero();
  }

private:
  static Vector6 predict_proxy(const PoseRTV& x1, const PoseRTV& x2,
      double dt, const Vector6& meas) {
    Vector6 hx = x1.imuPrediction(x2, dt);
    return meas - hx;
  }
};

} // \namespace gtsam