gtsam  4.1.0
gtsam
IMUFactor.h
Go to the documentation of this file.
1 
7 #pragma once
8 
12 
13 namespace gtsam {
14 
20 template<class POSE>
21 class IMUFactor : public NoiseModelFactor2<POSE, POSE> {
22 public:
24  typedef IMUFactor<POSE> This;
25 
26 protected:
27 
29  Vector3 accel_, gyro_;
30  double dt_;
31 
32 public:
33 
35  IMUFactor(const Vector3& accel, const Vector3& gyro,
36  double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model)
37  : Base(model, key1, key2), accel_(accel), gyro_(gyro), dt_(dt) {}
38 
40  IMUFactor(const Vector6& imu_vector,
41  double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model)
42  : Base(model, key1, key2), accel_(imu_vector.head(3)), gyro_(imu_vector.tail(3)), dt_(dt) {}
43 
44  virtual ~IMUFactor() {}
45 
47  gtsam::NonlinearFactor::shared_ptr clone() const override {
48  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
49  gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
50 
52  bool equals(const NonlinearFactor& e, double tol = 1e-9) const override {
53  const This* const f = dynamic_cast<const This*>(&e);
54  return f && Base::equals(e) &&
55  equal_with_abs_tol(accel_, f->accel_, tol) &&
56  equal_with_abs_tol(gyro_, f->gyro_, tol) &&
57  std::abs(dt_ - f->dt_) < tol;
58  }
59 
60  void print(const std::string& s="", const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const override {
61  std::string a = "IMUFactor: " + s;
62  Base::print(a, formatter);
63  gtsam::print((Vector)accel_, "accel");
64  gtsam::print((Vector)gyro_, "gyro");
65  std::cout << "dt: " << dt_ << std::endl;
66  }
67 
68  // access
69  const Vector3& gyro() const { return gyro_; }
70  const Vector3& accel() const { return accel_; }
71  Vector6 z() const { return (Vector6() << accel_, gyro_).finished(); }
72 
77  Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2,
78  boost::optional<Matrix&> H1 = boost::none,
79  boost::optional<Matrix&> H2 = boost::none) const override {
80  const Vector6 meas = z();
81  if (H1) *H1 = numericalDerivative21<Vector6, PoseRTV, PoseRTV>(
82  boost::bind(This::predict_proxy, _1, _2, dt_, meas), x1, x2, 1e-5);
83  if (H2) *H2 = numericalDerivative22<Vector6, PoseRTV, PoseRTV>(
84  boost::bind(This::predict_proxy, _1, _2, dt_, meas), x1, x2, 1e-5);
85  return predict_proxy(x1, x2, dt_, meas);
86  }
87 
89  virtual Vector evaluateError(const Pose3& x1, const Pose3& x2,
90  boost::optional<Matrix&> H1 = boost::none,
91  boost::optional<Matrix&> H2 = boost::none) const {
92  assert(false); // no corresponding factor here
93  return Vector6::Zero();
94  }
95 
96 private:
98  static Vector6 predict_proxy(const PoseRTV& x1, const PoseRTV& x2,
99  double dt, const Vector6& meas) {
100  Vector6 hx = x1.imuPrediction(x2, dt);
101  return meas - hx;
102  }
103 };
104 
105 } // \namespace gtsam
gtsam::IMUFactor
Class that represents integrating IMU measurements over time for dynamic systems Templated to allow f...
Definition: IMUFactor.h:21
gtsam::equal_with_abs_tol
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
equals with a tolerance
Definition: Matrix.h:84
gtsam::NoiseModelFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Print.
Definition: NonlinearFactor.cpp:63
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Note, deliberately not in noiseModel namespace.
Definition: NoiseModel.h:734
numericalDerivative.h
Some functions to compute numerical derivatives.
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::NoiseModelFactor2< POSE, POSE >::key1
Key key1() const
methods to retrieve both keys
Definition: NonlinearFactor.h:387
gtsam::IMUFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: IMUFactor.h:47
gtsam::NoiseModelFactor::equals
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
Check if two factors are equal.
Definition: NonlinearFactor.cpp:71
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
PoseRTV.h
Pose3 with translational velocity.
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:155
gtsam::IMUFactor::IMUFactor
IMUFactor(const Vector3 &accel, const Vector3 &gyro, double dt, const Key &key1, const Key &key2, const SharedNoiseModel &model)
time between measurements
Definition: IMUFactor.h:35
gtsam::PoseRTV::imuPrediction
Vector6 imuPrediction(const PoseRTV &x2, double dt) const
Dynamics predictor for both ground and flying robots, given states at 1 and 2 Always move from time 1...
Definition: PoseRTV.cpp:133
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition: NonlinearFactor.h:43
NonlinearFactor.h
Non-linear factor base classes.
gtsam::IMUFactor::accel_
Vector3 accel_
measurements from the IMU
Definition: IMUFactor.h:29
gtsam::IMUFactor::print
void print(const std::string &s="", const gtsam::KeyFormatter &formatter=gtsam::DefaultKeyFormatter) const override
Print.
Definition: IMUFactor.h:60
gtsam::IMUFactor::equals
bool equals(const NonlinearFactor &e, double tol=1e-9) const override
Check if two factors are equal.
Definition: IMUFactor.h:52
gtsam::Pose3
Definition: Pose3.h:37
gtsam::KeyFormatter
boost::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition: Key.h:35
gtsam::Factor
This is the base class for all factor types.
Definition: Factor.h:55
gtsam::NoiseModelFactor2
A convenient base class for creating your own NoiseModelFactor with 2 variables.
Definition: NonlinearFactor.h:355
gtsam::IMUFactor::evaluateError
Vector evaluateError(const PoseRTV &x1, const PoseRTV &x2, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
Error evaluation with optional derivatives - calculates z - h(x1,x2)
Definition: IMUFactor.h:77
gtsam::IMUFactor::evaluateError
virtual Vector evaluateError(const Pose3 &x1, const Pose3 &x2, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const
dummy version that fails for non-dynamic poses
Definition: IMUFactor.h:89
gtsam::PoseRTV
Robot state for use with IMU measurements.
Definition: PoseRTV.h:23
gtsam::IMUFactor::IMUFactor
IMUFactor(const Vector6 &imu_vector, double dt, const Key &key1, const Key &key2, const SharedNoiseModel &model)
Full IMU vector specification.
Definition: IMUFactor.h:40