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>
 
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);
  }
 
 
  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 = boost::none);
 
  GTSAM_EXPORT static Vector3 Logmap(const Pose2& p, ChartJacobian H = boost::none);
 
  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 = boost::none);
        GTSAM_EXPORT static Vector3 Local(const Pose2& r, ChartJacobian H = boost::none);
  };
 
  using LieGroup<Pose2, 3>::inverse; // version with derivative
 
 
  GTSAM_EXPORT Point2 transformTo(const Point2& point,
      OptionalJacobian<2, 3> Dpose = boost::none,
      OptionalJacobian<2, 2> Dpoint = boost::none) const;
 
  GTSAM_EXPORT Point2 transformFrom(const Point2& point,
      OptionalJacobian<2, 3> Dpose = boost::none,
      OptionalJacobian<2, 2> Dpoint = boost::none) 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() const { return t_; }
 
  inline const Rot2&   rotation() const { return r_; }
 
  GTSAM_EXPORT Matrix3 matrix() const;
 
  GTSAM_EXPORT Rot2 bearing(const Point2& point,
               OptionalJacobian<1, 3> H1=boost::none, OptionalJacobian<1, 2> H2=boost::none) const;
 
  GTSAM_EXPORT Rot2 bearing(const Pose2& pose,
               OptionalJacobian<1, 3> H1=boost::none, OptionalJacobian<1, 3> H2=boost::none) const;
 
  GTSAM_EXPORT double range(const Point2& point,
      OptionalJacobian<1, 3> H1=boost::none,
      OptionalJacobian<1, 2> H2=boost::none) const;
 
  GTSAM_EXPORT double range(const Pose2& point,
      OptionalJacobian<1, 3> H1=boost::none,
      OptionalJacobian<1, 3> H2=boost::none) const;
 
 
  inline static std::pair<size_t, size_t> translationInterval() { return std::make_pair(0, 1); }
 
  static std::pair<size_t, size_t> rotationInterval() { return std::make_pair(2, 2); }
 
  GTSAM_EXPORT
  friend std::ostream &operator<<(std::ostream &os, const Pose2& p);
 
 
 private:
 
  // 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_);
  }
 
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();
}
 
typedef std::pair<Point2,Point2> Point2Pair;
GTSAM_EXPORT boost::optional<Pose2> align(const std::vector<Point2Pair>& pairs);
 
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