sim3.hpp

Go to the documentation of this file.

 
#ifndef SOPHUS_SIM3_HPP
#define SOPHUS_SIM3_HPP
 
#include "rxso3.hpp"
#include "sim_details.hpp"
 
namespace Sophus {
template <class Scalar_, int Options = 0>
class Sim3;
using Sim3d = Sim3<double>;
using Sim3f = Sim3<float>;
}  // namespace Sophus
 
namespace Eigen {
namespace internal {
 
template <class Scalar_, int Options>
struct traits<Sophus::Sim3<Scalar_, Options>> {
  using Scalar = Scalar_;
  using TranslationType = Sophus::Vector3<Scalar, Options>;
  using RxSO3Type = Sophus::RxSO3<Scalar, Options>;
};
 
template <class Scalar_, int Options>
struct traits<Map<Sophus::Sim3<Scalar_>, Options>>
    : traits<Sophus::Sim3<Scalar_, Options>> {
  using Scalar = Scalar_;
  using TranslationType = Map<Sophus::Vector3<Scalar>, Options>;
  using RxSO3Type = Map<Sophus::RxSO3<Scalar>, Options>;
};
 
template <class Scalar_, int Options>
struct traits<Map<Sophus::Sim3<Scalar_> const, Options>>
    : traits<Sophus::Sim3<Scalar_, Options> const> {
  using Scalar = Scalar_;
  using TranslationType = Map<Sophus::Vector3<Scalar> const, Options>;
  using RxSO3Type = Map<Sophus::RxSO3<Scalar> const, Options>;
};
}  // namespace internal
}  // namespace Eigen
 
namespace Sophus {
 
template <class Derived>
class Sim3Base {
 public:
  using Scalar = typename Eigen::internal::traits<Derived>::Scalar;
  using TranslationType =
      typename Eigen::internal::traits<Derived>::TranslationType;
  using RxSO3Type = typename Eigen::internal::traits<Derived>::RxSO3Type;
  using QuaternionType = typename RxSO3Type::QuaternionType;
 
  static int constexpr DoF = 7;
  static int constexpr num_parameters = 7;
  static int constexpr N = 4;
  using Transformation = Matrix<Scalar, N, N>;
  using Point = Vector3<Scalar>;
  using HomogeneousPoint = Vector4<Scalar>;
  using Line = ParametrizedLine3<Scalar>;
  using Tangent = Vector<Scalar, DoF>;
  using Adjoint = Matrix<Scalar, DoF, DoF>;
 
  template <typename OtherDerived>
  using ReturnScalar = typename Eigen::ScalarBinaryOpTraits<
      Scalar, typename OtherDerived::Scalar>::ReturnType;
 
  template <typename OtherDerived>
  using Sim3Product = Sim3<ReturnScalar<OtherDerived>>;
 
  template <typename PointDerived>
  using PointProduct = Vector3<ReturnScalar<PointDerived>>;
 
  template <typename HPointDerived>
  using HomogeneousPointProduct = Vector4<ReturnScalar<HPointDerived>>;
 
  SOPHUS_FUNC Adjoint Adj() const {
    Matrix3<Scalar> const R = rxso3().rotationMatrix();
    Adjoint res;
    res.setZero();
    res.template block<3, 3>(0, 0) = rxso3().matrix();
    res.template block<3, 3>(0, 3) = SO3<Scalar>::hat(translation()) * R;
    res.template block<3, 1>(0, 6) = -translation();
 
    res.template block<3, 3>(3, 3) = R;
 
    res(6, 6) = Scalar(1);
    return res;
  }
 
  template <class NewScalarType>
  SOPHUS_FUNC Sim3<NewScalarType> cast() const {
    return Sim3<NewScalarType>(rxso3().template cast<NewScalarType>(),
                               translation().template cast<NewScalarType>());
  }
 
  SOPHUS_FUNC Sim3<Scalar> inverse() const {
    RxSO3<Scalar> invR = rxso3().inverse();
    return Sim3<Scalar>(invR, invR * (translation() * Scalar(-1)));
  }
 
