gtsam 4.2.0
gtsam
Loading...
Searching...
No Matches
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
25namespace gtsam {
26
28namespace imuBias {
29
30class GTSAM_EXPORT ConstantBias {
31private:
32 Vector3 biasAcc_;
33 Vector3 biasGyro_;
34
35public:
37 static const size_t dimension = 6;
38
41
42 ConstantBias() :
43 biasAcc_(0.0, 0.0, 0.0), biasGyro_(0.0, 0.0, 0.0) {
44 }
45
46 ConstantBias(const Vector3& biasAcc, const Vector3& biasGyro) :
47 biasAcc_(biasAcc), biasGyro_(biasGyro) {
48 }
49
50 explicit ConstantBias(const Vector6& v) :
51 biasAcc_(v.head<3>()), biasGyro_(v.tail<3>()) {
52 }
53
55
57 Vector6 vector() const {
58 Vector6 v;
59 v << biasAcc_, biasGyro_;
60 return v;
61 }
62
64 const Vector3& accelerometer() const {
65 return biasAcc_;
66 }
67
69 const Vector3& gyroscope() const {
70 return biasGyro_;
71 }
72
74 Vector3 correctAccelerometer(const Vector3& measurement,
75 OptionalJacobian<3, 6> H1 = boost::none,
76 OptionalJacobian<3, 3> H2 = boost::none) const {
77 if (H1) (*H1) << -I_3x3, Z_3x3;
78 if (H2) (*H2) << I_3x3;
79 return measurement - biasAcc_;
80 }
81
83 Vector3 correctGyroscope(const Vector3& measurement,
84 OptionalJacobian<3, 6> H1 = boost::none,
85 OptionalJacobian<3, 3> H2 = boost::none) const {
86 if (H1) (*H1) << Z_3x3, -I_3x3;
87 if (H2) (*H2) << I_3x3;
88 return measurement - biasGyro_;
89 }
90
93
95 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
96 const ConstantBias& bias);
97
99 void print(const std::string& s = "") const;
100
102 inline bool equals(const ConstantBias& expected, double tol = 1e-5) const {
103 return equal_with_abs_tol(biasAcc_, expected.biasAcc_, tol)
104 && equal_with_abs_tol(biasGyro_, expected.biasGyro_, tol);
105 }
106
110
113 return ConstantBias();
114 }
115
117 inline ConstantBias operator-() const {
118 return ConstantBias(-biasAcc_, -biasGyro_);
119 }
120
122 ConstantBias operator+(const Vector6& v) const {
123 return ConstantBias(biasAcc_ + v.head<3>(), biasGyro_ + v.tail<3>());
124 }
125
128 return ConstantBias(biasAcc_ + b.biasAcc_, biasGyro_ + b.biasGyro_);
129 }
130
133 return ConstantBias(biasAcc_ - b.biasAcc_, biasGyro_ - b.biasGyro_);
134 }
135
137
138#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
141 ConstantBias GTSAM_DEPRECATED inverse() { return -(*this); }
142 ConstantBias GTSAM_DEPRECATED compose(const ConstantBias& q) {
143 return (*this) + q;
144 }
145 ConstantBias GTSAM_DEPRECATED between(const ConstantBias& q) {
146 return q - (*this);
147 }
148 Vector6 GTSAM_DEPRECATED localCoordinates(const ConstantBias& q) {
149 return (q - (*this)).vector();
150 }
151 ConstantBias GTSAM_DEPRECATED retract(const Vector6& v) {
152 return (*this) + ConstantBias(v);
153 }
154 static Vector6 GTSAM_DEPRECATED Logmap(const ConstantBias& p) {
155 return p.vector();
156 }
157 static ConstantBias GTSAM_DEPRECATED Expmap(const Vector6& v) {
158 return ConstantBias(v);
159 }
161#endif
162
163private:
164
167
169 friend class boost::serialization::access;
170 template<class ARCHIVE>
171 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
172 ar & BOOST_SERIALIZATION_NVP(biasAcc_);
173 ar & BOOST_SERIALIZATION_NVP(biasGyro_);
174 }
175
176
177public:
180
181}; // ConstantBias class
182} // namespace imuBias
183
184template<>
185struct traits<imuBias::ConstantBias> : public internal::VectorSpace<
186 imuBias::ConstantBias> {
187};
188
189} // namespace gtsam
190
Special class for optional Jacobian arguments.
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
Global functions in a separate testing namespace.
Definition chartTesting.h:28
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
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:81
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
VectorSpace provides both Testable and VectorSpaceTraits.
Definition VectorSpace.h:207
Definition ImuBias.h:30
const Vector3 & gyroscope() const
get gyroscope bias
Definition ImuBias.h:69
static ConstantBias Identity()
identity for group operation
Definition ImuBias.h:112
bool equals(const ConstantBias &expected, double tol=1e-5) const
equality up to tolerance
Definition ImuBias.h:102
ConstantBias operator-(const ConstantBias &b) const
subtraction
Definition ImuBias.h:132
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:74
ConstantBias operator-() const
inverse
Definition ImuBias.h:117
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:83
Vector6 vector() const
return the accelerometer and gyro biases in a single vector
Definition ImuBias.h:57
ConstantBias operator+(const Vector6 &v) const
addition of vector on right
Definition ImuBias.h:122
ConstantBias operator+(const ConstantBias &b) const
addition
Definition ImuBias.h:127
const Vector3 & accelerometer() const
get accelerometer bias
Definition ImuBias.h:64