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/ExtendedPose3.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/base/Lie.h>
 
#if GTSAM_ENABLE_BOOST_SERIALIZATION
#include <boost/serialization/base_object.hpp>
#endif
 
namespace gtsam {
 
class Pose2;
// forward declare

class GTSAM_EXPORT Pose3: public ExtendedPose3<1, Pose3> {
public:
  using Base = ExtendedPose3<1, Pose3>;

  typedef Rot3 Rotation;
  typedef Point3 Translation;
  inline constexpr static auto dimension = 6;
 
public:
  using Vector16 = Eigen::Matrix<double, 16, 1>;
  using Base::operator*;


  Pose3() : Base() {}

  Pose3(const Pose3& pose) = default;
 
  Pose3& operator=(const Pose3& other) = default;
 
  Pose3(const Base& other) : Base(other) {}

  Pose3(const Rot3& R, const Point3& t)
      : Base(R, Vector3(t.x(), t.y(), t.z())) {}

  explicit Pose3(const Pose2& pose2);

  Pose3(const Matrix &T) : Base(Matrix4(T)) {}

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

  static Pose3 FromPose2(const Pose2& p, OptionalJacobian<6,3> H = {});

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


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

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


  Pose3 interpolateRt(const Pose3& T, double t,
                      OptionalJacobian<6, 6> Hself = {},
                      OptionalJacobian<6, 6> Harg = {},
                      OptionalJacobian<6, 1> Ht = {}) const;

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

 
  using LieAlgebra = Matrix4;

  static Pose3 Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi = {});
 
  // 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);}
 
  // Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP
  struct GTSAM_EXPORT ChartAtOrigin {
    static Pose3 Retract(const Vector6& xi, ChartJacobian Hxi = {});
    static Vector6 Local(const Pose3& pose, ChartJacobian Hpose = {});
  };


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

  Matrix transformFrom(ConstMatrixView 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(ConstMatrixView points) const;


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

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

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

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

  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);

 
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
  static inline LieAlgebra wedge(double wx, double wy, double wz, double vx,
                                 double vy, double vz) {
    return Hat((TangentVector() << wx, wy, wz, vx, vy, vz).finished());
  }
#endif
 
 private:
#if 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
};
// Pose3 class
 
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
template<>
inline Matrix wedge<Pose3>(const Vector& xi) {
  // NOTE(chris): Need eval() as workaround for Apple clang + avx2.
  return Matrix(Pose3::Hat(xi)).eval();
}
#endif
 
// 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::MatrixLieGroup<Pose3, 4> {
  inline constexpr static int QcqpVectorDim = 13;

  template <int D = 1>
  static Matrix QcqpValue(const Pose3& value) {
    if constexpr (D == 1) {
      const Matrix4 T = value.matrix();
      Eigen::Matrix<double, 13, 1> X;
      X(0, 0) = 1.0;
      X.segment<3>(1) = T.col(0).head<3>();
      X.segment<3>(4) = T.col(1).head<3>();
      X.segment<3>(7) = T.col(2).head<3>();
      X.segment<3>(10) = T.col(3).head<3>();
      return X;
    } else {
      throw std::invalid_argument(
          "traits<Pose3>::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(10);
 
      Matrix A = Matrix::Zero(13, 13);
 
      // Homogenization.
      A(0, 0) = 1.0;
      constraints.emplace_back(A, 1.0);
 
      // cross(R.col(1), R.col(2)) = x(0) * R.col(0).
      A.setZero();
      A(5, 9) = 0.5;
      A(9, 5) = 0.5;
      A(6, 8) = -0.5;
      A(8, 6) = -0.5;
      A(0, 1) = -0.5;
      A(1, 0) = -0.5;
      constraints.emplace_back(A, 0.0);
 
      A.setZero();
      A(6, 7) = 0.5;
      A(7, 6) = 0.5;
      A(4, 9) = -0.5;
      A(9, 4) = -0.5;
      A(0, 2) = -0.5;
      A(2, 0) = -0.5;
      constraints.emplace_back(A, 0.0);
 
      A.setZero();
      A(4, 8) = 0.5;
      A(8, 4) = 0.5;
      A(5, 7) = -0.5;
      A(7, 5) = -0.5;
      A(0, 3) = -0.5;
      A(3, 0) = -0.5;
      constraints.emplace_back(A, 0.0);
 
      // RR^T = I.
      A.setZero();
      A(1, 1) = 1.0;
      A(4, 4) = 1.0;
      A(7, 7) = 1.0;
      constraints.emplace_back(A, 1.0);
 
      A.setZero();
      A(1, 2) = 0.5;
      A(2, 1) = 0.5;
      A(4, 5) = 0.5;
      A(5, 4) = 0.5;
      A(7, 8) = 0.5;
      A(8, 7) = 0.5;
      constraints.emplace_back(A, 0.0);
 
      A.setZero();
      A(1, 3) = 0.5;
      A(3, 1) = 0.5;
      A(4, 6) = 0.5;
      A(6, 4) = 0.5;
      A(7, 9) = 0.5;
      A(9, 7) = 0.5;
      constraints.emplace_back(A, 0.0);
 
      A.setZero();
      A(2, 2) = 1.0;
      A(5, 5) = 1.0;
      A(8, 8) = 1.0;
      constraints.emplace_back(A, 1.0);
 
      A.setZero();
      A(2, 3) = 0.5;
      A(3, 2) = 0.5;
      A(5, 6) = 0.5;
      A(6, 5) = 0.5;
      A(8, 9) = 0.5;
      A(9, 8) = 0.5;
      constraints.emplace_back(A, 0.0);
 
      A.setZero();
      A(3, 3) = 1.0;
      A(6, 6) = 1.0;
      A(9, 9) = 1.0;
      constraints.emplace_back(A, 1.0);
 
      return constraints;
    } else {
      throw std::invalid_argument(
          "traits<Pose3>::QcqpConstraints only supports D=1.");
    }
  }

  template <int D>
  static Pose3 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<Pose3>::FromQcqpValue requires a 13-by-1 vector with a "
            "nonzero homogenization entry.");
      }
      const Vector x = X.col(0) / X(0, 0);
      Matrix3 R;
      R.col(0) = x.segment<3>(1);
      R.col(1) = x.segment<3>(4);
      R.col(2) = x.segment<3>(7);
      return Pose3(Rot3::ClosestTo(R), x.segment<3>(10));
    } else {
      throw std::invalid_argument(
          "traits<Pose3>::FromQcqpValue only supports D=1.");
    }
  }
};
 
template <>
struct traits<const Pose3> : public traits<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