ExtendedPose3.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
 
 * -------------------------------------------------------------------------- */

 
#pragma once
 
#include <gtsam/base/MatrixConstants.h>
#include <gtsam/base/MatrixLieGroup.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Rot3.h>
 
#include <cassert>
#include <iostream>
#include <string>
#include <type_traits>
 
#if GTSAM_ENABLE_BOOST_SERIALIZATION
#include <boost/serialization/nvp.hpp>
#endif
 
namespace gtsam {
 
template <int K, class Derived = void>
class ExtendedPose3;

template <int K_, class Derived>
class ExtendedPose3 : public MatrixLieGroup<
                          std::conditional_t<std::is_void_v<Derived>,
                                             ExtendedPose3<K_, void>, Derived>,
                          (K_ == Eigen::Dynamic) ? Eigen::Dynamic : 3 + 3 * K_,
                          (K_ == Eigen::Dynamic) ? Eigen::Dynamic : 3 + K_> {
 public:
  static constexpr int K = K_;
  using This = std::conditional_t<std::is_void_v<Derived>,
                                  ExtendedPose3<K, void>, Derived>;
  inline constexpr static int dimension =
      (K == Eigen::Dynamic) ? Eigen::Dynamic : 3 + 3 * K;
  inline constexpr static int matrixDim =
      (K == Eigen::Dynamic) ? Eigen::Dynamic : 3 + K;
 
  using Base = MatrixLieGroup<This, dimension, matrixDim>;
  using TangentVector = typename Base::TangentVector;
  using Jacobian = typename Base::Jacobian;
  using ChartJacobian = typename Base::ChartJacobian;
  using ComponentJacobian =
      std::conditional_t<dimension == Eigen::Dynamic,
                         OptionalJacobian<Eigen::Dynamic, Eigen::Dynamic>,
                         OptionalJacobian<3, dimension>>;
  using MatrixRep = Eigen::Matrix<double, matrixDim, matrixDim>;
  using LieAlgebra = Eigen::Matrix<double, matrixDim, matrixDim>;
  using Matrix3K = Eigen::Matrix<double, 3, K>;
 
  static_assert(K == Eigen::Dynamic || K >= 1,
                "ExtendedPose3<K>: K should be >= 1 or Eigen::Dynamic.");
 
 protected:
  Rot3 R_;      
  Matrix3K t_;  
 
  template <int K__>
  using IsDynamic = typename std::enable_if<K__ == Eigen::Dynamic, void>::type;
  template <int K__>
  using IsFixed = typename std::enable_if<K__ >= 1, void>::type;
 
 public:

  template <int K__ = K_, typename = IsFixed<K__>>
  ExtendedPose3() : R_(Rot3::Identity()), t_(Matrix3K::Zero()) {}

  template <int K__ = K_, typename = IsDynamic<K__>>
  explicit ExtendedPose3(size_t k = 0)
      : R_(Rot3::Identity()), t_(3, static_cast<Eigen::Index>(k)) {
    t_.setZero();
  }

  ExtendedPose3(const ExtendedPose3&) = default;

  ExtendedPose3& operator=(const ExtendedPose3&) = default;

  ExtendedPose3(const Rot3& R, const Matrix3K& x);

  template <int FixedK = K, typename = IsFixed<FixedK>, typename... Vecs,
            typename = std::enable_if_t<
                sizeof...(Vecs) == FixedK &&
                (std::is_constructible_v<Point3, Vecs> && ...)>>
  ExtendedPose3(const Rot3& R, const Vecs&... xs);

  explicit ExtendedPose3(const MatrixRep& T);


  static size_t Dimension(size_t k) { return 3 + 3 * k; }

  size_t k() const { return static_cast<size_t>(t_.cols()); }

  size_t dim() const { return Dimension(k()); }

  const Rot3& rotation(ComponentJacobian H = {}) const;

  Point3 x(size_t i, ComponentJacobian H = {}) const;

  const Matrix3K& xMatrix() const;

  Matrix3K& xMatrix();


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

  bool equals(const ExtendedPose3& other, double tol = 1e-9) const;


  template <int K__ = K_, typename = IsFixed<K__>>
  static This Identity() {
    return MakeReturn(ExtendedPose3());
  }

  template <int K__ = K_, typename = IsDynamic<K__>>
  static This Identity(size_t k = 0) {
    return MakeReturn(ExtendedPose3(k));
  }

  This inverse() const;

  This operator*(const This& other) const;


  static This Expmap(const TangentVector& xi, ChartJacobian Hxi = {});

  static TangentVector Logmap(const This& pose, ChartJacobian Hpose = {});

  Jacobian AdjointMap() const;

  static Jacobian adjointMap(const TangentVector& xi);

  static Jacobian ExpmapDerivative(const TangentVector& xi);

  static Jacobian LogmapDerivative(const TangentVector& xi);

  static Jacobian LogmapDerivative(const This& pose);

  struct ChartAtOrigin {
    static This Retract(const TangentVector& xi, ChartJacobian Hxi = {});

    static TangentVector Local(const This& pose, ChartJacobian Hpose = {});
  };
 
  using LieGroup<This, dimension>::inverse;


  MatrixRep matrix() const;

  static LieAlgebra Hat(const TangentVector& xi);

  static TangentVector Vee(const LieAlgebra& X);

 
  friend std::ostream& operator<<(std::ostream& os, const ExtendedPose3& p) {
    os << "R: " << p.R_ << "\n";
    os << "x: " << p.t_;
    return os;
  }
 
 protected:
  static This MakeReturn(const ExtendedPose3& value) {
    if constexpr (std::is_void_v<Derived>) {
      return value;
    } else {
      return This(value);
    }
  }
 
  static const ExtendedPose3& AsBase(const This& value) {
    if constexpr (std::is_void_v<Derived>) {
      return value;
    } else {
      return static_cast<const ExtendedPose3&>(value);
    }
  }
 
  static size_t RuntimeK(const TangentVector& xi);
  static void ZeroJacobian(ChartJacobian H, Eigen::Index d);
 
 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
};

using Se23 = ExtendedPose3<2>;
using ExtendedPose3d = ExtendedPose3<Eigen::Dynamic>;
 
template <int K, class Derived>
struct traits<ExtendedPose3<K, Derived>>
    : public internal::MatrixLieGroup<ExtendedPose3<K, Derived>,
                                      ExtendedPose3<K, Derived>::matrixDim> {};
 
template <int K, class Derived>
struct traits<const ExtendedPose3<K, Derived>>
    : public internal::MatrixLieGroup<ExtendedPose3<K, Derived>,
                                      ExtendedPose3<K, Derived>::matrixDim> {};
 
}  // namespace gtsam
 
#include "ExtendedPose3-inl.h"