FullIMUFactor.h
Go to the documentation of this file.
1 
7 #pragma once
8 
12 
13 #include <cassert>
14 
15 namespace gtsam {
16 
24 template<class POSE>
25 class FullIMUFactor : public NoiseModelFactorN<POSE, POSE> {
26 public:
29 
30 protected:
31 
34  double dt_;
35 
36 public:
37 
38  // Provide access to the Matrix& version of evaluateError:
39  using Base::evaluateError;
40 
43  double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model)
44  : Base(model, key1, key2), accel_(accel), gyro_(gyro), dt_(dt) {
45  assert(model->dim() == 9);
46  }
47 
49  FullIMUFactor(const Vector6& imu,
50  double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model)
51  : Base(model, key1, key2), accel_(imu.head(3)), gyro_(imu.tail(3)), dt_(dt) {
52  assert(imu.size() == 6);
53  assert(model->dim() == 9);
54  }
55 
56  ~FullIMUFactor() override {}
57 
60  return std::static_pointer_cast<gtsam::NonlinearFactor>(
62 
64  bool equals(const NonlinearFactor& e, double tol = 1e-9) const override {
65  const This* const f = dynamic_cast<const This*>(&e);
66  return f && Base::equals(e) &&
67  equal_with_abs_tol(accel_, f->accel_, tol) &&
68  equal_with_abs_tol(gyro_, f->gyro_, tol) &&
69  std::abs(dt_ - f->dt_) < tol;
70  }
71 
72  void print(const std::string& s="", const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const override {
73  std::string a = "FullIMUFactor: " + s;
75  gtsam::print((Vector)accel_, "accel");
76  gtsam::print((Vector)gyro_, "gyro");
77  std::cout << "dt: " << dt_ << std::endl;
78  }
79 
80  // access
81  const Vector3& gyro() const { return gyro_; }
82  const Vector3& accel() const { return accel_; }
83  Vector6 z() const { return (Vector(6) << accel_, gyro_).finished(); }
84 
90  OptionalMatrixType H1, OptionalMatrixType H2) const override {
91  Vector9 z;
92  z.head(3).operator=(accel_); // Strange syntax to work around ambiguous operator error with clang
93  z.segment(3, 3).operator=(gyro_); // Strange syntax to work around ambiguous operator error with clang
94  z.tail(3).operator=(x2.t()); // Strange syntax to work around ambiguous operator error with clang
95  if (H1) *H1 = numericalDerivative21<Vector9, PoseRTV, PoseRTV>(
96  std::bind(This::predict_proxy, std::placeholders::_1, std::placeholders::_2, dt_), x1, x2, 1e-5);
97  if (H2) *H2 = numericalDerivative22<Vector9, PoseRTV, PoseRTV>(
98  std::bind(This::predict_proxy, std::placeholders::_1, std::placeholders::_2, dt_), x1, x2, 1e-5);
99  return z - predict_proxy(x1, x2, dt_);
100  }
101 
103  virtual Vector evaluateError(const Pose3& x1, const Pose3& x2,
104  OptionalMatrixType H1, OptionalMatrixType H2) const {
105  assert(false);
106  return Vector6::Zero();
107  }
108 
109 private:
110 
112  static Vector9 predict_proxy(const PoseRTV& x1, const PoseRTV& x2, double dt) {
113  Vector9 hx;
114  hx.head(6).operator=(x1.imuPrediction(x2, dt)); // Strange syntax to work around ambiguous operator error with clang
115  hx.tail(3).operator=(x1.translationIntegration(x2, dt)); // Strange syntax to work around ambiguous operator error with clang
116  return hx;
117  }
118 };
119 
120 } // \namespace gtsam
gtsam::NonlinearFactor::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: NonlinearFactor.h:79
gtsam::FullIMUFactor::evaluateError
Vector evaluateError(const PoseRTV &x1, const PoseRTV &x2, OptionalMatrixType H1, OptionalMatrixType H2) const override
Definition: FullIMUFactor.h:89
tail
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE FixedSegmentReturnType< internal::get_fixed_value< NType >::value >::Type tail(NType n)
Definition: BlockMethods.h:1257
gtsam::FullIMUFactor::accel
const Vector3 & accel() const
Definition: FullIMUFactor.h:82
gtsam::FullIMUFactor::print
void print(const std::string &s="", const gtsam::KeyFormatter &formatter=gtsam::DefaultKeyFormatter) const override
Definition: FullIMUFactor.h:72
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::NoiseModelFactorN< POSE, POSE >::evaluateError
virtual Vector evaluateError(const ValueTypes &... x, OptionalMatrixTypeT< ValueTypes >... H) const=0
gtsam::FullIMUFactor::predict_proxy
static Vector9 predict_proxy(const PoseRTV &x1, const PoseRTV &x2, double dt)
Definition: FullIMUFactor.h:112
gtsam::FullIMUFactor::z
Vector6 z() const
Definition: FullIMUFactor.h:83
dt
const double dt
Definition: testVelocityConstraint.cpp:15
formatter
const KeyFormatter & formatter
Definition: treeTraversal-inst.h:204
gtsam::equal_with_abs_tol
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
Definition: base/Matrix.h:80
gtsam::Factor
Definition: Factor.h:70
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:44
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:39
gtsam::FullIMUFactor::FullIMUFactor
FullIMUFactor(const Vector6 &imu, double dt, const Key &key1, const Key &key2, const SharedNoiseModel &model)
Definition: FullIMUFactor.h:49
gtsam::DefaultKeyFormatter
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Definition: Key.cpp:30
gtsam::FullIMUFactor::FullIMUFactor
FullIMUFactor(const Vector3 &accel, const Vector3 &gyro, double dt, const Key &key1, const Key &key2, const SharedNoiseModel &model)
Definition: FullIMUFactor.h:42
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:156
numericalDerivative.h
Some functions to compute numerical derivatives.
x1
Pose3 x1
Definition: testPose3.cpp:663
gtsam::KeyFormatter
std::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::Pose3
Definition: Pose3.h:37
gtsam::NoiseModelFactorN
Definition: NonlinearFactor.h:432
gtsam::NoiseModelFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: NonlinearFactor.cpp:74
gtsam::FullIMUFactor::dt_
double dt_
Definition: FullIMUFactor.h:34
PoseRTV.h
Pose3 with translational velocity.
gtsam::NoiseModelFactor::equals
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
Definition: NonlinearFactor.cpp:82
gtsam::FullIMUFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: FullIMUFactor.h:59
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
gtsam::NoiseModelFactorN< POSE, POSE >::key1
Key key1() const
Definition: NonlinearFactor.h:732
head
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE FixedSegmentReturnType< internal::get_fixed_value< NType >::value >::Type head(NType n)
Definition: BlockMethods.h:1208
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
gtsam::FullIMUFactor::gyro_
Vector3 gyro_
Definition: FullIMUFactor.h:33
gtsam::PoseRTV
Definition: PoseRTV.h:23
gtsam::FullIMUFactor::~FullIMUFactor
~FullIMUFactor() override
Definition: FullIMUFactor.h:56
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
NonlinearFactor.h
Non-linear factor base classes.
gtsam::NoiseModelFactorN< POSE, POSE >::key2
Key key2() const
Definition: NonlinearFactor.h:736
a
ArrayXXi a
Definition: Array_initializer_list_23_cxx11.cpp:1
gtsam::FullIMUFactor::equals
bool equals(const NonlinearFactor &e, double tol=1e-9) const override
Definition: FullIMUFactor.h:64
gtsam
traits
Definition: SFMdata.h:40
gtsam::FullIMUFactor::accel_
Vector3 accel_
Definition: FullIMUFactor.h:33
gtsam::FullIMUFactor::This
FullIMUFactor< POSE > This
Definition: FullIMUFactor.h:28
gtsam::NonlinearFactor
Definition: NonlinearFactor.h:69
gtsam::FullIMUFactor::evaluateError
virtual Vector evaluateError(const Pose3 &x1, const Pose3 &x2, OptionalMatrixType H1, OptionalMatrixType H2) const
Definition: FullIMUFactor.h:103
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::FullIMUFactor::gyro
const Vector3 & gyro() const
Definition: FullIMUFactor.h:81
gtsam::OptionalMatrixType
Matrix * OptionalMatrixType
Definition: NonlinearFactor.h:56
gtsam::FullIMUFactor
Definition: FullIMUFactor.h:25
abs
#define abs(x)
Definition: datatypes.h:17
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
x2
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
gtsam::FullIMUFactor::Base
NoiseModelFactorN< POSE, POSE > Base
Definition: FullIMUFactor.h:27


gtsam
Author(s):
autogenerated on Sun Dec 22 2024 04:11:32