Pose2.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
 
 * -------------------------------------------------------------------------- */
 
// \callgraph
 
#pragma once
 
#include <gtsam/geometry/BearingRange.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Rot2.h>
#include <gtsam/base/Lie.h>
#include <gtsam/dllexport.h>
#include <gtsam/base/std_optional_serialization.h>
 
#include <optional>
 
namespace gtsam {
 
class GTSAM_EXPORT Pose2: public LieGroup<Pose2, 3> {
 
public:
 
  typedef Rot2 Rotation;
  typedef Point2 Translation;
 
private:
 
  Rot2 r_;
  Point2 t_;
 
public:
 
 
  Pose2() :
      r_(traits<Rot2>::Identity()), t_(traits<Point2>::Identity()) {
  }
 
  Pose2(const Pose2& pose) = default;
  //  : r_(pose.r_), t_(pose.t_) {}
 
  Pose2& operator=(const Pose2& other) = default;
 
  Pose2(double x, double y, double theta) :
    r_(Rot2::fromAngle(theta)), t_(x, y) {
  }
 
  Pose2(double theta, const Point2& t) :
    r_(Rot2::fromAngle(theta)), t_(t) {
  }
 
  Pose2(const Rot2& r, const Point2& t) : r_(r), t_(t) {}
 
  Pose2(const Matrix &T)
      : r_(Rot2::atan2(T(1, 0), T(0, 0))), t_(T(0, 2), T(1, 2)) {
#ifndef NDEBUG
    if (T.rows() != 3 || T.cols() != 3) {
      throw;
    }
#endif
  }
 
 
  Pose2(const Vector& v) : Pose2() {
    *this = Expmap(v);
  }
 
  static std::optional<Pose2> Align(const Point2Pairs& abPointPairs);
 
  // Version of Pose2::Align that takes 2 matrices.
  static std::optional<Pose2> Align(const Matrix& a, const Matrix& b);
 
 
  void print(const std::string& s = "") const;
 
  bool equals(const Pose2& pose, double tol = 1e-9) const;
 
 
  inline static Pose2 Identity() { return Pose2(); }
 
  Pose2 inverse() const;
 
  inline Pose2 operator*(const Pose2& p2) const {
    return Pose2(r_*p2.r(), t_ + r_*p2.t());
  }
 
 
  static Pose2 Expmap(const Vector3& xi, ChartJacobian H = {});
 
  static Vector3 Logmap(const Pose2& p, ChartJacobian H = {});
 
  Matrix3 AdjointMap() const;
 
  inline Vector3 Adjoint(const Vector3& xi) const {
    return AdjointMap()*xi;
  }
 
  static Matrix3 adjointMap(const Vector3& v);
 
  static Vector3 adjoint(const Vector3& xi, const Vector3& y) {
    return adjointMap(xi) * y;
  }
 
  static Vector3 adjointTranspose(const Vector3& xi, const Vector3& y) {
    return adjointMap(xi).transpose() * y;
  }
 
  // temporary fix for wrappers until case issue is resolved
  static Matrix3 adjointMap_(const Vector3 &xi) { return adjointMap(xi);}
  static Vector3 adjoint_(const Vector3 &xi, const Vector3 &y) { return adjoint(xi, y);}
 
  static inline Matrix3 wedge(double vx, double vy, double w) {
    Matrix3 m;
    m << 0.,-w,  vx,
         w,  0., vy,
         0., 0.,  0.;
    return m;
  }
 
  static Matrix3 ExpmapDerivative(const Vector3& v);
 
  static Matrix3 LogmapDerivative(const Pose2& v);
 
  // Chart at origin, depends on compile-time flag SLOW_BUT_CORRECT_EXPMAP
  struct GTSAM_EXPORT ChartAtOrigin {
    static Pose2 Retract(const Vector3& v, ChartJacobian H = {});
    static Vector3 Local(const Pose2& r, ChartJacobian H = {});
  };
 
  using LieGroup<Pose2, 3>::inverse; // version with derivative
 
 
  Point2 transformTo(const Point2& point,
      OptionalJacobian<2, 3> Dpose = {},
      OptionalJacobian<2, 2> Dpoint = {}) const;
 
  Matrix transformTo(const Matrix& points) const;
 
  Point2 transformFrom(const Point2& point,
      OptionalJacobian<2, 3> Dpose = {},
      OptionalJacobian<2, 2> Dpoint = {}) const;
 
  Matrix transformFrom(const Matrix& points) const;
 
  inline Point2 operator*(const Point2& point) const { 
    return transformFrom(point);
  }
 
 
  inline double x()     const { return t_.x(); }
 
  inline double y()     const { return t_.y(); }
 
  inline double theta() const { return r_.theta(); }
 
  inline const Point2& t() const { return t_; }
 
  inline const Rot2&   r() const { return r_; }
 
  inline const Point2& translation(OptionalJacobian<2, 3> Hself={}) const {
    if (Hself) {
      *Hself = Matrix::Zero(2, 3);
      (*Hself).block<2, 2>(0, 0) = rotation().matrix();
    }
    return t_;
  }
 
  inline const Rot2&   rotation(OptionalJacobian<1, 3> Hself={}) const {
    if (Hself) *Hself << 0, 0, 1;
    return r_;
  }
 
  Matrix3 matrix() const;
 
  Rot2 bearing(const Point2& point,
               OptionalJacobian<1, 3> H1={}, OptionalJacobian<1, 2> H2={}) const;
 
  Rot2 bearing(const Pose2& pose,
               OptionalJacobian<1, 3> H1={}, OptionalJacobian<1, 3> H2={}) const;
 
  double range(const Point2& point,
      OptionalJacobian<1, 3> H1={},
      OptionalJacobian<1, 2> H2={}) const;
 
  double range(const Pose2& point,
      OptionalJacobian<1, 3> H1={},
      OptionalJacobian<1, 3> H2={}) const;
 
 
  inline static std::pair<size_t, size_t> translationInterval() { return {0, 1}; }
 
  static std::pair<size_t, size_t> rotationInterval() { return {2, 2}; }
 
  GTSAM_EXPORT
  friend std::ostream &operator<<(std::ostream &os, const Pose2& p);
 
 
 private:
 
#if GTSAM_ENABLE_BOOST_SERIALIZATION  //
  // Serialization function
  friend class boost::serialization::access;
  template<class Archive>
  void serialize(Archive & ar, const unsigned int /*version*/) {
    ar & BOOST_SERIALIZATION_NVP(t_);
    ar & BOOST_SERIALIZATION_NVP(r_);
  }
#endif
 
public:
  // Align for Point2, which is either derived from, or is typedef, of Vector2
  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
}; // Pose2
 
template <>
inline Matrix wedge<Pose2>(const Vector& xi) {
  // NOTE(chris): Need eval() as workaround for Apple clang + avx2.
  return Matrix(Pose2::wedge(xi(0),xi(1),xi(2))).eval();
}
 
// Convenience typedef
using Pose2Pair = std::pair<Pose2, Pose2>;
using Pose2Pairs = std::vector<Pose2Pair>;
 
template <>
struct traits<Pose2> : public internal::LieGroup<Pose2> {};
 
template <>
struct traits<const Pose2> : public internal::LieGroup<Pose2> {};
 
// bearing and range traits, used in RangeFactor
template <typename T>
struct Bearing<Pose2, T> : HasBearing<Pose2, T, Rot2> {};
 
template <typename T>
struct Range<Pose2, T> : HasRange<Pose2, T, double> {};
 
} // namespace gtsam