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 {
 
using Velocity3 = Vector3;
 
class GTSAM_EXPORT NavState : public LieGroup<NavState, 9> {
 private:
 
  // TODO(frank):
  // - should we rename t_ to p_? if not, we should rename dP do dT
  Rot3 R_; 
  Point3 t_; 
  Velocity3 v_; 
 
public:
 
  inline constexpr static auto dimension = 9;
 
 
 
  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 Vector6& tv) :
      R_(R), t_(tv.head<3>()), v_(tv.tail<3>()) {
  }
 
  NavState(const Matrix5& T) :
    R_(T.block<3, 3>(0, 0)), t_(T.block<3, 1>(0, 3)), v_(T.block<3, 1>(0, 4)) {
  }
 
  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 = {}) const;
  const Point3& position(OptionalJacobian<3, 9> H = {}) const;
  const Velocity3& velocity(OptionalJacobian<3, 9> H = {}) 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 = {}) const;
 
  Matrix5 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;
 
 
  static NavState Identity() {
    return NavState();
  }
 
  NavState inverse() const;
 
  using LieGroup<NavState, 9>::inverse;  // version with derivative
 
  NavState operator*(const NavState& T) const {
    return NavState(R_ * T.R_, t_ + R_ * T.t_, v_ + R_ * T.v_);
  }
 
  const Rot3& rotation() const { return attitude(); };
 
  // 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 = {}, OptionalJacobian<9, 9> H2 =
          {}) const;
 
  Vector9 localCoordinates(const NavState& g, //
      OptionalJacobian<9, 9> H1 = {}, OptionalJacobian<9, 9> H2 =
          {}) const;
 
 
  using LieAlgebra = Matrix5;
 
  static NavState Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi = {});
 
  static Vector9 Logmap(const NavState& pose, OptionalJacobian<9, 9> Hpose = {});
 
  Matrix9 AdjointMap() const;
 
  Vector9 Adjoint(const Vector9& xi_b,
                  OptionalJacobian<9, 9> H_this = {},
                  OptionalJacobian<9, 9> H_xib = {}) const;
  
  static Matrix9 adjointMap(const Vector9& xi);
 
  static Vector9 adjoint(const Vector9& xi, const Vector9& y,
                         OptionalJacobian<9, 9> Hxi = {},
                         OptionalJacobian<9, 9> H_y = {});
 
  static Matrix9 ExpmapDerivative(const Vector9& xi);
 
  static Matrix9 LogmapDerivative(const NavState& xi);
 
  // Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP
  struct GTSAM_EXPORT ChartAtOrigin {
    static NavState Retract(const Vector9& xi, ChartJacobian Hxi = {});
    static Vector9 Local(const NavState& state, ChartJacobian Hstate = {});
  };
 
  static Matrix5 Hat(const Vector9& xi);
 
  static Vector9 Vee(const Matrix5& X);
 
 
  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 = {}) const;
 
  Vector9 correctPIM(const Vector9& pim, double dt, const Vector3& n_gravity,
      const std::optional<Vector3>& omegaCoriolis, bool use2ndOrderCoriolis =
          false, OptionalJacobian<9, 9> H1 = {},
      OptionalJacobian<9, 9> H2 = {}) const;
 
 
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(R_);
    ar & BOOST_SERIALIZATION_NVP(t_);
    ar & BOOST_SERIALIZATION_NVP(v_);
  }
#endif
};
 
// Specialize NavState traits to use a Retract/Local that agrees with IMUFactors
template <>
struct traits<NavState> : public internal::MatrixLieGroup<NavState> {};
 
template <>
struct traits<const NavState> : public internal::MatrixLieGroup<NavState> {};
 
} // namespace gtsam