gtsam  4.1.0
gtsam
ImuBias.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
18 #pragma once
19 
21 #include <gtsam/base/VectorSpace.h>
22 #include <iosfwd>
23 #include <boost/serialization/nvp.hpp>
24 
25 namespace gtsam {
26 
28 namespace imuBias {
29 
30 class GTSAM_EXPORT ConstantBias {
31 private:
32  Vector3 biasAcc_;
33  Vector3 biasGyro_;
34 
35 public:
37  static const size_t dimension = 6;
38 
39  ConstantBias() :
40  biasAcc_(0.0, 0.0, 0.0), biasGyro_(0.0, 0.0, 0.0) {
41  }
42 
43  ConstantBias(const Vector3& biasAcc, const Vector3& biasGyro) :
44  biasAcc_(biasAcc), biasGyro_(biasGyro) {
45  }
46 
47  explicit ConstantBias(const Vector6& v) :
48  biasAcc_(v.head<3>()), biasGyro_(v.tail<3>()) {
49  }
50 
52  Vector6 vector() const {
53  Vector6 v;
54  v << biasAcc_, biasGyro_;
55  return v;
56  }
57 
59  const Vector3& accelerometer() const {
60  return biasAcc_;
61  }
62 
64  const Vector3& gyroscope() const {
65  return biasGyro_;
66  }
67 
69  Vector3 correctAccelerometer(const Vector3& measurement,
70  OptionalJacobian<3, 6> H1 = boost::none,
71  OptionalJacobian<3, 3> H2 = boost::none) const {
72  if (H1) (*H1) << -I_3x3, Z_3x3;
73  if (H2) (*H2) << I_3x3;
74  return measurement - biasAcc_;
75  }
76 
78  Vector3 correctGyroscope(const Vector3& measurement,
79  OptionalJacobian<3, 6> H1 = boost::none,
80  OptionalJacobian<3, 3> H2 = boost::none) const {
81  if (H1) (*H1) << Z_3x3, -I_3x3;
82  if (H2) (*H2) << I_3x3;
83  return measurement - biasGyro_;
84  }
85 
89 
91  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
92  const ConstantBias& bias);
93 
95  void print(const std::string& s = "") const;
96 
98  inline bool equals(const ConstantBias& expected, double tol = 1e-5) const {
99  return equal_with_abs_tol(biasAcc_, expected.biasAcc_, tol)
100  && equal_with_abs_tol(biasGyro_, expected.biasGyro_, tol);
101  }
102 
106 
109  return ConstantBias();
110  }
111 
113  inline ConstantBias operator-() const {
114  return ConstantBias(-biasAcc_, -biasGyro_);
115  }
116 
118  ConstantBias operator+(const Vector6& v) const {
119  return ConstantBias(biasAcc_ + v.head<3>(), biasGyro_ + v.tail<3>());
120  }
121 
124  return ConstantBias(biasAcc_ + b.biasAcc_, biasGyro_ + b.biasGyro_);
125  }
126 
129  return ConstantBias(biasAcc_ - b.biasAcc_, biasGyro_ - b.biasGyro_);
130  }
131 
133 
136  ConstantBias inverse() {
137  return -(*this);
138  }
139  ConstantBias compose(const ConstantBias& q) {
140  return (*this) + q;
141  }
142  ConstantBias between(const ConstantBias& q) {
143  return q - (*this);
144  }
145  Vector6 localCoordinates(const ConstantBias& q) {
146  return between(q).vector();
147  }
148  ConstantBias retract(const Vector6& v) {
149  return compose(ConstantBias(v));
150  }
151  static Vector6 Logmap(const ConstantBias& p) {
152  return p.vector();
153  }
154  static ConstantBias Expmap(const Vector6& v) {
155  return ConstantBias(v);
156  }
158 
159 private:
160 
163 
165  friend class boost::serialization::access;
166  template<class ARCHIVE>
167  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
168  ar & BOOST_SERIALIZATION_NVP(biasAcc_);
169  ar & BOOST_SERIALIZATION_NVP(biasGyro_);
170  }
171 
172 
173 public:
176 
177 }; // ConstantBias class
178 } // namespace imuBias
179 
180 template<>
181 struct traits<imuBias::ConstantBias> : public internal::VectorSpace<
182  imuBias::ConstantBias> {
183 };
184 
185 } // namespace gtsam
186 
gtsam::imuBias::operator<<
std::ostream & operator<<(std::ostream &os, const ConstantBias &bias)
ostream operator
Definition: ImuBias.cpp:68
gtsam::imuBias::ConstantBias::operator-
ConstantBias operator-() const
inverse
Definition: ImuBias.h:113
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::imuBias::ConstantBias
Definition: ImuBias.h:30
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition: OptionalJacobian.h:39
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
gtsam::serialize
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:100
gtsam::imuBias::ConstantBias::accelerometer
const Vector3 & accelerometer() const
get accelerometer bias
Definition: ImuBias.h:59
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
OptionalJacobian.h
Special class for optional Jacobian arguments.
gtsam::imuBias::ConstantBias::equals
bool equals(const ConstantBias &expected, double tol=1e-5) const
equality up to tolerance
Definition: ImuBias.h:98
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::internal::VectorSpace
VectorSpace provides both Testable and VectorSpaceTraits.
Definition: VectorSpace.h:207
gtsam::imuBias::ConstantBias::identity
static ConstantBias identity()
identity for group operation
Definition: ImuBias.h:108
gtsam::imuBias::ConstantBias::operator-
ConstantBias operator-(const ConstantBias &b) const
subtraction
Definition: ImuBias.h:128
gtsam::imuBias::ConstantBias::correctGyroscope
Vector3 correctGyroscope(const Vector3 &measurement, OptionalJacobian< 3, 6 > H1=boost::none, OptionalJacobian< 3, 3 > H2=boost::none) const
Correct a gyroscope measurement using this bias model, and optionally compute Jacobians.
Definition: ImuBias.h:78
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition: types.h:269
gtsam::imuBias::ConstantBias::gyroscope
const Vector3 & gyroscope() const
get gyroscope bias
Definition: ImuBias.h:64
gtsam::imuBias::ConstantBias::correctAccelerometer
Vector3 correctAccelerometer(const Vector3 &measurement, OptionalJacobian< 3, 6 > H1=boost::none, OptionalJacobian< 3, 3 > H2=boost::none) const
Correct an accelerometer measurement using this bias model, and optionally compute Jacobians.
Definition: ImuBias.h:69
gtsam::imuBias::ConstantBias::vector
Vector6 vector() const
return the accelerometer and gyro biases in a single vector
Definition: ImuBias.h:52
gtsam::testing::between
T between(const T &t1, const T &t2)
binary functions
Definition: lieProxies.h:36
gtsam::imuBias::ConstantBias::operator+
ConstantBias operator+(const Vector6 &v) const
addition of vector on right
Definition: ImuBias.h:118
gtsam::imuBias::ConstantBias::operator+
ConstantBias operator+(const ConstantBias &b) const
addition
Definition: ImuBias.h:123