gtsam 4.1.1
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
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
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
159private:
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
173public:
176
177}; // ConstantBias class
178} // namespace imuBias
179
180template<>
181struct traits<imuBias::ConstantBias> : public internal::VectorSpace<
182 imuBias::ConstantBias> {
183};
184
185} // namespace gtsam
186
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition: types.h:277
Special class for optional Jacobian arguments.
T between(const T &t1, const T &t2)
binary functions
Definition: lieProxies.h:36
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:112
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:155
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
std::ostream & operator<<(std::ostream &os, const ConstantBias &bias)
ostream operator
Definition: ImuBias.cpp:68
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:39
VectorSpace provides both Testable and VectorSpaceTraits.
Definition: VectorSpace.h:207
Definition: ImuBias.h:30
const Vector3 & gyroscope() const
get gyroscope bias
Definition: ImuBias.h:64
bool equals(const ConstantBias &expected, double tol=1e-5) const
equality up to tolerance
Definition: ImuBias.h:98
ConstantBias operator-(const ConstantBias &b) const
subtraction
Definition: ImuBias.h:128
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
ConstantBias operator-() const
inverse
Definition: ImuBias.h:113
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
static ConstantBias identity()
identity for group operation
Definition: ImuBias.h:108
Vector6 vector() const
return the accelerometer and gyro biases in a single vector
Definition: ImuBias.h:52
ConstantBias operator+(const Vector6 &v) const
addition of vector on right
Definition: ImuBias.h:118
ConstantBias operator+(const ConstantBias &b) const
addition
Definition: ImuBias.h:123
const Vector3 & accelerometer() const
get accelerometer bias
Definition: ImuBias.h:59