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


private:


#ifdef 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


public:
  GTSAM_MAKE_ALIGNED_OPERATOR_NEW

}; // ConstantBias class
} // namespace imuBias

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

} // namespace gtsam