Pose3.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/config.h>

#ifndef GTSAM_POSE3_EXPMAP
#define POSE3_DEFAULT_COORDINATES_MODE Pose3::FIRST_ORDER
#else
#define POSE3_DEFAULT_COORDINATES_MODE Pose3::EXPMAP
#endif

#include <gtsam/base/DerivedValue.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Rot3.h>

namespace gtsam {

class Pose2;
// forward declare

class GTSAM_EXPORT Pose3: public DerivedValue<Pose3> {
public:
  static const size_t dimension = 6;

  typedef Rot3 Rotation;
  typedef Point3 Translation;

private:

  Rot3 R_;   
  Point3 t_; 

public:


  Pose3() {
  }

  Pose3(const Pose3& pose) :
      R_(pose.R_), t_(pose.t_) {
  }

  Pose3(const Rot3& R, const Point3& t) :
      R_(R), t_(t) {
  }

  explicit Pose3(const Pose2& pose2);

  Pose3(const Matrix &T) :
      R_(T(0, 0), T(0, 1), T(0, 2), T(1, 0), T(1, 1), T(1, 2), T(2, 0), T(2, 1),
          T(2, 2)), t_(T(0, 3), T(1, 3), T(2, 3)) {
  }


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

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


  static Pose3 identity() {
    return Pose3();
  }

  Pose3 inverse(boost::optional<Matrix&> H1 = boost::none) const;

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

  Pose3 operator*(const Pose3& T) const {
    return Pose3(R_ * T.R_, t_ + R_ * T.t_);
  }

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


  enum CoordinatesMode {
    EXPMAP, 
    FIRST_ORDER 
  };

  static size_t Dim() {
    return dimension;
  }

  size_t dim() const {
    return dimension;
  }

  Pose3 retractFirstOrder(const Vector& d) const;

  Pose3 retract(const Vector& d, Pose3::CoordinatesMode mode =
      POSE3_DEFAULT_COORDINATES_MODE) const;

      Vector6 localCoordinates(const Pose3& T2, Pose3::CoordinatesMode mode =POSE3_DEFAULT_COORDINATES_MODE) const;


    static Pose3 Expmap(const Vector& xi);

    static Vector6 Logmap(const Pose3& p);

    Matrix6 AdjointMap() const; 

    Vector Adjoint(const Vector& xi_b) const {return AdjointMap()*xi_b; } 

    static Matrix6 adjointMap(const Vector& xi);

    static Vector adjoint(const Vector& xi, const Vector& y, boost::optional<Matrix&> H = boost::none);

    static Vector adjointTranspose(const Vector& xi, const Vector& y, boost::optional<Matrix&> H = boost::none);

    static Matrix6 dExpInv_exp(const Vector&  xi);

    static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz) {
      return (Matrix(4,4) <<
          0.,-wz,  wy,  vx,
          wz,  0.,-wx,  vy,
          -wy, wx,   0., vz,
          0.,  0.,  0.,  0.);
    }


    Point3 transform_from(const Point3& p,
        boost::optional<Matrix&> Dpose=boost::none, boost::optional<Matrix&> Dpoint=boost::none) const;

    inline Point3 operator*(const Point3& p) const { return transform_from(p); }

    Point3 transform_to(const Point3& p,
        boost::optional<Matrix&> Dpose=boost::none, boost::optional<Matrix&> Dpoint=boost::none) const;


    const Rot3& rotation() const { return R_; }

    const Point3& translation() const { return t_; }

    double x() const { return t_.x(); }

    double y() const { return t_.y(); }

    double z() const { return t_.z(); }

    Matrix4 matrix() const;

    Pose3 transform_to(const Pose3& pose) const;

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

    double range(const Pose3& pose,
        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(3, 5); }

    static std::pair<size_t, size_t> rotationInterval() { return std::make_pair(0, 2); }

    GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Pose3& p);

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

  };// Pose3 class

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

typedef std::pair<Point3, Point3> Point3Pair;
GTSAM_EXPORT boost::optional<Pose3> align(const std::vector<Point3Pair>& pairs);

} // namespace gtsam