Program Listing for File sim2.hpp

↰ Return to documentation for file (sophus/sim2.hpp)

#pragma once

#include "rxso2.hpp"
#include "sim_details.hpp"

namespace Sophus {
template <class Scalar_, int Options = 0>
class Sim2;
using Sim2d = Sim2<double>;
using Sim2f = Sim2<float>;
}  // namespace Sophus

namespace Eigen {
namespace internal {

template <class Scalar_, int Options>
struct traits<Sophus::Sim2<Scalar_, Options>> {
  using Scalar = Scalar_;
  using TranslationType = Sophus::Vector2<Scalar, Options>;
  using RxSO2Type = Sophus::RxSO2<Scalar, Options>;
};

template <class Scalar_, int Options>
struct traits<Map<Sophus::Sim2<Scalar_>, Options>>
    : traits<Sophus::Sim2<Scalar_, Options>> {
  using Scalar = Scalar_;
  using TranslationType = Map<Sophus::Vector2<Scalar>, Options>;
  using RxSO2Type = Map<Sophus::RxSO2<Scalar>, Options>;
};

template <class Scalar_, int Options>
struct traits<Map<Sophus::Sim2<Scalar_> const, Options>>
    : traits<Sophus::Sim2<Scalar_, Options> const> {
  using Scalar = Scalar_;
  using TranslationType = Map<Sophus::Vector2<Scalar> const, Options>;
  using RxSO2Type = Map<Sophus::RxSO2<Scalar> const, Options>;
};
}  // namespace internal
}  // namespace Eigen

namespace Sophus {

template <class Derived>
class Sim2Base {
 public:
  using Scalar = typename Eigen::internal::traits<Derived>::Scalar;
  using TranslationType =
      typename Eigen::internal::traits<Derived>::TranslationType;
  using RxSO2Type = typename Eigen::internal::traits<Derived>::RxSO2Type;

  static int constexpr DoF = 4;
  static int constexpr num_parameters = 4;
  static int constexpr N = 3;
  static int constexpr Dim = 2;
  using Transformation = Matrix<Scalar, N, N>;
  using Point = Vector2<Scalar>;
  using HomogeneousPoint = Vector3<Scalar>;
  using Line = ParametrizedLine2<Scalar>;
  using Hyperplane = Hyperplane2<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 Sim2Product = Sim2<ReturnScalar<OtherDerived>>;

  template <typename PointDerived>
  using PointProduct = Vector2<ReturnScalar<PointDerived>>;

  template <typename HPointDerived>
  using HomogeneousPointProduct = Vector3<ReturnScalar<HPointDerived>>;

  SOPHUS_FUNC Adjoint Adj() const {
    Adjoint res;
    res.setZero();
    res.template block<2, 2>(0, 0) = rxso2().matrix();
    res(0, 2) = translation()[1];
    res(1, 2) = -translation()[0];
    res.template block<2, 1>(0, 3) = -translation();

    res(2, 2) = Scalar(1);

    res(3, 3) = Scalar(1);
    return res;
  }

  template <class NewScalarType>
  SOPHUS_FUNC Sim2<NewScalarType> cast() const {
    return Sim2<NewScalarType>(rxso2().template cast<NewScalarType>(),
                               translation().template cast<NewScalarType>());
  }

  SOPHUS_FUNC Sim2<Scalar> inverse() const {
    RxSO2<Scalar> invR = rxso2().inverse();
    return Sim2<Scalar>(invR, invR * (translation() * Scalar(-1)));
  }

  SOPHUS_FUNC Tangent log() const {
    Tangent res;
    Vector2<Scalar> const theta_sigma = rxso2().log();
    Scalar const theta = theta_sigma[0];
    Scalar const sigma = theta_sigma[1];
    Matrix2<Scalar> const Omega = SO2<Scalar>::hat(theta);
    Matrix2<Scalar> const W_inv =
        details::calcWInv<Scalar, 2>(Omega, theta, sigma, scale());

    res.segment(0, 2) = W_inv * translation();
    res[2] = theta;
    res[3] = sigma;
    return res;
  }

