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/MatrixConstants.h>
#include <gtsam/base/OptionalJacobian.h>
#include <gtsam/base/VectorSpace.h>
 
#include <iosfwd>
#if GTSAM_ENABLE_BOOST_SERIALIZATION
#include <boost/serialization/nvp.hpp>
#endif
 
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 = {},
                               OptionalJacobian<3, 3> H2 = {}) 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 = {},
                           OptionalJacobian<3, 3> H2 = {}) 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 retract(const Vector6& v) const {
    return ConstantBias(biasAcc_ + v.head<3>(), biasGyro_ + v.tail<3>());
  }

  Vector6 localCoordinates(const ConstantBias& other) const {
    return other.vector() - vector();
  }

 
private:

 
#if GTSAM_ENABLE_BOOST_SERIALIZATION
  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_);
  }
#endif

 
}; // ConstantBias class
} // namespace imuBias
 
template<>
struct traits<imuBias::ConstantBias> : public internal::VectorSpace<
    imuBias::ConstantBias> {
};
 
} // namespace gtsam