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 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) : r_(pose.r_), t_(pose.t_) {}

  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)) {
    assert(T.rows() == 3 && T.cols() == 3);
  }


  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);


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

  GTSAM_EXPORT bool equals(const Pose2& pose, double tol = 1e-9) const;


  inline static Pose2 Identity() { return Pose2(); }

  GTSAM_EXPORT Pose2 inverse() const;

  inline Pose2 operator*(const Pose2& p2) const {
    return Pose2(r_*p2.r(), t_ + r_*p2.t());
  }


  GTSAM_EXPORT static Pose2 Expmap(const Vector3& xi, ChartJacobian H = {});

  GTSAM_EXPORT static Vector3 Logmap(const Pose2& p, ChartJacobian H = {});

  GTSAM_EXPORT Matrix3 AdjointMap() const;

  inline Vector3 Adjoint(const Vector3& xi) const {
    return AdjointMap()*xi;
  }

  GTSAM_EXPORT 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;
  }

  GTSAM_EXPORT static Matrix3 ExpmapDerivative(const Vector3& v);

  GTSAM_EXPORT static Matrix3 LogmapDerivative(const Pose2& v);

  // Chart at origin, depends on compile-time flag SLOW_BUT_CORRECT_EXPMAP
  struct ChartAtOrigin {
        GTSAM_EXPORT static Pose2 Retract(const Vector3& v, ChartJacobian H = {});
        GTSAM_EXPORT static Vector3 Local(const Pose2& r, ChartJacobian H = {});
  };

  using LieGroup<Pose2, 3>::inverse; // version with derivative


  GTSAM_EXPORT Point2 transformTo(const Point2& point,
      OptionalJacobian<2, 3> Dpose = {},
      OptionalJacobian<2, 2> Dpoint = {}) const;

  Matrix transformTo(const Matrix& points) const;

  GTSAM_EXPORT 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_;
  }

  GTSAM_EXPORT Matrix3 matrix() const;

  GTSAM_EXPORT Rot2 bearing(const Point2& point,
               OptionalJacobian<1, 3> H1={}, OptionalJacobian<1, 2> H2={}) const;

  GTSAM_EXPORT Rot2 bearing(const Pose2& pose,
               OptionalJacobian<1, 3> H1={}, OptionalJacobian<1, 3> H2={}) const;

  GTSAM_EXPORT double range(const Point2& point,
      OptionalJacobian<1, 3> H1={},
      OptionalJacobian<1, 2> H2={}) const;

  GTSAM_EXPORT 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:

#ifdef 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