  SOPHUS_FUNC Tangent log() const {
    // The derivation of the closed-form Sim(3) logarithm for is done
    // analogously to the closed-form solution of the SE(3) logarithm, see
    // J. Gallier, D. Xu, "Computing exponentials of skew symmetric matrices
    // and logarithms of orthogonal matrices", IJRA 2002.
    // https:///pdfs.semanticscholar.org/cfe3/e4b39de63c8cabd89bf3feff7f5449fc981d.pdf
    // (Sec. 6., pp. 8)
    Tangent res;
    auto omega_sigma_and_theta = rxso3().logAndTheta();
    Vector3<Scalar> const omega =
        omega_sigma_and_theta.tangent.template head<3>();
    Scalar sigma = omega_sigma_and_theta.tangent[3];
    Matrix3<Scalar> const Omega = SO3<Scalar>::hat(omega);
    Matrix3<Scalar> const W_inv = details::calcWInv<Scalar, 3>(
        Omega, omega_sigma_and_theta.theta, sigma, scale());
 
    res.segment(0, 3) = W_inv * translation();
    res.segment(3, 3) = omega;
    res[6] = sigma;
    return res;
  }
 
  SOPHUS_FUNC Transformation matrix() const {
    Transformation homogenious_matrix;
    homogenious_matrix.template topLeftCorner<3, 4>() = matrix3x4();
    homogenious_matrix.row(3) =
        Matrix<Scalar, 4, 1>(Scalar(0), Scalar(0), Scalar(0), Scalar(1));
    return homogenious_matrix;
  }
 
  SOPHUS_FUNC Matrix<Scalar, 3, 4> matrix3x4() const {
    Matrix<Scalar, 3, 4> matrix;
    matrix.template topLeftCorner<3, 3>() = rxso3().matrix();
    matrix.col(3) = translation();
    return matrix;
  }
 
  SOPHUS_FUNC Sim3Base& operator=(Sim3Base const& other) = default;
 
  template <class OtherDerived>
  SOPHUS_FUNC Sim3Base<Derived>& operator=(
      Sim3Base<OtherDerived> const& other) {
    rxso3() = other.rxso3();
    translation() = other.translation();
    return *this;
  }
 
  template <typename OtherDerived>
  SOPHUS_FUNC Sim3Product<OtherDerived> operator*(
      Sim3Base<OtherDerived> const& other) const {
    return Sim3Product<OtherDerived>(
        rxso3() * other.rxso3(), translation() + rxso3() * other.translation());
  }
 
  template <typename PointDerived,
            typename = typename std::enable_if<
                IsFixedSizeVector<PointDerived, 3>::value>::type>
  SOPHUS_FUNC PointProduct<PointDerived> operator*(
      Eigen::MatrixBase<PointDerived> const& p) const {
    return rxso3() * p + translation();
  }
 
  template <typename HPointDerived,
            typename = typename std::enable_if<
                IsFixedSizeVector<HPointDerived, 4>::value>::type>
  SOPHUS_FUNC HomogeneousPointProduct<HPointDerived> operator*(
      Eigen::MatrixBase<HPointDerived> const& p) const {
    const PointProduct<HPointDerived> tp =
        rxso3() * p.template head<3>() + p(3) * translation();
    return HomogeneousPointProduct<HPointDerived>(tp(0), tp(1), tp(2), p(3));
  }
 
  SOPHUS_FUNC Line operator*(Line const& l) const {
    Line rotatedLine = rxso3() * l;
    return Line(rotatedLine.origin() + translation(), rotatedLine.direction());
  }
 
  template <typename OtherDerived,
            typename = typename std::enable_if<
                std::is_same<Scalar, ReturnScalar<OtherDerived>>::value>::type>
  SOPHUS_FUNC Sim3Base<Derived>& operator*=(
      Sim3Base<OtherDerived> const& other) {
    *static_cast<Derived*>(this) = *this * other;
    return *this;
  }
 
