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_);
  }


  ConstantBias inverse() {
    return -(*this);
  }
  ConstantBias compose(const ConstantBias& q) {
    return (*this) + q;
  }
  ConstantBias between(const ConstantBias& q) {
    return q - (*this);
  }
  Vector6 localCoordinates(const ConstantBias& q) {
    return between(q).vector();
  }
  ConstantBias retract(const Vector6& v) {
    return compose(ConstantBias(v));
  }
  static Vector6 Logmap(const ConstantBias& p) {
    return p.vector();
  }
  static ConstantBias Expmap(const Vector6& v) {
    return ConstantBias(v);
  }

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:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

}; // ConstantBias class
} // namespace imuBias

template<>
struct traits<imuBias::ConstantBias> : public internal::VectorSpace<
    imuBias::ConstantBias> {
};

} // namespace gtsam