NavState.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/geometry/Pose3.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/Manifold.h>

namespace gtsam {

typedef Vector3 Velocity3;

class GTSAM_EXPORT NavState {
private:

  // TODO(frank):
  // - should we rename t_ to p_? if not, we should rename dP do dT
  Rot3 R_; 
  Point3 t_; 
  Velocity3 v_; 

public:

  enum {
    dimension = 9
  };

  typedef std::pair<Point3, Velocity3> PositionAndVelocity;


  NavState() :
      t_(0, 0, 0), v_(Vector3::Zero()) {
  }
  NavState(const Rot3& R, const Point3& t, const Velocity3& v) :
      R_(R), t_(t), v_(v) {
  }
  NavState(const Pose3& pose, const Velocity3& v) :
      R_(pose.rotation()), t_(pose.translation()), v_(v) {
  }
  NavState(const Matrix3& R, const Vector9 tv) :
      R_(R), t_(tv.head<3>()), v_(tv.tail<3>()) {
  }
  static NavState Create(const Rot3& R, const Point3& t, const Velocity3& v,
      OptionalJacobian<9, 3> H1, OptionalJacobian<9, 3> H2,
      OptionalJacobian<9, 3> H3);
  static NavState FromPoseVelocity(const Pose3& pose, const Vector3& vel,
      OptionalJacobian<9, 6> H1, OptionalJacobian<9, 3> H2);


  const Rot3& attitude(OptionalJacobian<3, 9> H = boost::none) const;
  const Point3& position(OptionalJacobian<3, 9> H = boost::none) const;
  const Velocity3& velocity(OptionalJacobian<3, 9> H = boost::none) const;

  const Pose3 pose() const {
    return Pose3(attitude(), position());
  }


  Matrix3 R() const {
    return R_.matrix();
  }
  Quaternion quaternion() const {
    return R_.toQuaternion();
  }
  Vector3 t() const {
    return t_;
  }
  const Vector3& v() const {
    return v_;
  }
  // Return velocity in body frame
  Velocity3 bodyVelocity(OptionalJacobian<3, 9> H = boost::none) const;

  Matrix7 matrix() const;


  GTSAM_EXPORT
  friend std::ostream &operator<<(std::ostream &os, const NavState& state);

  void print(const std::string& s = "") const;

  bool equals(const NavState& other, double tol = 1e-8) const;


  // Tangent space sugar.
  // TODO(frank): move to private navstate namespace in cpp
  static Eigen::Block<Vector9, 3, 1> dR(Vector9& v) {
    return v.segment<3>(0);
  }
  static Eigen::Block<Vector9, 3, 1> dP(Vector9& v) {
    return v.segment<3>(3);
  }
  static Eigen::Block<Vector9, 3, 1> dV(Vector9& v) {
    return v.segment<3>(6);
  }
  static Eigen::Block<const Vector9, 3, 1> dR(const Vector9& v) {
    return v.segment<3>(0);
  }
  static Eigen::Block<const Vector9, 3, 1> dP(const Vector9& v) {
    return v.segment<3>(3);
  }
  static Eigen::Block<const Vector9, 3, 1> dV(const Vector9& v) {
    return v.segment<3>(6);
  }

  NavState retract(const Vector9& v, //
      OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 9> H2 =
          boost::none) const;

  Vector9 localCoordinates(const NavState& g, //
      OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 9> H2 =
          boost::none) const;


  NavState update(const Vector3& b_acceleration, const Vector3& b_omega,
      const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1,
      OptionalJacobian<9, 3> G2) const;

  Vector9 coriolis(double dt, const Vector3& omega, bool secondOrder = false,
      OptionalJacobian<9, 9> H = boost::none) const;

  Vector9 correctPIM(const Vector9& pim, double dt, const Vector3& n_gravity,
      const boost::optional<Vector3>& omegaCoriolis, bool use2ndOrderCoriolis =
          false, OptionalJacobian<9, 9> H1 = boost::none,
      OptionalJacobian<9, 9> H2 = boost::none) const;


private:
  friend class boost::serialization::access;
  template<class ARCHIVE>
  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
    ar & BOOST_SERIALIZATION_NVP(R_);
    ar & BOOST_SERIALIZATION_NVP(t_);
    ar & BOOST_SERIALIZATION_NVP(v_);
  }
};

// Specialize NavState traits to use a Retract/Local that agrees with IMUFactors
template<>
struct traits<NavState> : internal::Manifold<NavState> {
};

} // namespace gtsam