  SOPHUS_FUNC Transformation matrix() const {
    Transformation homogeneous_matrix;
    homogeneous_matrix.template topLeftCorner<2, 3>() = matrix2x3();
    homogeneous_matrix.row(2) =
        Matrix<Scalar, 3, 1>(Scalar(0), Scalar(0), Scalar(1));
    return homogeneous_matrix;
  }

  SOPHUS_FUNC Matrix<Scalar, 2, 3> matrix2x3() const {
    Matrix<Scalar, 2, 3> matrix;
    matrix.template topLeftCorner<2, 2>() = rxso2().matrix();
    matrix.col(2) = translation();
    return matrix;
  }

  template <class OtherDerived>
  SOPHUS_FUNC Sim2Base<Derived>& operator=(
      Sim2Base<OtherDerived> const& other) {
    rxso2() = other.rxso2();
    translation() = other.translation();
    return *this;
  }

  template <typename OtherDerived>
  SOPHUS_FUNC Sim2Product<OtherDerived> operator*(
      Sim2Base<OtherDerived> const& other) const {
    return Sim2Product<OtherDerived>(
        rxso2() * other.rxso2(), translation() + rxso2() * other.translation());
  }

  template <typename PointDerived,
            typename = typename std::enable_if<
                IsFixedSizeVector<PointDerived, 2>::value>::type>
  SOPHUS_FUNC PointProduct<PointDerived> operator*(
      Eigen::MatrixBase<PointDerived> const& p) const {
    return rxso2() * p + translation();
  }

  template <typename HPointDerived,
            typename = typename std::enable_if<
                IsFixedSizeVector<HPointDerived, 3>::value>::type>
  SOPHUS_FUNC HomogeneousPointProduct<HPointDerived> operator*(
      Eigen::MatrixBase<HPointDerived> const& p) const {
    const PointProduct<HPointDerived> tp =
        rxso2() * p.template head<2>() + p(2) * translation();
    return HomogeneousPointProduct<HPointDerived>(tp(0), tp(1), p(2));
  }

  SOPHUS_FUNC Line operator*(Line const& l) const {
    Line rotatedLine = rxso2() * l;
    return Line(rotatedLine.origin() + translation(), rotatedLine.direction());
  }

  SOPHUS_FUNC Hyperplane operator*(Hyperplane const& p) const {
    Hyperplane const rotated = rxso2() * p;
    return Hyperplane(rotated.normal(),
                      rotated.offset() - translation().dot(rotated.normal()));
  }

  SOPHUS_FUNC Sophus::Vector<Scalar, num_parameters> params() const {
    Sophus::Vector<Scalar, num_parameters> p;
    p << rxso2().params(), translation();
    return p;
  }

  template <typename OtherDerived,
            typename = typename std::enable_if<
                std::is_same<Scalar, ReturnScalar<OtherDerived>>::value>::type>
  SOPHUS_FUNC Sim2Base<Derived>& operator*=(
      Sim2Base<OtherDerived> const& other) {
    *static_cast<Derived*>(this) = *this * other;
    return *this;
  }

  SOPHUS_FUNC Matrix<Scalar, num_parameters, DoF> Dx_this_mul_exp_x_at_0()
      const {
    Matrix<Scalar, num_parameters, DoF> J;
    J.template block<2, 2>(0, 0).setZero();
    J.template block<2, 2>(0, 2) = rxso2().Dx_this_mul_exp_x_at_0();
    J.template block<2, 2>(2, 2).setZero();
    J.template block<2, 2>(2, 0) = rxso2().matrix();
    return J;
  }

  SOPHUS_FUNC Matrix<Scalar, DoF, num_parameters> Dx_log_this_inv_by_x_at_this()
      const {
    Matrix<Scalar, num_parameters, DoF> J;
    J.template block<2, 2>(0, 0).setZero();
    J.template block<2, 2>(0, 2) = rxso2().inverse().matrix();
    J.template block<2, 2>(2, 0) = rxso2().Dx_log_this_inv_by_x_at_this();
    J.template block<2, 2>(2, 2).setZero();
    return J;
  }

