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 NoiseModelFactorN<POSE, POSE> {
24 public:
27 
28 protected:
29 
32  double dt_;
33 
34 public:
35 
36  // Provide access to the Matrix& version of evaluateError:
37  using Base::evaluateError;
38 
41  double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model)
42  : Base(model, key1, key2), accel_(accel), gyro_(gyro), dt_(dt) {
43  assert(model->dim() == 9);
44  }
45 
47  FullIMUFactor(const Vector6& imu,
48  double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model)
49  : Base(model, key1, key2), accel_(imu.head(3)), gyro_(imu.tail(3)), dt_(dt) {
50  assert(imu.size() == 6);
51  assert(model->dim() == 9);
52  }
53 
54  ~FullIMUFactor() override {}
55 
58  return std::static_pointer_cast<gtsam::NonlinearFactor>(
60 
62  bool equals(const NonlinearFactor& e, double tol = 1e-9) const override {
63  const This* const f = dynamic_cast<const This*>(&e);
64  return f && Base::equals(e) &&
65  equal_with_abs_tol(accel_, f->accel_, tol) &&
66  equal_with_abs_tol(gyro_, f->gyro_, tol) &&
67  std::abs(dt_ - f->dt_) < tol;
68  }
69 
70  void print(const std::string& s="", const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const override {
71  std::string a = "FullIMUFactor: " + s;
73  gtsam::print((Vector)accel_, "accel");
74  gtsam::print((Vector)gyro_, "gyro");
75  std::cout << "dt: " << dt_ << std::endl;
76  }
77 
78  // access
79  const Vector3& gyro() const { return gyro_; }
80  const Vector3& accel() const { return accel_; }
81  Vector6 z() const { return (Vector(6) << accel_, gyro_).finished(); }
82 
88  OptionalMatrixType H1, OptionalMatrixType H2) const override {
89  Vector9 z;
90  z.head(3).operator=(accel_); // Strange syntax to work around ambiguous operator error with clang
91  z.segment(3, 3).operator=(gyro_); // Strange syntax to work around ambiguous operator error with clang
92  z.tail(3).operator=(x2.t()); // Strange syntax to work around ambiguous operator error with clang
93  if (H1) *H1 = numericalDerivative21<Vector9, PoseRTV, PoseRTV>(
94  std::bind(This::predict_proxy, std::placeholders::_1, std::placeholders::_2, dt_), x1, x2, 1e-5);
95  if (H2) *H2 = numericalDerivative22<Vector9, PoseRTV, PoseRTV>(
96  std::bind(This::predict_proxy, std::placeholders::_1, std::placeholders::_2, dt_), x1, x2, 1e-5);
97  return z - predict_proxy(x1, x2, dt_);
98  }
99 
101  virtual Vector evaluateError(const Pose3& x1, const Pose3& x2,
102  OptionalMatrixType H1, OptionalMatrixType H2) const {
103  assert(false);
104  return Vector6::Zero();
105  }
106 
107 private:
108 
110  static Vector9 predict_proxy(const PoseRTV& x1, const PoseRTV& x2, double dt) {
111  Vector9 hx;
112  hx.head(6).operator=(x1.imuPrediction(x2, dt)); // Strange syntax to work around ambiguous operator error with clang
113  hx.tail(3).operator=(x1.translationIntegration(x2, dt)); // Strange syntax to work around ambiguous operator error with clang
114  return hx;
115  }
116 };
117 
118 } // \namespace gtsam
gtsam::NonlinearFactor::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: NonlinearFactor.h:78
gtsam::FullIMUFactor::evaluateError
Vector evaluateError(const PoseRTV &x1, const PoseRTV &x2, OptionalMatrixType H1, OptionalMatrixType H2) const override
Definition: FullIMUFactor.h:87
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:80
gtsam::FullIMUFactor::print
void print(const std::string &s="", const gtsam::KeyFormatter &formatter=gtsam::DefaultKeyFormatter) const override
Definition: FullIMUFactor.h:70
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:110
gtsam::FullIMUFactor::z
Vector6 z() const
Definition: FullIMUFactor.h:81
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:43
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::FullIMUFactor::FullIMUFactor
FullIMUFactor(const Vector6 &imu, double dt, const Key &key1, const Key &key2, const SharedNoiseModel &model)
Definition: FullIMUFactor.h:47
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:40
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
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:431
gtsam::NoiseModelFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: NonlinearFactor.cpp:72
gtsam::FullIMUFactor::dt_
double dt_
Definition: FullIMUFactor.h:32
PoseRTV.h
Pose3 with translational velocity.
gtsam::NoiseModelFactor::equals
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
Definition: NonlinearFactor.cpp:80
gtsam::FullIMUFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: FullIMUFactor.h:57
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
gtsam::NoiseModelFactorN< POSE, POSE >::key1
Key key1() const
Definition: NonlinearFactor.h:731
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:31
gtsam::PoseRTV
Definition: PoseRTV.h:23
gtsam::FullIMUFactor::~FullIMUFactor
~FullIMUFactor() override
Definition: FullIMUFactor.h:54
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:735
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:62
gtsam
traits
Definition: SFMdata.h:40
gtsam::FullIMUFactor::accel_
Vector3 accel_
Definition: FullIMUFactor.h:31
gtsam::FullIMUFactor::This
FullIMUFactor< POSE > This
Definition: FullIMUFactor.h:26
gtsam::NonlinearFactor
Definition: NonlinearFactor.h:68
gtsam::FullIMUFactor::evaluateError
virtual Vector evaluateError(const Pose3 &x1, const Pose3 &x2, OptionalMatrixType H1, OptionalMatrixType H2) const
Definition: FullIMUFactor.h:101
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::FullIMUFactor::gyro
const Vector3 & gyro() const
Definition: FullIMUFactor.h:79
gtsam::OptionalMatrixType
Matrix * OptionalMatrixType
Definition: NonlinearFactor.h:55
gtsam::FullIMUFactor
Definition: FullIMUFactor.h:23
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:25


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:32