gtsam  4.0.0
gtsam
FullIMUFactor.h
Go to the documentation of this file.
1 
7 #pragma once
8 
12 
13 namespace gtsam {
14 
22 template<class POSE>
23 class FullIMUFactor : public NoiseModelFactor2<POSE, POSE> {
24 public:
26  typedef FullIMUFactor<POSE> This;
27 
28 protected:
29 
31  Vector3 accel_, gyro_;
32  double dt_;
33 
34 public:
35 
37  FullIMUFactor(const Vector3& accel, const Vector3& gyro,
38  double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model)
39  : Base(model, key1, key2), accel_(accel), gyro_(gyro), dt_(dt) {
40  assert(model->dim() == 9);
41  }
42 
44  FullIMUFactor(const Vector6& imu,
45  double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model)
46  : Base(model, key1, key2), accel_(imu.head(3)), gyro_(imu.tail(3)), dt_(dt) {
47  assert(imu.size() == 6);
48  assert(model->dim() == 9);
49  }
50 
51  virtual ~FullIMUFactor() {}
52 
54  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
55  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
56  gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
57 
59  virtual bool equals(const NonlinearFactor& e, double tol = 1e-9) const {
60  const This* const f = dynamic_cast<const This*>(&e);
61  return f && Base::equals(e) &&
62  equal_with_abs_tol(accel_, f->accel_, tol) &&
63  equal_with_abs_tol(gyro_, f->gyro_, tol) &&
64  fabs(dt_ - f->dt_) < tol;
65  }
66 
67  void print(const std::string& s="", const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const {
68  std::string a = "FullIMUFactor: " + s;
69  Base::print(a, formatter);
70  gtsam::print((Vector)accel_, "accel");
71  gtsam::print((Vector)gyro_, "gyro");
72  std::cout << "dt: " << dt_ << std::endl;
73  }
74 
75  // access
76  const Vector3& gyro() const { return gyro_; }
77  const Vector3& accel() const { return accel_; }
78  Vector6 z() const { return (Vector(6) << accel_, gyro_).finished(); }
79 
84  virtual Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2,
85  boost::optional<Matrix&> H1 = boost::none,
86  boost::optional<Matrix&> H2 = boost::none) const {
87  Vector9 z;
88  z.head(3).operator=(accel_); // Strange syntax to work around ambiguous operator error with clang
89  z.segment(3, 3).operator=(gyro_); // Strange syntax to work around ambiguous operator error with clang
90  z.tail(3).operator=(x2.t()); // Strange syntax to work around ambiguous operator error with clang
91  if (H1) *H1 = numericalDerivative21<Vector9, PoseRTV, PoseRTV>(
92  boost::bind(This::predict_proxy, _1, _2, dt_), x1, x2, 1e-5);
93  if (H2) *H2 = numericalDerivative22<Vector9, PoseRTV, PoseRTV>(
94  boost::bind(This::predict_proxy, _1, _2, dt_), x1, x2, 1e-5);
95  return z - predict_proxy(x1, x2, dt_);
96  }
97 
99  virtual Vector evaluateError(const Pose3& x1, const Pose3& x2,
100  boost::optional<Matrix&> H1 = boost::none,
101  boost::optional<Matrix&> H2 = boost::none) const {
102  assert(false);
103  return Vector6::Zero();
104  }
105 
106 private:
107 
109  static Vector9 predict_proxy(const PoseRTV& x1, const PoseRTV& x2, double dt) {
110  Vector9 hx;
111  hx.head(6).operator=(x1.imuPrediction(x2, dt)); // Strange syntax to work around ambiguous operator error with clang
112  hx.tail(3).operator=(x1.translationIntegration(x2, dt)); // Strange syntax to work around ambiguous operator error with clang
113  return hx;
114  }
115 };
116 
117 } // \namespace gtsam
This is the base class for all factor types.
Definition: Factor.h:54
A convenient base class for creating your own NoiseModelFactor with 2 variables.
Definition: NonlinearFactor.h:345
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:141
virtual void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Print.
Definition: NonlinearFactor.cpp:63
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:82
Robot state for use with IMU measurements.
Definition: PoseRTV.h:23
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: FullIMUFactor.h:99
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:57
virtual Vector evaluateError(const PoseRTV &x1, const PoseRTV &x2, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const
Error evaluation with optional derivatives - calculates z - h(x1,x2)
Definition: FullIMUFactor.h:84
Class that represents integrating IMU measurements over time for dynamic systems This factor has dime...
Definition: FullIMUFactor.h:23
Pose3 with translational velocity.
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:33
Nonlinear factor base class.
Definition: NonlinearFactor.h:50
void print(const std::string &s="", const gtsam::KeyFormatter &formatter=gtsam::DefaultKeyFormatter) const
Print.
Definition: FullIMUFactor.h:67
Point3 translationIntegration(const Rot3 &r2, const Velocity3 &v2, double dt) const
predict measurement and where Point3 for x2 should be, as a way of enforcing a velocity constraint Th...
Definition: PoseRTV.cpp:161
Vector3 accel_
measurements from the IMU
Definition: FullIMUFactor.h:31
virtual gtsam::NonlinearFactor::shared_ptr clone() const
Definition: FullIMUFactor.h:54
Non-linear factor base classes.
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
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
noiseModel::Base::shared_ptr SharedNoiseModel
Note, deliberately not in noiseModel namespace.
Definition: NoiseModel.h:1072
FullIMUFactor(const Vector6 &imu, double dt, const Key &key1, const Key &key2, const SharedNoiseModel &model)
Single IMU vector - imu = [accel, gyro].
Definition: FullIMUFactor.h:44
virtual bool equals(const NonlinearFactor &e, double tol=1e-9) const
Check if two factors are equal.
Definition: FullIMUFactor.h:59
Some functions to compute numerical derivatives.
Definition: Pose3.h:37
virtual bool equals(const NonlinearFactor &f, double tol=1e-9) const
Check if two factors are equal.
Definition: NonlinearFactor.cpp:71
FullIMUFactor(const Vector3 &accel, const Vector3 &gyro, double dt, const Key &key1, const Key &key2, const SharedNoiseModel &model)
time between measurements
Definition: FullIMUFactor.h:37