21#include <gtsam/base/VectorSpace.h>
23#include <boost/serialization/nvp.hpp>
37 static const size_t dimension = 6;
43 biasAcc_(0.0, 0.0, 0.0), biasGyro_(0.0, 0.0, 0.0) {
46 ConstantBias(
const Vector3& biasAcc,
const Vector3& biasGyro) :
47 biasAcc_(biasAcc), biasGyro_(biasGyro) {
50 explicit ConstantBias(
const Vector6& v) :
51 biasAcc_(v.head<3>()), biasGyro_(v.tail<3>()) {
59 v << biasAcc_, biasGyro_;
77 if (H1) (*H1) << -I_3x3, Z_3x3;
78 if (H2) (*H2) << I_3x3;
79 return measurement - biasAcc_;
86 if (H1) (*H1) << Z_3x3, -I_3x3;
87 if (H2) (*H2) << I_3x3;
88 return measurement - biasGyro_;
95 GTSAM_EXPORT
friend std::ostream& operator<<(std::ostream& os,
99 void print(
const std::string& s =
"")
const;
123 return ConstantBias(biasAcc_ + v.head<3>(), biasGyro_ + v.tail<3>());
128 return ConstantBias(biasAcc_ + b.biasAcc_, biasGyro_ + b.biasGyro_);
133 return ConstantBias(biasAcc_ - b.biasAcc_, biasGyro_ - b.biasGyro_);
138#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
141 ConstantBias GTSAM_DEPRECATED inverse() {
return -(*this); }
142 ConstantBias GTSAM_DEPRECATED compose(
const ConstantBias& q) {
145 ConstantBias GTSAM_DEPRECATED between(
const ConstantBias& q) {
148 Vector6 GTSAM_DEPRECATED localCoordinates(
const ConstantBias& q) {
149 return (q - (*
this)).vector();
151 ConstantBias GTSAM_DEPRECATED retract(
const Vector6& v) {
152 return (*
this) + ConstantBias(v);
154 static Vector6 GTSAM_DEPRECATED Logmap(
const ConstantBias& p) {
157 static ConstantBias GTSAM_DEPRECATED Expmap(
const Vector6& v) {
158 return ConstantBias(v);
169 friend class boost::serialization::access;
170 template<
class ARCHIVE>
171 void serialize(ARCHIVE & ar,
const unsigned int ) {
172 ar & BOOST_SERIALIZATION_NVP(biasAcc_);
173 ar & BOOST_SERIALIZATION_NVP(biasGyro_);
186 imuBias::ConstantBias> {
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
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