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/base/Manifold.h>
#include <gtsam/base/MatrixConstants.h>
#include <gtsam/base/Vector.h>
#include <gtsam/geometry/BearingRange.h>
#include <gtsam/geometry/ExtendedPose3.h>
#include <gtsam/geometry/Pose3.h>
 
#if GTSAM_ENABLE_BOOST_SERIALIZATION
#include <boost/serialization/base_object.hpp>
#endif
 
namespace gtsam {

using Velocity3 = Vector3;

class GTSAM_EXPORT NavState : public ExtendedPose3<2, NavState> {
public:
  using Base = ExtendedPose3<2, NavState>;
  using LieAlgebra = Matrix5;
  using Vector25 = Eigen::Matrix<double, 25, 1>;
  inline constexpr static auto dimension = 9;


  NavState() : Base() {}
 
  NavState(const Base& other) : Base(other) {}

  NavState(const Rot3& R, const Point3& t, const Velocity3& v)
      : Base(R, Matrix32{{t.x(), v.x()}, {t.y(), v.y()}, {t.z(), v.z()}}) {}

  NavState(const Pose3& pose, const Velocity3& v)
      : NavState(pose.rotation(), pose.translation(), v) {}

  NavState(const Matrix3& R, const Vector6& tv)
      : NavState(Rot3(R), tv.head<3>(), tv.tail<3>()) {}

  NavState(const Matrix5& T) : Base(T) {}

  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;
  Point3 position(OptionalJacobian<3, 9> H = {}) const;
  Velocity3 velocity(OptionalJacobian<3, 9> H = {}) const;
 
  const Pose3 pose() const {
    return Pose3(attitude(), position());
  }

  double range(const Point3& point, OptionalJacobian<1, 9> Hself = {},
               OptionalJacobian<1, 3> Hpoint = {}) const;

  Unit3 bearing(const Point3& point, OptionalJacobian<2, 9> Hself = {},
                OptionalJacobian<2, 3> Hpoint = {}) const;


  Matrix3 R() const {
    return R_.matrix();
  }

  Quaternion quaternion() const {
    return R_.toQuaternion();
  }

  Vector3 t() const {
    return t_.col(0);
  }

  Vector3 v() const {
    return velocity();
  }
  // Return velocity in body frame
  Velocity3 bodyVelocity(OptionalJacobian<3, 9> H = {}) 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;


  const Rot3& rotation(OptionalJacobian<3, 9> H = {}) const {
    return attitude(H);
  };
 
  // 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;


 
  // φ: autonomous flow where velocity acts on position for
  //   dt (R, p, v) -> p += v·dt.
  struct AutonomousFlow {
    double dt;
 
    // Differential at identity: Φ = I with ∂p/∂v = dt·I.
    Jacobian dIdentity() const {
      Jacobian Phi = I_9x9;
      Phi.template block<3, 3>(3, 6) = I_3x3 * dt;
      return Phi;
    }
 
    // Apply φ(x) by p += v·dt
    NavState operator()(const NavState& X) const {
      return {X.attitude(), X.position() + X.velocity() * dt, X.velocity()};
    }
  };

  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;
 
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
  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 = {},
                     OptionalJacobian<9, 3> H3 = {}) const;
#endif

 
private:
  friend class PreintegrationBase;
 
  // Return the predicted state directly, avoiding a local/retract round-trip.
  NavState predictPIM(const Vector9& pim, double dt,
                      const Vector3& n_gravity,
                      const std::optional<Vector3>& omegaCoriolis,
                      OptionalJacobian<9, 9> H1 = {},
                      OptionalJacobian<9, 9> H2 = {},
                      OptionalJacobian<9, 3> H3 = {}) const;

#if GTSAM_ENABLE_BOOST_SERIALIZATION  
  friend class boost::serialization::access;
  template<class ARCHIVE>
  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
    ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
  }
#endif
};
 
namespace internal {

GTSAM_EXPORT NavState navStateComponentWiseRetract(
    const NavState& state, const Vector9& v,
    OptionalJacobian<9, 9> H1 = {}, OptionalJacobian<9, 9> H2 = {});

GTSAM_EXPORT Vector9 navStateComponentWiseLocalCoordinates(
    const NavState& state, const NavState& other,
    OptionalJacobian<9, 9> H1 = {}, OptionalJacobian<9, 9> H2 = {});
 
}  // namespace internal
 
// Specialize NavState traits to use the configured optimization chart.
template <>
struct traits<NavState> : public internal::MatrixLieGroup<NavState, 5> {};
 
template <>
struct traits<const NavState> : public internal::MatrixLieGroup<NavState, 5> {};
 
// bearing and range traits, used in RangeFactor and BearingFactor
template <>
struct Bearing<NavState, Point3> : HasBearing<NavState, Point3, Unit3> {};
 
template <>
struct Range<NavState, Point3> : HasRange<NavState, Point3, double> {};
 
} // namespace gtsam