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 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
24 #include <boost/serialization/nvp.hpp>
25 #endif
26 
27 namespace gtsam {
28 
30 namespace imuBias {
31 
32 class GTSAM_EXPORT ConstantBias {
33 private:
36 
37 public:
39  static const size_t dimension = 6;
40 
43 
45  biasAcc_(0.0, 0.0, 0.0), biasGyro_(0.0, 0.0, 0.0) {
46  }
47 
48  ConstantBias(const Vector3& biasAcc, const Vector3& biasGyro) :
49  biasAcc_(biasAcc), biasGyro_(biasGyro) {
50  }
51 
52  explicit ConstantBias(const Vector6& v) :
53  biasAcc_(v.head<3>()), biasGyro_(v.tail<3>()) {
54  }
55 
57 
59  Vector6 vector() const {
60  Vector6 v;
61  v << biasAcc_, biasGyro_;
62  return v;
63  }
64 
66  const Vector3& accelerometer() const {
67  return biasAcc_;
68  }
69 
71  const Vector3& gyroscope() const {
72  return biasGyro_;
73  }
74 
77  OptionalJacobian<3, 6> H1 = {},
78  OptionalJacobian<3, 3> H2 = {}) const {
79  if (H1) (*H1) << -I_3x3, Z_3x3;
80  if (H2) (*H2) << I_3x3;
81  return measurement - biasAcc_;
82  }
83 
86  OptionalJacobian<3, 6> H1 = {},
87  OptionalJacobian<3, 3> H2 = {}) const {
88  if (H1) (*H1) << Z_3x3, -I_3x3;
89  if (H2) (*H2) << I_3x3;
90  return measurement - biasGyro_;
91  }
92 
95 
97  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
98  const ConstantBias& bias);
99 
101  void print(const std::string& s = "") const;
102 
104  inline bool equals(const ConstantBias& expected, double tol = 1e-5) const {
105  return equal_with_abs_tol(biasAcc_, expected.biasAcc_, tol)
106  && equal_with_abs_tol(biasGyro_, expected.biasGyro_, tol);
107  }
108 
112 
115  return ConstantBias();
116  }
117 
119  inline ConstantBias operator-() const {
120  return ConstantBias(-biasAcc_, -biasGyro_);
121  }
122 
124  ConstantBias operator+(const Vector6& v) const {
125  return ConstantBias(biasAcc_ + v.head<3>(), biasGyro_ + v.tail<3>());
126  }
127 
130  return ConstantBias(biasAcc_ + b.biasAcc_, biasGyro_ + b.biasGyro_);
131  }
132 
135  return ConstantBias(biasAcc_ - b.biasAcc_, biasGyro_ - b.biasGyro_);
136  }
137 
139 
140 private:
141 
144 
145 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
146 
147  friend class boost::serialization::access;
148  template<class ARCHIVE>
149  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
150  ar & BOOST_SERIALIZATION_NVP(biasAcc_);
151  ar & BOOST_SERIALIZATION_NVP(biasGyro_);
152  }
153 #endif
154 
155 
156 public:
159 
160 }; // ConstantBias class
161 } // namespace imuBias
162 
163 template<>
164 struct traits<imuBias::ConstantBias> : public internal::VectorSpace<
165  imuBias::ConstantBias> {
166 };
167 
168 } // namespace gtsam
169 
gtsam::imuBias::ConstantBias::gyroscope
const Vector3 & gyroscope() const
Definition: ImuBias.h:71
tail
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE FixedSegmentReturnType< internal::get_fixed_value< NType >::value >::Type tail(NType n)
Definition: BlockMethods.h:1257
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::imuBias::ConstantBias::operator-
ConstantBias operator-() const
Definition: ImuBias.h:119
gtsam::imuBias::ConstantBias::operator+
ConstantBias operator+(const Vector6 &v) const
Definition: ImuBias.h:124
gtsam::internal::VectorSpace
VectorSpace provides both Testable and VectorSpaceTraits.
Definition: VectorSpace.h:207
gtsam::imuBias::ConstantBias::ConstantBias
ConstantBias()
Definition: ImuBias.h:44
biased_x_rotation::bias
const Vector3 bias(1, 2, 3)
gtsam::imuBias::ConstantBias::equals
bool equals(const ConstantBias &expected, double tol=1e-5) const
Definition: ImuBias.h:104
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::imuBias::ConstantBias::correctAccelerometer
Vector3 correctAccelerometer(const Vector3 &measurement, OptionalJacobian< 3, 6 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
Definition: ImuBias.h:76
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
gtsam::imuBias::ConstantBias::vector
Vector6 vector() const
Definition: ImuBias.h:59
os
ofstream os("timeSchurFactors.csv")
gtsam::imuBias::operator<<
std::ostream & operator<<(std::ostream &os, const ConstantBias &bias)
ostream operator
Definition: ImuBias.cpp:68
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
gtsam::imuBias::ConstantBias::biasAcc_
Vector3 biasAcc_
The units for stddev are σ = m/s² or m √Hz/s²
Definition: ImuBias.h:34
gtsam::imuBias::ConstantBias::accelerometer
const Vector3 & accelerometer() const
Definition: ImuBias.h:66
gtsam::imuBias::ConstantBias::operator+
ConstantBias operator+(const ConstantBias &b) const
Definition: ImuBias.h:129
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
OptionalJacobian.h
Special class for optional Jacobian arguments.
head
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE FixedSegmentReturnType< internal::get_fixed_value< NType >::value >::Type head(NType n)
Definition: BlockMethods.h:1208
gtsam::imuBias::ConstantBias::ConstantBias
ConstantBias(const Vector3 &biasAcc, const Vector3 &biasGyro)
Definition: ImuBias.h:48
gtsam::b
const G & b
Definition: Group.h:79
gtsam::imuBias::ConstantBias
Definition: ImuBias.h:32
gtsam
traits
Definition: SFMdata.h:40
gtsam::traits
Definition: Group.h:36
gtsam::OptionalJacobian
Definition: OptionalJacobian.h:38
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::imuBias::ConstantBias::Identity
static ConstantBias Identity()
Definition: ImuBias.h:114
gtsam::imuBias::ConstantBias::correctGyroscope
Vector3 correctGyroscope(const Vector3 &measurement, OptionalJacobian< 3, 6 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
Definition: ImuBias.h:85
gtsam::imuBias::ConstantBias::biasGyro_
Vector3 biasGyro_
The units for stddev are σ = rad/s or rad √Hz/s.
Definition: ImuBias.h:35
gtsam::imuBias::ConstantBias::ConstantBias
ConstantBias(const Vector6 &v)
Definition: ImuBias.h:52
gtsam::imuBias::ConstantBias::operator-
ConstantBias operator-(const ConstantBias &b) const
Definition: ImuBias.h:134
VectorSpace.h
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
Definition: types.h:279
measurement
static Point2 measurement(323.0, 240.0)


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:02:27