  SOPHUS_FUNC Sophus::Vector<Scalar, num_parameters> params() const {
    Sophus::Vector<Scalar, num_parameters> p;
    p << rxso3().params(), translation();
    return p;
  }
 
  SOPHUS_FUNC void setQuaternion(Eigen::Quaternion<Scalar> const& quat) {
    rxso3().setQuaternion(quat);
  }
 
  SOPHUS_FUNC QuaternionType const& quaternion() const {
    return rxso3().quaternion();
  }
 
  SOPHUS_FUNC Matrix3<Scalar> rotationMatrix() const {
    return rxso3().rotationMatrix();
  }
 
  SOPHUS_FUNC RxSO3Type& rxso3() {
    return static_cast<Derived*>(this)->rxso3();
  }
 
  SOPHUS_FUNC RxSO3Type const& rxso3() const {
    return static_cast<Derived const*>(this)->rxso3();
  }
 
  SOPHUS_FUNC Scalar scale() const { return rxso3().scale(); }
 
  SOPHUS_FUNC void setRotationMatrix(Matrix3<Scalar>& R) {
    rxso3().setRotationMatrix(R);
  }
 
  SOPHUS_FUNC void setScale(Scalar const& scale) { rxso3().setScale(scale); }
 
  SOPHUS_FUNC void setScaledRotationMatrix(Matrix3<Scalar> const& sR) {
    rxso3().setScaledRotationMatrix(sR);
  }
 
  SOPHUS_FUNC TranslationType& translation() {
    return static_cast<Derived*>(this)->translation();
  }
 
  SOPHUS_FUNC TranslationType const& translation() const {
    return static_cast<Derived const*>(this)->translation();
  }
};
 
template <class Scalar_, int Options>
class Sim3 : public Sim3Base<Sim3<Scalar_, Options>> {
 public:
  using Base = Sim3Base<Sim3<Scalar_, Options>>;
  using Scalar = Scalar_;
  using Transformation = typename Base::Transformation;
  using Point = typename Base::Point;
  using HomogeneousPoint = typename Base::HomogeneousPoint;
  using Tangent = typename Base::Tangent;
  using Adjoint = typename Base::Adjoint;
  using RxSo3Member = RxSO3<Scalar, Options>;
  using TranslationMember = Vector3<Scalar, Options>;
 
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  SOPHUS_FUNC Sim3();
 
  SOPHUS_FUNC Sim3(Sim3 const& other) = default;
 
  template <class OtherDerived>
  SOPHUS_FUNC Sim3(Sim3Base<OtherDerived> const& other)
      : rxso3_(other.rxso3()), translation_(other.translation()) {
    static_assert(std::is_same<typename OtherDerived::Scalar, Scalar>::value,
                  "must be same Scalar type");
  }
 
  template <class OtherDerived, class D>
  SOPHUS_FUNC Sim3(RxSO3Base<OtherDerived> const& rxso3,
                   Eigen::MatrixBase<D> const& translation)
      : rxso3_(rxso3), translation_(translation) {
    static_assert(std::is_same<typename OtherDerived::Scalar, Scalar>::value,
                  "must be same Scalar type");
    static_assert(std::is_same<typename D::Scalar, Scalar>::value,
                  "must be same Scalar type");
  }
 
  template <class D1, class D2>
  SOPHUS_FUNC Sim3(Eigen::QuaternionBase<D1> const& quaternion,
                   Eigen::MatrixBase<D2> const& translation)
      : rxso3_(quaternion), translation_(translation) {
    static_assert(std::is_same<typename D1::Scalar, Scalar>::value,
                  "must be same Scalar type");
    static_assert(std::is_same<typename D2::Scalar, Scalar>::value,
                  "must be same Scalar type");
  }
 
  SOPHUS_FUNC explicit Sim3(Matrix<Scalar, 4, 4> const& T)
      : rxso3_(T.template topLeftCorner<3, 3>()),
        translation_(T.template block<3, 1>(0, 3)) {}
 
