Gal3.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/MatrixLieGroup.h>
#include <gtsam/geometry/Event.h>
#include <gtsam/geometry/BearingRange.h>
#include <gtsam/geometry/Pose3.h>
 
#include <cmath>       // For std::sqrt, std::cos, std::sin
#include <functional>  // For std::function used in numerical derivatives
 
namespace gtsam {
 
// Forward declaration
class Gal3;
 
// Use Vector3 for velocity for consistency with NavState
using Velocity3 = Vector3;

class GTSAM_EXPORT Gal3 : public MatrixLieGroup<Gal3, 10, 5> {
 private:
  Rot3 R_;       
  Point3 r_;     
  Velocity3 v_;  
  double t_;     
 
 public:

  Gal3()
      : R_(Rot3::Identity()),
        r_(Point3::Zero()),
        v_(Velocity3::Zero()),
        t_(0.0) {}

  Gal3(const Rot3& R, const Point3& r, const Velocity3& v, double t)
      : R_(R), r_(r), v_(v), t_(t) {}

  explicit Gal3(const Matrix5& M);

  static Gal3 Create(const Rot3& R, const Point3& r, const Velocity3& v,
                     double t, OptionalJacobian<10, 3> H1 = {},
                     OptionalJacobian<10, 3> H2 = {},
                     OptionalJacobian<10, 3> H3 = {},
                     OptionalJacobian<10, 1> H4 = {});

  static Gal3 FromPoseVelocityTime(const Pose3& pose, const Velocity3& v,
                                   double t, OptionalJacobian<10, 6> H1 = {},
                                   OptionalJacobian<10, 3> H2 = {},
                                   OptionalJacobian<10, 1> H3 = {});


  const Rot3& rotation(OptionalJacobian<3, 10> H = {}) const;

  const Point3& translation(OptionalJacobian<3, 10> H = {}) const;

  const Velocity3& velocity(OptionalJacobian<3, 10> H = {}) const;

  const double& time(OptionalJacobian<1, 10> H = {}) const;
 
  // Accessors when viewed as a "pose" manifold
  const Rot3& attitude(OptionalJacobian<3, 10> H = {}) const {
    return rotation(H);
  }
  const Point3& position(OptionalJacobian<3, 10> H = {}) const {
    return translation(H);
  }


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

  Vector3 r() const { return Vector3(r_); }  // Conversion from Point3

  const Vector3& v() const { return v_; }

  const double& t() const { return t_; }

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

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


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

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

  bool equals(const Gal3& other, double tol = 1e-9) const;


  static Gal3 Identity() { return Gal3(); }

  Gal3 inverse() const;
 
  // Bring LieGroup::inverse() into scope (version with derivative)
  using LieGroup<Gal3, 10>::inverse;

  Gal3 operator*(const Gal3& other) const;


  Event act(const Event& e, OptionalJacobian<4, 10> Hself = {},
            OptionalJacobian<4, 4> He = {}) const;


  static Gal3 Expmap(const TangentVector& xi,
                     OptionalJacobian<10, 10> Hxi = {});

  static TangentVector Logmap(const Gal3& g, OptionalJacobian<10, 10> Hg = {});

  Jacobian AdjointMap() const;

  static Jacobian adjointMap(const TangentVector& xi);

  static Jacobian ExpmapDerivative(const TangentVector& xi);

  static Jacobian LogmapDerivative(const Gal3& g);

  static Jacobian LogmapDerivative(const TangentVector& xi);

  struct GTSAM_EXPORT ChartAtOrigin {
    static Gal3 Retract(const TangentVector& xi, ChartJacobian Hxi = {});
    static TangentVector Local(const Gal3& g, ChartJacobian Hg = {});
  };

 
  using LieAlgebra = Matrix5;
  using Vector25 = Eigen::Matrix<double, 25, 1>;

  Matrix5 matrix() const;

  Vector25 vec(OptionalJacobian<25, 10> H = {}) const;

  static LieAlgebra Hat(const TangentVector& xi);

  static TangentVector Vee(const LieAlgebra& X);

 
 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(r_);
    ar& BOOST_SERIALIZATION_NVP(v_);
    ar& BOOST_SERIALIZATION_NVP(t_);
  }
#endif
 
};  // class Gal3

template <>
struct traits<Gal3> : public internal::MatrixLieGroup<Gal3, 5> {};
 
template <>
struct traits<const Gal3> : public internal::MatrixLieGroup<Gal3, 5> {};
 
// bearing and range traits, used in RangeFactor and BearingFactor
template <>
struct Bearing<Gal3, Point3> : HasBearing<Gal3, Point3, Unit3> {};
 
template <>
struct Range<Gal3, Point3> : HasRange<Gal3, Point3, double> {};
 
}  // namespace gtsam