  SOPHUS_FUNC void setComplex(Vector2<Scalar> const& z) {
    rxso2().setComplex(z);
  }

  SOPHUS_FUNC
  typename Eigen::internal::traits<Derived>::RxSO2Type::ComplexType const&
  complex() const {
    return rxso2().complex();
  }

  SOPHUS_FUNC Matrix2<Scalar> rotationMatrix() const {
    return rxso2().rotationMatrix();
  }

  SOPHUS_FUNC RxSO2Type& rxso2() {
    return static_cast<Derived*>(this)->rxso2();
  }

  SOPHUS_FUNC RxSO2Type const& rxso2() const {
    return static_cast<Derived const*>(this)->rxso2();
  }

  SOPHUS_FUNC Scalar scale() const { return rxso2().scale(); }

  SOPHUS_FUNC void setRotationMatrix(Matrix2<Scalar>& R) {
    rxso2().setRotationMatrix(R);
  }

  SOPHUS_FUNC void setScale(Scalar const& scale) { rxso2().setScale(scale); }

  SOPHUS_FUNC void setScaledRotationMatrix(Matrix2<Scalar> const& sR) {
    rxso2().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 Sim2 : public Sim2Base<Sim2<Scalar_, Options>> {
 public:
  using Base = Sim2Base<Sim2<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 RxSo2Member = RxSO2<Scalar, Options>;
  using TranslationMember = Vector2<Scalar, Options>;

  using Base::operator=;

  SOPHUS_FUNC Sim2& operator=(Sim2 const& other) = default;

  static int constexpr DoF = Base::DoF;
  static int constexpr num_parameters = Base::num_parameters;

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  SOPHUS_FUNC Sim2();

  SOPHUS_FUNC Sim2(Sim2 const& other) = default;

  template <class OtherDerived>
  SOPHUS_FUNC Sim2(Sim2Base<OtherDerived> const& other)
      : rxso2_(other.rxso2()), 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 Sim2(RxSO2Base<OtherDerived> const& rxso2,
                   Eigen::MatrixBase<D> const& translation)
      : rxso2_(rxso2), 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 D>
  SOPHUS_FUNC Sim2(Vector2<Scalar> const& complex_number,
                   Eigen::MatrixBase<D> const& translation)
      : rxso2_(complex_number), translation_(translation) {
    static_assert(std::is_same<typename D::Scalar, Scalar>::value,
                  "must be same Scalar type");
  }

  SOPHUS_FUNC explicit Sim2(Matrix<Scalar, 3, 3> const& T)
      : rxso2_((T.template topLeftCorner<2, 2>()).eval()),
        translation_(T.template block<2, 1>(0, 2)) {}

  SOPHUS_FUNC Scalar* data() {
    // rxso2_ and translation_ are laid out sequentially with no padding
    return rxso2_.data();
  }

  SOPHUS_FUNC Scalar const* data() const {
    // rxso2_ and translation_ are laid out sequentially with no padding
    return rxso2_.data();
  }

  SOPHUS_FUNC RxSo2Member& rxso2() { return rxso2_; }

  SOPHUS_FUNC RxSo2Member const& rxso2() const { return rxso2_; }

  SOPHUS_FUNC TranslationMember& translation() { return translation_; }

  SOPHUS_FUNC TranslationMember const& translation() const {
    return translation_;
  }

  SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF>
  Dx_exp_x_at_0() {
    Sophus::Matrix<Scalar, num_parameters, DoF> J;
    J.template block<2, 2>(0, 0).setZero();
    J.template block<2, 2>(0, 2) = RxSO2<Scalar>::Dx_exp_x_at_0();
    J.template block<2, 2>(2, 0).setIdentity();
    J.template block<2, 2>(2, 2).setZero();
    return J;
  }

  SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF> Dx_exp_x(
      const Tangent& a) {
    static Matrix2<Scalar> const I = Matrix2<Scalar>::Identity();
    static Scalar const one(1.0);

    Scalar const theta = a[2];
    Scalar const sigma = a[3];

    Matrix2<Scalar> const Omega = SO2<Scalar>::hat(theta);
    Matrix2<Scalar> const Omega_dtheta = SO2<Scalar>::hat(one);
    Matrix2<Scalar> const Omega2 = Omega * Omega;
    Matrix2<Scalar> const Omega2_dtheta =
        Omega_dtheta * Omega + Omega * Omega_dtheta;
    Matrix2<Scalar> const W = details::calcW<Scalar, 2>(Omega, theta, sigma);
    Vector2<Scalar> const upsilon = a.segment(0, 2);

    Sophus::Matrix<Scalar, num_parameters, DoF> J;
    J.template block<2, 2>(0, 0).setZero();
    J.template block<2, 2>(0, 2) =
        RxSO2<Scalar>::Dx_exp_x(a.template tail<2>());
    J.template block<2, 2>(2, 0) = W;

    Scalar A, B, C, A_dtheta, B_dtheta, A_dsigma, B_dsigma, C_dsigma;
    details::calcW_derivatives(theta, sigma, A, B, C, A_dsigma, B_dsigma,
                               C_dsigma, A_dtheta, B_dtheta);

    J.template block<2, 1>(2, 2) = (A_dtheta * Omega + A * Omega_dtheta +
                                    B_dtheta * Omega2 + B * Omega2_dtheta) *
                                   upsilon;
    J.template block<2, 1>(2, 3) =
        (A_dsigma * Omega + B_dsigma * Omega2 + C_dsigma * I) * upsilon;

    return J;
  }

  SOPHUS_FUNC static Sophus::Matrix<Scalar, 2, DoF> Dx_exp_x_times_point_at_0(
      Point const& point) {
    Sophus::Matrix<Scalar, 2, DoF> J;
    J << Sophus::Matrix2<Scalar>::Identity(),
        Sophus::RxSO2<Scalar>::Dx_exp_x_times_point_at_0(point);
    return J;
  }

  SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) {
    return generator(i);
  }


  SOPHUS_FUNC static Sim2<Scalar> exp(Tangent const& a) {
    // For the derivation of the exponential map of Sim(N) 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)
    Vector2<Scalar> const upsilon = a.segment(0, 2);
    Scalar const theta = a[2];
    Scalar const sigma = a[3];
    RxSO2<Scalar> rxso2 = RxSO2<Scalar>::exp(a.template tail<2>());
    Matrix2<Scalar> const Omega = SO2<Scalar>::hat(theta);
    Matrix2<Scalar> const W = details::calcW<Scalar, 2>(Omega, theta, sigma);
    return Sim2<Scalar>(rxso2, W * upsilon);
  }

  SOPHUS_FUNC static Transformation generator(int i) {
    SOPHUS_ENSURE(i >= 0 || i <= 3, "i should be in range [0,3].");
    Tangent e;
    e.setZero();
    e[i] = Scalar(1);
    return hat(e);
  }

  SOPHUS_FUNC static Transformation hat(Tangent const& a) {
    Transformation Omega;
    Omega.template topLeftCorner<2, 2>() =
        RxSO2<Scalar>::hat(a.template tail<2>());
    Omega.col(2).template head<2>() = a.template head<2>();
    Omega.row(2).setZero();
    return Omega;
  }

  SOPHUS_FUNC static Tangent lieBracket(Tangent const& a, Tangent const& b) {
    Vector2<Scalar> const upsilon1 = a.template head<2>();
    Vector2<Scalar> const upsilon2 = b.template head<2>();
    Scalar const theta1 = a[2];
    Scalar const theta2 = b[2];
    Scalar const sigma1 = a[3];
    Scalar const sigma2 = b[3];

    Tangent res;
    res[0] = -theta1 * upsilon2[1] + theta2 * upsilon1[1] +
             sigma1 * upsilon2[0] - sigma2 * upsilon1[0];
    res[1] = theta1 * upsilon2[0] - theta2 * upsilon1[0] +
             sigma1 * upsilon2[1] - sigma2 * upsilon1[1];
    res[2] = Scalar(0);
    res[3] = Scalar(0);

    return res;
  }

  template <class UniformRandomBitGenerator>
  static Sim2 sampleUniform(UniformRandomBitGenerator& generator) {
    std::uniform_real_distribution<Scalar> uniform(Scalar(-1), Scalar(1));
    return Sim2(RxSO2<Scalar>::sampleUniform(generator),
                Vector2<Scalar>(uniform(generator), uniform(generator)));
  }

  SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
    Tangent upsilon_omega_sigma;
    upsilon_omega_sigma.template head<2>() = Omega.col(2).template head<2>();
    upsilon_omega_sigma.template tail<2>() =
        RxSO2<Scalar>::vee(Omega.template topLeftCorner<2, 2>());
    return upsilon_omega_sigma;
  }