  SOPHUS_FUNC Scalar* data() {
    // rxso3_ and translation_ are laid out sequentially with no padding
    return rxso3_.data();
  }
 
  SOPHUS_FUNC Scalar const* data() const {
    // rxso3_ and translation_ are laid out sequentially with no padding
    return rxso3_.data();
  }
 
  SOPHUS_FUNC RxSo3Member& rxso3() { return rxso3_; }
 
  SOPHUS_FUNC RxSo3Member const& rxso3() const { return rxso3_; }
 
  SOPHUS_FUNC TranslationMember& translation() { return translation_; }
 
  SOPHUS_FUNC TranslationMember const& translation() const {
    return translation_;
  }
 
  SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) {
    return generator(i);
  }
 
  SOPHUS_FUNC static Sim3<Scalar> exp(Tangent const& a) {
    // For the derivation of the exponential map of Sim(3) see
    // H. Strasdat, "Local Accuracy and Global Consistency for Efficient Visual
    // SLAM", PhD thesis, 2012.
    // http:///hauke.strasdat.net/files/strasdat_thesis_2012.pdf (A.5, pp. 186)
    Vector3<Scalar> const upsilon = a.segment(0, 3);
    Vector3<Scalar> const omega = a.segment(3, 3);
    Scalar const sigma = a[6];
    Scalar theta;
    RxSO3<Scalar> rxso3 =
        RxSO3<Scalar>::expAndTheta(a.template tail<4>(), &theta);
    Matrix3<Scalar> const Omega = SO3<Scalar>::hat(omega);
 
    Matrix3<Scalar> const W = details::calcW<Scalar, 3>(Omega, theta, sigma);
    return Sim3<Scalar>(rxso3, W * upsilon);
  }
 
  SOPHUS_FUNC static Transformation generator(int i) {
    SOPHUS_ENSURE(i >= 0 || i <= 6, "i should be in range [0,6].");
    Tangent e;
    e.setZero();
    e[i] = Scalar(1);
    return hat(e);
  }
 
  SOPHUS_FUNC static Transformation hat(Tangent const& a) {
    Transformation Omega;
    Omega.template topLeftCorner<3, 3>() =
        RxSO3<Scalar>::hat(a.template tail<4>());
    Omega.col(3).template head<3>() = a.template head<3>();
    Omega.row(3).setZero();
    return Omega;
  }
 
  SOPHUS_FUNC static Tangent lieBracket(Tangent const& a, Tangent const& b) {
    Vector3<Scalar> const upsilon1 = a.template head<3>();
    Vector3<Scalar> const upsilon2 = b.template head<3>();
    Vector3<Scalar> const omega1 = a.template segment<3>(3);
    Vector3<Scalar> const omega2 = b.template segment<3>(3);
    Scalar sigma1 = a[6];
    Scalar sigma2 = b[6];
 
    Tangent res;
    res.template head<3>() = SO3<Scalar>::hat(omega1) * upsilon2 +
                             SO3<Scalar>::hat(upsilon1) * omega2 +
                             sigma1 * upsilon2 - sigma2 * upsilon1;
    res.template segment<3>(3) = omega1.cross(omega2);
    res[6] = Scalar(0);
 
    return res;
  }
 
  template <class UniformRandomBitGenerator>
  static Sim3 sampleUniform(UniformRandomBitGenerator& generator) {
    std::uniform_real_distribution<Scalar> uniform(Scalar(-1), Scalar(1));
    return Sim3(RxSO3<Scalar>::sampleUniform(generator),
                Vector3<Scalar>(uniform(generator), uniform(generator),
                                uniform(generator)));
  }
 
  SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
    Tangent upsilon_omega_sigma;
    upsilon_omega_sigma.template head<3>() = Omega.col(3).template head<3>();
    upsilon_omega_sigma.template tail<4>() =
        RxSO3<Scalar>::vee(Omega.template topLeftCorner<3, 3>());
    return upsilon_omega_sigma;
  }
 
 protected:
  RxSo3Member rxso3_;
  TranslationMember translation_;
};
 
