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 MatrixLieGroup<Pose2, 3, 3> {
 
public:

  using Rotation = Rot2;
  using Translation = Point2;

  using LieAlgebra = Matrix3;
 
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
  }


  static std::optional<Pose2> Align(const Point2Pairs& abPointPairs);
 
  // Version of Pose2::Align that takes 2 matrices.
  static std::optional<Pose2> Align(ConstMatrixView a, ConstMatrixView 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;

  static Matrix3 adjointMap(const Vector3& v);
 
  // 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 Matrix3 ExpmapDerivative(const Vector3& v);

  static Matrix3 LogmapDerivative(const Pose2& pose);

  static Matrix3 LogmapDerivative(const Vector3& xi);
 
  // 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

  static Matrix3 Hat(const Vector3& xi);

  static Vector3 Vee(const Matrix3& X);


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

  Matrix transformTo(ConstMatrixView points) const;

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

  Matrix transformFrom(ConstMatrixView 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 = Matrix13{{0, 0, 1}};
    return r_;
  }

  Matrix3 matrix() const;

  Vector9 vec(OptionalJacobian<9, 3> H = {}) 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);

 
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
  static inline Matrix3 wedge(double vx, double vy, double w) {
    return Hat(TangentVector(vx, vy, w));
  }

  Pose2(const Vector& v) : Pose2() {
    *this = Expmap(v);
  }
#endif
 
 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
}; // Pose2
 
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
template <>
inline Matrix wedge<Pose2>(const Vector& xi) {
  // NOTE(chris): Need eval() as workaround for Apple clang + avx2.
  return Matrix(Pose2::Hat(xi)).eval();
}
#endif
 
// Convenience typedef
using Pose2Pair = std::pair<Pose2, Pose2>;
using Pose2Pairs = std::vector<Pose2Pair>;

template <>
struct traits<Pose2> : public internal::MatrixLieGroup<Pose2, 3> {
  inline constexpr static int QcqpVectorDim = 7;

  template <int D = 1>
  static Matrix QcqpValue(const Pose2& value) {
    if constexpr (D == 1) {
      const Matrix3 T = value.matrix();
      Vector7 X;
      X(0, 0) = 1.0;
      X.segment<2>(1) = T.col(0).head<2>();
      X.segment<2>(3) = T.col(1).head<2>();
      X.segment<2>(5) = T.col(2).head<2>();
      return X;
    } else {
      throw std::invalid_argument(
          "traits<Pose2>::QcqpValue only supports D=1.");
    }
  }

  template <int D = 1>
  static std::vector<std::pair<Matrix, double>> QcqpConstraints() {
    if constexpr (D == 1) {
      std::vector<std::pair<Matrix, double>> constraints;
      constraints.reserve(5);
 
      Matrix A = Matrix::Zero(7, 7);
 
      // Homogenization.
      A(0, 0) = 1.0;
      constraints.emplace_back(A, 1.0);
 
      // det(R) = r00*r11 - r10*r01 = 1.
      A.setZero();
      A(1, 4) = 0.5;
      A(4, 1) = 0.5;
      A(2, 3) = -0.5;
      A(3, 2) = -0.5;
      constraints.emplace_back(A, 1.0);
 
      // RR^T = I.
      A.setZero();
      A(1, 1) = 1.0;
      A(3, 3) = 1.0;
      constraints.emplace_back(A, 1.0);
 
      A.setZero();
      A(1, 2) = 0.5;
      A(2, 1) = 0.5;
      A(3, 4) = 0.5;
      A(4, 3) = 0.5;
      constraints.emplace_back(A, 0.0);
 
      A.setZero();
      A(2, 2) = 1.0;
      A(4, 4) = 1.0;
      constraints.emplace_back(A, 1.0);
 
      return constraints;
    } else {
      throw std::invalid_argument(
          "traits<Pose2>::QcqpConstraints only supports D=1.");
    }
  }

  template <int D>
  static Pose2 FromQcqpValue(const Matrix& X) {
    if constexpr (D == 1) {
      if (X.rows() != QcqpVectorDim || X.cols() != 1 ||
          std::abs(X(0, 0)) < 1e-9) {
        throw std::invalid_argument(
            "traits<Pose2>::FromQcqpValue requires a 7-by-1 vector with a "
            "nonzero homogenization entry.");
      }
      const Vector x = X.col(0) / X(0, 0);
      Matrix2 R;
      R.col(0) = x.segment<2>(1);
      R.col(1) = x.segment<2>(3);
      return Pose2(Rot2::ClosestTo(R), x.segment<2>(5));
    } else {
      throw std::invalid_argument(
          "traits<Pose2>::FromQcqpValue only supports D=1.");
    }
  }
};
 
template <>
struct traits<const Pose2> : public traits<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