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 <boost/optional.hpp>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/DerivedValue.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Rot2.h>

namespace gtsam {

class GTSAM_EXPORT Pose2 : public DerivedValue<Pose2> {

public:
  static const size_t dimension = 3;

  typedef Rot2 Rotation;
  typedef Point2 Translation;

private:
  Rot2 r_;
  Point2 t_;

public:


  Pose2() {} // default is origin

  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) {
    *this = Expmap(v);
  }


  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(boost::optional<Matrix&> H1=boost::none) const;

  Pose2 compose(const Pose2& p2,
      boost::optional<Matrix&> H1 = boost::none,
      boost::optional<Matrix&> H2 = boost::none) const;

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

  Pose2 between(const Pose2& p2,
      boost::optional<Matrix&> H1=boost::none,
      boost::optional<Matrix&> H2=boost::none) const;



  inline static size_t Dim() { return dimension; }

  inline size_t dim() const { return dimension; }

  Pose2 retract(const Vector& v) const;

  Vector localCoordinates(const Pose2& p2) const;


  static Pose2 Expmap(const Vector& xi);

  static Vector Logmap(const Pose2& p);

  Matrix AdjointMap() const;
  inline Vector Adjoint(const Vector& xi) const {
    assert(xi.size() == 3);
    return AdjointMap()*xi;
  }

  static inline Matrix wedge(double vx, double vy, double w) {
    return (Matrix(3,3) <<
        0.,-w,  vx,
        w,  0., vy,
        0., 0.,  0.);
  }


  Point2 transform_to(const Point2& point,
      boost::optional<Matrix&> H1=boost::none,
      boost::optional<Matrix&> H2=boost::none) const;

  Point2 transform_from(const Point2& point,
      boost::optional<Matrix&> H1=boost::none,
      boost::optional<Matrix&> H2=boost::none) const;

  inline Point2 operator*(const Point2& point) const { return transform_from(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_; }

  Matrix matrix() const;

  Rot2 bearing(const Point2& point,
      boost::optional<Matrix&> H1=boost::none,
      boost::optional<Matrix&> H2=boost::none) const;

  Rot2 bearing(const Pose2& point,
      boost::optional<Matrix&> H1=boost::none,
      boost::optional<Matrix&> H2=boost::none) const;

  double range(const Point2& point,
      boost::optional<Matrix&> H1=boost::none,
      boost::optional<Matrix&> H2=boost::none) const;

  double range(const Pose2& point,
      boost::optional<Matrix&> H1=boost::none,
      boost::optional<Matrix&> 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); }

private:

  // Serialization function
  friend class boost::serialization::access;
  template<class Archive>
  void serialize(Archive & ar, const unsigned int version) {
    ar & boost::serialization::make_nvp("Pose2",
        boost::serialization::base_object<Value>(*this));
    ar & BOOST_SERIALIZATION_NVP(t_);
    ar & BOOST_SERIALIZATION_NVP(r_);
  }
}; // Pose2

template <>
inline Matrix wedge<Pose2>(const Vector& xi) {
  return Pose2::wedge(xi(0),xi(1),xi(2));
}

typedef std::pair<Point2,Point2> Point2Pair;
GTSAM_EXPORT boost::optional<Pose2> align(const std::vector<Point2Pair>& pairs);


} // namespace gtsam