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>

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

namespace gtsam {

class Pose2;
// forward declare

class GTSAM_EXPORT Pose3: public LieGroup<Pose3, 6> {
public:

  typedef Rot3 Rotation;
  typedef Point3 Translation;

private:

  Rot3 R_; 
  Point3 t_; 

public:


  Pose3() : R_(traits<Rot3>::Identity()), t_(traits<Point3>::Identity()) {}

  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)) {
  }

  static Pose3 Create(const Rot3& R, const Point3& t,
                      OptionalJacobian<6, 3> HR = {},
                      OptionalJacobian<6, 3> Ht = {});

  static std::optional<Pose3> Align(const Point3Pairs& abPointPairs);

  // Version of Pose3::Align that takes 2 matrices.
  static std::optional<Pose3> Align(const Matrix& a, const Matrix& b);


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

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


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

  Pose3 inverse() const;

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

  Pose3 interpolateRt(const Pose3& T, double t) const;


  static Pose3 Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi = {});

  static Vector6 Logmap(const Pose3& pose, OptionalJacobian<6, 6> Hpose = {});

  Matrix6 AdjointMap() const;

  Vector6 Adjoint(const Vector6& xi_b,
                  OptionalJacobian<6, 6> H_this = {},
                  OptionalJacobian<6, 6> H_xib = {}) const;
  
  Vector6 AdjointTranspose(const Vector6& x,
                           OptionalJacobian<6, 6> H_this = {},
                           OptionalJacobian<6, 6> H_x = {}) const;

  static Matrix6 adjointMap(const Vector6& xi);

  static Vector6 adjoint(const Vector6& xi, const Vector6& y,
                         OptionalJacobian<6, 6> Hxi = {},
                         OptionalJacobian<6, 6> H_y = {});

  // temporary fix for wrappers until case issue is resolved
  static Matrix6 adjointMap_(const Vector6 &xi) { return adjointMap(xi);}
  static Vector6 adjoint_(const Vector6 &xi, const Vector6 &y) { return adjoint(xi, y);}

  static Vector6 adjointTranspose(const Vector6& xi, const Vector6& y,
                                  OptionalJacobian<6, 6> Hxi = {},
                                  OptionalJacobian<6, 6> H_y = {});

  static Matrix6 ExpmapDerivative(const Vector6& xi);

  static Matrix6 LogmapDerivative(const Pose3& xi);

  // Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP
  struct ChartAtOrigin {
    static Pose3 Retract(const Vector6& xi, ChartJacobian Hxi = {});
    static Vector6 Local(const Pose3& pose, ChartJacobian Hpose = {});
  };

  static Matrix3 ComputeQforExpmapDerivative(
      const Vector6& xi, double nearZeroThreshold = 1e-5);

  using LieGroup<Pose3, 6>::inverse; // version with derivative

  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.).finished();
  }


  Point3 transformFrom(const Point3& point, OptionalJacobian<3, 6> Hself =
      {}, OptionalJacobian<3, 3> Hpoint = {}) const;

  Matrix transformFrom(const Matrix& points) const;

  inline Point3 operator*(const Point3& point) const {
    return transformFrom(point);
  }

  Point3 transformTo(const Point3& point, OptionalJacobian<3, 6> Hself =
      {}, OptionalJacobian<3, 3> Hpoint = {}) const;

  Matrix transformTo(const Matrix& points) const;


  const Rot3& rotation(OptionalJacobian<3, 6> Hself = {}) const;

  const Point3& translation(OptionalJacobian<3, 6> Hself = {}) const;

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

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

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

  Matrix4 matrix() const;

  Pose3 transformPoseFrom(const Pose3& aTb, OptionalJacobian<6, 6> Hself = {},
                                            OptionalJacobian<6, 6> HaTb = {}) const;

  Pose3 transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> Hself = {},
                                          OptionalJacobian<6, 6> HwTb = {}) const;

  double range(const Point3& point, OptionalJacobian<1, 6> Hself = {},
      OptionalJacobian<1, 3> Hpoint = {}) const;

  double range(const Pose3& pose, OptionalJacobian<1, 6> Hself = {},
      OptionalJacobian<1, 6> Hpose = {}) const;

  Unit3 bearing(const Point3& point, OptionalJacobian<2, 6> Hself = {},
      OptionalJacobian<2, 3> Hpoint = {}) const;

  Unit3 bearing(const Pose3& pose, OptionalJacobian<2, 6> Hself = {},
      OptionalJacobian<2, 6> Hpose = {}) const;


  inline static std::pair<size_t, size_t> translationInterval() {
    return {3, 5};
  }

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

  Pose3 slerp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx = {},
                                             OptionalJacobian<6, 6> Hy = {}) const;

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

 private:
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION

  friend class boost::serialization::access;
  template<class Archive>
  void serialize(Archive & ar, const unsigned int /*version*/) {
    ar & BOOST_SERIALIZATION_NVP(R_);
    ar & BOOST_SERIALIZATION_NVP(t_);
  }
#endif

#ifdef GTSAM_USE_QUATERNIONS
  // Align if we are using Quaternions
  public:
    GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#endif
};
// 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));
}

// Convenience typedef
using Pose3Pair = std::pair<Pose3, Pose3>;
using Pose3Pairs = std::vector<std::pair<Pose3, Pose3> >;

// For MATLAB wrapper
typedef std::vector<Pose3> Pose3Vector;

template <>
struct traits<Pose3> : public internal::LieGroup<Pose3> {};

template <>
struct traits<const Pose3> : public internal::LieGroup<Pose3> {};

// bearing and range traits, used in RangeFactor
template <>
struct Bearing<Pose3, Point3> : HasBearing<Pose3, Point3, Unit3> {};

template<>
struct Bearing<Pose3, Pose3> : HasBearing<Pose3, Pose3, Unit3> {};

template <typename T>
struct Range<Pose3, T> : HasRange<Pose3, T, double> {};

}  // namespace gtsam