 protected:
  RxSo2Member rxso2_;
  TranslationMember translation_;
};

template <class Scalar, int Options>
SOPHUS_FUNC Sim2<Scalar, Options>::Sim2()
    : translation_(TranslationMember::Zero()) {
  static_assert(std::is_standard_layout<Sim2>::value,
                "Assume standard layout for the use of offset of check below.");
  static_assert(
      offsetof(Sim2, rxso2_) + sizeof(Scalar) * RxSO2<Scalar>::num_parameters ==
          offsetof(Sim2, 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::Sim2<Scalar_>, Options>
    : public Sophus::Sim2Base<Map<Sophus::Sim2<Scalar_>, Options>> {
 public:
  using Base = Sophus::Sim2Base<Map<Sophus::Sim2<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 Base::operator=;
  using Base::operator*=;
  using Base::operator*;

  SOPHUS_FUNC explicit Map(Scalar* coeffs)
      : rxso2_(coeffs),
        translation_(coeffs + Sophus::RxSO2<Scalar>::num_parameters) {}

  SOPHUS_FUNC Map<Sophus::RxSO2<Scalar>, Options>& rxso2() { return rxso2_; }

  SOPHUS_FUNC Map<Sophus::RxSO2<Scalar>, Options> const& rxso2() const {
    return rxso2_;
  }

  SOPHUS_FUNC Map<Sophus::Vector2<Scalar>, Options>& translation() {
    return translation_;
  }

  SOPHUS_FUNC Map<Sophus::Vector2<Scalar>, Options> const& translation() const {
    return translation_;
  }

 protected:
  Map<Sophus::RxSO2<Scalar>, Options> rxso2_;
  Map<Sophus::Vector2<Scalar>, Options> translation_;
};

template <class Scalar_, int Options>
class Map<Sophus::Sim2<Scalar_> const, Options>
    : public Sophus::Sim2Base<Map<Sophus::Sim2<Scalar_> const, Options>> {
 public:
  using Base = Sophus::Sim2Base<Map<Sophus::Sim2<Scalar_> const, 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 Base::operator*=;
  using Base::operator*;

  SOPHUS_FUNC explicit Map(Scalar const* coeffs)
      : rxso2_(coeffs),
        translation_(coeffs + Sophus::RxSO2<Scalar>::num_parameters) {}

  SOPHUS_FUNC Map<Sophus::RxSO2<Scalar> const, Options> const& rxso2() const {
    return rxso2_;
  }

  SOPHUS_FUNC Map<Sophus::Vector2<Scalar> const, Options> const& translation()
      const {
    return translation_;
  }

 protected:
  Map<Sophus::RxSO2<Scalar> const, Options> const rxso2_;
  Map<Sophus::Vector2<Scalar> const, Options> const translation_;
};
}  // namespace Eigen