Loading...
Searching...
No Matches
ImuBias.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/base/OptionalJacobian.h>
#include <gtsam/base/VectorSpace.h>
#include <iosfwd>
#include <boost/serialization/nvp.hpp>
namespace gtsam {
namespace imuBias {
class GTSAM_EXPORT ConstantBias {
private:
Vector3 biasAcc_;
Vector3 biasGyro_;
public:
static const size_t dimension = 6;
ConstantBias() :
biasAcc_(0.0, 0.0, 0.0), biasGyro_(0.0, 0.0, 0.0) {
}
ConstantBias(const Vector3& biasAcc, const Vector3& biasGyro) :
biasAcc_(biasAcc), biasGyro_(biasGyro) {
}
explicit ConstantBias(const Vector6& v) :
biasAcc_(v.head<3>()), biasGyro_(v.tail<3>()) {
}
Vector6 vector() const {
Vector6 v;
v << biasAcc_, biasGyro_;
return v;
}
const Vector3& accelerometer() const {
return biasAcc_;
}
const Vector3& gyroscope() const {
return biasGyro_;
}
Vector3 correctAccelerometer(const Vector3& measurement,
OptionalJacobian<3, 6> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) const {
if (H1) (*H1) << -I_3x3, Z_3x3;
if (H2) (*H2) << I_3x3;
return measurement - biasAcc_;
}
Vector3 correctGyroscope(const Vector3& measurement,
OptionalJacobian<3, 6> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) const {
if (H1) (*H1) << Z_3x3, -I_3x3;
if (H2) (*H2) << I_3x3;
return measurement - biasGyro_;
}
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
const ConstantBias& bias);
void print(const std::string& s = "") const;
inline bool equals(const ConstantBias& expected, double tol = 1e-5) const {
return equal_with_abs_tol(biasAcc_, expected.biasAcc_, tol)
&& equal_with_abs_tol(biasGyro_, expected.biasGyro_, tol);
}
static ConstantBias Identity() {
return ConstantBias();
}
inline ConstantBias operator-() const {
return ConstantBias(-biasAcc_, -biasGyro_);
}
ConstantBias operator+(const Vector6& v) const {
return ConstantBias(biasAcc_ + v.head<3>(), biasGyro_ + v.tail<3>());
}
ConstantBias operator+(const ConstantBias& b) const {
return ConstantBias(biasAcc_ + b.biasAcc_, biasGyro_ + b.biasGyro_);
}
ConstantBias operator-(const ConstantBias& b) const {
return ConstantBias(biasAcc_ - b.biasAcc_, biasGyro_ - b.biasGyro_);
}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
ConstantBias GTSAM_DEPRECATED inverse() { return -(*this); }
ConstantBias GTSAM_DEPRECATED compose(const ConstantBias& q) {
return (*this) + q;
}
ConstantBias GTSAM_DEPRECATED between(const ConstantBias& q) {
return q - (*this);
}
Vector6 GTSAM_DEPRECATED localCoordinates(const ConstantBias& q) {
return (q - (*this)).vector();
}
ConstantBias GTSAM_DEPRECATED retract(const Vector6& v) {
return (*this) + ConstantBias(v);
}
static Vector6 GTSAM_DEPRECATED Logmap(const ConstantBias& p) {
return p.vector();
}
static ConstantBias GTSAM_DEPRECATED Expmap(const Vector6& v) {
return ConstantBias(v);
}
#endif
private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_NVP(biasAcc_);
ar & BOOST_SERIALIZATION_NVP(biasGyro_);
}
public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
}; // ConstantBias class
} // namespace imuBias
template<>
struct traits<imuBias::ConstantBias> : public internal::VectorSpace<
imuBias::ConstantBias> {
};
} // namespace gtsam
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
OptionalJacobian.h
Special class for optional Jacobian arguments.
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
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:81
gtsam::imuBias
All bias models live in the imuBias namespace.
Definition ImuBias.cpp:26
gtsam::imuBias::operator<<
std::ostream & operator<<(std::ostream &os, const ConstantBias &bias)
ostream operator
Definition ImuBias.cpp:68
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::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
gtsam::internal::VectorSpace
VectorSpace provides both Testable and VectorSpaceTraits.
Definition VectorSpace.h:207
gtsam::imuBias::ConstantBias
Definition ImuBias.h:30
gtsam::imuBias::ConstantBias::gyroscope
const Vector3 & gyroscope() const
get gyroscope bias
Definition ImuBias.h:69
gtsam::imuBias::ConstantBias::Identity
static ConstantBias Identity()
identity for group operation
Definition ImuBias.h:112
gtsam::imuBias::ConstantBias::equals
bool equals(const ConstantBias &expected, double tol=1e-5) const
equality up to tolerance
Definition ImuBias.h:102
gtsam::imuBias::ConstantBias::operator-
ConstantBias operator-(const ConstantBias &b) const
subtraction
Definition ImuBias.h:132
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:74
gtsam::imuBias::ConstantBias::operator-
ConstantBias operator-() const
inverse
Definition ImuBias.h:117
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:83
gtsam::imuBias::ConstantBias::dimension
static const size_t dimension
dimension of the variable - used to autodetect sizes
Definition ImuBias.h:37
gtsam::imuBias::ConstantBias::vector
Vector6 vector() const
return the accelerometer and gyro biases in a single vector
Definition ImuBias.h:57
gtsam::imuBias::ConstantBias::operator+
ConstantBias operator+(const Vector6 &v) const
addition of vector on right
Definition ImuBias.h:122
gtsam::imuBias::ConstantBias::operator+
ConstantBias operator+(const ConstantBias &b) const
addition
Definition ImuBias.h:127
gtsam::imuBias::ConstantBias::accelerometer
const Vector3 & accelerometer() const
get accelerometer bias
Definition ImuBias.h:64