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) = default;
  //  :
  //     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 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(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 GTSAM_EXPORT 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);
 
  static Vector3 ExpmapTranslation(const Vector3& w, const Vector3& v,
                                   OptionalJacobian<3, 3> Q = {},
                                   OptionalJacobian<3, 3> J = {},
                                   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:
#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
 
#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