template <class Scalar, int Options>
Sim3<Scalar, Options>::Sim3() : translation_(TranslationMember::Zero()) {
  static_assert(std::is_standard_layout<Sim3>::value,
                "Assume standard layout for the use of offsetof check below.");
  static_assert(
      offsetof(Sim3, rxso3_) + sizeof(Scalar) * RxSO3<Scalar>::num_parameters ==
          offsetof(Sim3, translation_),
      "This class assumes packed storage and hence will only work "
      "correctly depending on the compiler (options) - in "
      "particular when using [this->data(), this-data() + "
      "num_parameters] to access the raw data in a contiguous fashion.");
}
 
}  // namespace Sophus
 
namespace Eigen {
 
template <class Scalar_, int Options>
class Map<Sophus::Sim3<Scalar_>, Options>
    : public Sophus::Sim3Base<Map<Sophus::Sim3<Scalar_>, Options>> {
 public:
  using Base = Sophus::Sim3Base<Map<Sophus::Sim3<Scalar_>, Options>>;
  using Scalar = Scalar_;
  using Transformation = typename Base::Transformation;
  using Point = typename Base::Point;
  using HomogeneousPoint = typename Base::HomogeneousPoint;
  using Tangent = typename Base::Tangent;
  using Adjoint = typename Base::Adjoint;
 
  // LCOV_EXCL_START
  EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map);
  // LCOV_EXCL_STOP
 
  using Base::operator*=;
  using Base::operator*;
 
  SOPHUS_FUNC Map(Scalar* coeffs)
      : rxso3_(coeffs),
        translation_(coeffs + Sophus::RxSO3<Scalar>::num_parameters) {}
 
  SOPHUS_FUNC Map<Sophus::RxSO3<Scalar>, Options>& rxso3() { return rxso3_; }
 
  SOPHUS_FUNC Map<Sophus::RxSO3<Scalar>, Options> const& rxso3() const {
    return rxso3_;
  }
 
  SOPHUS_FUNC Map<Sophus::Vector3<Scalar>, Options>& translation() {
    return translation_;
  }
 
  SOPHUS_FUNC Map<Sophus::Vector3<Scalar>, Options> const& translation() const {
    return translation_;
  }
 
 protected:
  Map<Sophus::RxSO3<Scalar>, Options> rxso3_;
  Map<Sophus::Vector3<Scalar>, Options> translation_;
};
 
template <class Scalar_, int Options>
class Map<Sophus::Sim3<Scalar_> const, Options>
    : public Sophus::Sim3Base<Map<Sophus::Sim3<Scalar_> const, Options>> {
  using Base = Sophus::Sim3Base<Map<Sophus::Sim3<Scalar_> const, Options>>;
 
 public:
  using Scalar = Scalar_;
  using Transformation = typename Base::Transformation;
  using Point = typename Base::Point;
  using HomogeneousPoint = typename Base::HomogeneousPoint;
  using Tangent = typename Base::Tangent;
  using Adjoint = typename Base::Adjoint;
 
  using Base::operator*=;
  using Base::operator*;
 
  SOPHUS_FUNC Map(Scalar const* coeffs)
      : rxso3_(coeffs),
        translation_(coeffs + Sophus::RxSO3<Scalar>::num_parameters) {}
 
  SOPHUS_FUNC Map<Sophus::RxSO3<Scalar> const, Options> const& rxso3() const {
    return rxso3_;
  }
 
  SOPHUS_FUNC Map<Sophus::Vector3<Scalar> const, Options> const& translation()
      const {
    return translation_;
  }
 
 protected:
  Map<Sophus::RxSO3<Scalar> const, Options> const rxso3_;
  Map<Sophus::Vector3<Scalar> const, Options> const translation_;
};
}  // namespace Eigen
 
#endif