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/geometry/Pose3.h> // Includes Rot3, Point3
#include <gtsam/geometry/Event.h>
#include <gtsam/base/Lie.h>       // For LieGroup base class and traits
#include <gtsam/base/Manifold.h>  // For Manifold traits
 
#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;
// Define Vector10 for tangent space
using Vector10 = Eigen::Matrix<double, 10, 1>;
// Define Matrix5 for Lie Algebra matrix representation
using Matrix5 = Eigen::Matrix<double, 5, 5>;
// Define Matrix10 for Jacobians
using Matrix10 = Eigen::Matrix<double, 10, 10>;
 
class GTSAM_EXPORT Gal3 : public LieGroup<Gal3, 10> {
 private:
  Rot3 R_;      
  Point3 r_;    
  Velocity3 v_; 
  double t_;    
 
 public:
 
  inline static constexpr size_t dimension = 10;
 
 
  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;
 
  // Convenience accessors matching NavState names
  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_; }
 
  Matrix5 matrix() 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 Vector10& xi, OptionalJacobian<10, 10> Hxi = {});
 
  static Vector10 Logmap(const Gal3& g, OptionalJacobian<10, 10> Hg = {});
 
  Matrix10 AdjointMap() const;
 
  Vector10 Adjoint(const Vector10& xi_base, OptionalJacobian<10, 10> H_g = {},
                  OptionalJacobian<10, 10> H_xi = {}) const;
 
  static Vector10 adjoint(const Vector10& xi, const Vector10& y,
                         OptionalJacobian<10, 10> Hxi = {},
                         OptionalJacobian<10, 10> Hy = {});
 
  static Matrix10 adjointMap(const Vector10& xi);
 
  static Matrix10 ExpmapDerivative(const Vector10& xi);
 
  static Matrix10 LogmapDerivative(const Gal3& g);
 
  struct ChartAtOrigin {
    static Gal3 Retract(const Vector10& xi, ChartJacobian Hxi = {});
    static Vector10 Local(const Gal3& g, ChartJacobian Hg = {});
  };
 
  static Matrix5 Hat(const Vector10& xi);
 
  static Vector10 Vee(const Matrix5& 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::LieGroup<Gal3> {};
 
template <>
struct traits<const Gal3> : public internal::LieGroup<Gal3> {};
 
} // namespace gtsam