Program Listing for File rxso2.hpp

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

#pragma once

#include "so2.hpp"

namespace Sophus {
template <class Scalar_, int Options = 0>
class RxSO2;
using RxSO2d = RxSO2<double>;
using RxSO2f = RxSO2<float>;
}  // namespace Sophus

namespace Eigen {
namespace internal {

template <class Scalar_, int Options_>
struct traits<Sophus::RxSO2<Scalar_, Options_>> {
  static constexpr int Options = Options_;
  using Scalar = Scalar_;
  using ComplexType = Sophus::Vector2<Scalar, Options>;
};

template <class Scalar_, int Options_>
struct traits<Map<Sophus::RxSO2<Scalar_>, Options_>>
    : traits<Sophus::RxSO2<Scalar_, Options_>> {
  static constexpr int Options = Options_;
  using Scalar = Scalar_;
  using ComplexType = Map<Sophus::Vector2<Scalar>, Options>;
};

template <class Scalar_, int Options_>
struct traits<Map<Sophus::RxSO2<Scalar_> const, Options_>>
    : traits<Sophus::RxSO2<Scalar_, Options_> const> {
  static constexpr int Options = Options_;
  using Scalar = Scalar_;
  using ComplexType = Map<Sophus::Vector2<Scalar> const, Options>;
};
}  // namespace internal
}  // namespace Eigen

namespace Sophus {

template <class Derived>
class RxSO2Base {
 public:
  static constexpr int Options = Eigen::internal::traits<Derived>::Options;
  using Scalar = typename Eigen::internal::traits<Derived>::Scalar;
  using ComplexType = typename Eigen::internal::traits<Derived>::ComplexType;
  using ComplexTemporaryType = Sophus::Vector2<Scalar, Options>;

  static int constexpr DoF = 2;
  static int constexpr num_parameters = 2;
  static int constexpr N = 2;
  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 RxSO2Product = RxSO2<ReturnScalar<OtherDerived>>;

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

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

  SOPHUS_FUNC Adjoint Adj() const { return Adjoint::Identity(); }

  SOPHUS_FUNC Scalar angle() const { return SO2<Scalar>(complex()).log(); }

  template <class NewScalarType>
  SOPHUS_FUNC RxSO2<NewScalarType> cast() const {
    typename RxSO2<NewScalarType>::ComplexType c =
        complex().template cast<NewScalarType>();
    return RxSO2<NewScalarType>(c);
  }

  SOPHUS_FUNC Scalar* data() { return complex_nonconst().data(); }

  SOPHUS_FUNC Scalar const* data() const { return complex().data(); }

  SOPHUS_FUNC RxSO2<Scalar> inverse() const {
    Scalar squared_scale = complex().squaredNorm();
    Vector2<Scalar> xy = complex() / squared_scale;
    return RxSO2<Scalar>(xy.x(), -xy.y());
  }

  SOPHUS_FUNC Tangent log() const {
    using std::log;
    Tangent theta_sigma;
    theta_sigma[1] = log(scale());
    theta_sigma[0] = SO2<Scalar>(complex()).log();
    return theta_sigma;
  }

  SOPHUS_FUNC Transformation matrix() const {
    Transformation sR;
    // clang-format off
    sR << complex()[0], -complex()[1],
          complex()[1],  complex()[0];
    // clang-format on
    return sR;
  }

  template <class OtherDerived>
  SOPHUS_FUNC RxSO2Base<Derived>& operator=(
      RxSO2Base<OtherDerived> const& other) {
    complex_nonconst() = other.complex();
    return *this;
  }

  template <typename OtherDerived>
  SOPHUS_FUNC RxSO2Product<OtherDerived> operator*(
      RxSO2Base<OtherDerived> const& other) const {
    using ResultT = ReturnScalar<OtherDerived>;

    Scalar lhs_real = complex().x();
    Scalar lhs_imag = complex().y();
    typename OtherDerived::Scalar const& rhs_real = other.complex().x();
    typename OtherDerived::Scalar const& rhs_imag = other.complex().y();
    typename RxSO2Product<OtherDerived>::ComplexType result_complex(
        lhs_real * rhs_real - lhs_imag * rhs_imag,
        lhs_real * rhs_imag + lhs_imag * rhs_real);

    const ResultT squared_scale = result_complex.squaredNorm();

    if (squared_scale <
        Constants<ResultT>::epsilon() * Constants<ResultT>::epsilon()) {
      result_complex.normalize();
      result_complex *= Constants<ResultT>::epsilonPlus();
    }
    if (squared_scale > Scalar(1.) / (Constants<ResultT>::epsilon() *
                                      Constants<ResultT>::epsilon())) {
      result_complex.normalize();
      result_complex /= Constants<ResultT>::epsilonPlus();
    }
    return RxSO2Product<OtherDerived>(result_complex);
  }

  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 matrix() * p;
  }

  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 auto rsp = *this * p.template head<2>();
    return HomogeneousPointProduct<HPointDerived>(rsp(0), rsp(1), p(2));
  }

  SOPHUS_FUNC Line operator*(Line const& l) const {
    return Line((*this) * l.origin(), (*this) * l.direction() / scale());
  }

  SOPHUS_FUNC Hyperplane operator*(Hyperplane const& p) const {
    const auto this_scale = scale();
    return Hyperplane((*this) * p.normal() / this_scale,
                      this_scale * p.offset());
  }

  template <typename OtherDerived,
            typename = typename std::enable_if<
                std::is_same<Scalar, ReturnScalar<OtherDerived>>::value>::type>
  SOPHUS_FUNC RxSO2Base<Derived>& operator*=(
      RxSO2Base<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 << -complex().y(), complex().x(), complex().x(), complex().y();
    return J;
  }

  SOPHUS_FUNC Matrix<Scalar, DoF, num_parameters> Dx_log_this_inv_by_x_at_this()
      const {
    Matrix<Scalar, DoF, num_parameters> J;
    const Scalar norm_sq_inv = Scalar(1.) / complex().squaredNorm();
    J << -complex().y(), complex().x(), complex().x(), complex().y();
    return J * norm_sq_inv;
  }

  SOPHUS_FUNC Sophus::Vector<Scalar, num_parameters> params() const {
    return complex();
  }

  SOPHUS_FUNC void setComplex(Vector2<Scalar> const& z) {
    SOPHUS_ENSURE(z.squaredNorm() > Constants<Scalar>::epsilon() *
                                        Constants<Scalar>::epsilon(),
                  "Scale factor must be greater-equal epsilon.");
    SOPHUS_ENSURE(z.squaredNorm() < Scalar(1.) / (Constants<Scalar>::epsilon() *
                                                  Constants<Scalar>::epsilon()),
                  "Inverse scale factor must be greater-equal epsilon.");
    static_cast<Derived*>(this)->complex_nonconst() = z;
  }

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

  SOPHUS_FUNC Transformation rotationMatrix() const {
    ComplexTemporaryType norm_quad = complex();
    norm_quad.normalize();
    return SO2<Scalar>(norm_quad).matrix();
  }

  SOPHUS_FUNC Scalar scale() const {
    using std::hypot;
    return hypot(complex().x(), complex().y());
  }

  SOPHUS_FUNC void setAngle(Scalar const& theta) { setSO2(SO2<Scalar>(theta)); }

  SOPHUS_FUNC void setRotationMatrix(Transformation const& R) {
    setSO2(SO2<Scalar>(R));
  }

  SOPHUS_FUNC void setScale(Scalar const& scale) {
    using std::sqrt;
    complex_nonconst().normalize();
    complex_nonconst() *= scale;
  }

  SOPHUS_FUNC void setScaledRotationMatrix(Transformation const& sR) {
    SOPHUS_ENSURE(isScaledOrthogonalAndPositive(sR),
                  "sR must be scaled orthogonal:\n {}", SOPHUS_FMT_ARG(sR));
    complex_nonconst() = sR.col(0);
  }

  SOPHUS_FUNC void setSO2(SO2<Scalar> const& so2) {
    using std::sqrt;
    Scalar saved_scale = scale();
    complex_nonconst() = so2.unit_complex();
    complex_nonconst() *= saved_scale;
  }

  SOPHUS_FUNC SO2<Scalar> so2() const { return SO2<Scalar>(complex()); }

 private:
  SOPHUS_FUNC ComplexType& complex_nonconst() {
    return static_cast<Derived*>(this)->complex_nonconst();
  }
};

template <class Scalar_, int Options>
class RxSO2 : public RxSO2Base<RxSO2<Scalar_, Options>> {
 public:
  using Base = RxSO2Base<RxSO2<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 ComplexMember = Eigen::Matrix<Scalar, 2, 1, Options>;

  friend class RxSO2Base<RxSO2<Scalar_, Options>>;

  using Base::operator=;

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

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

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  SOPHUS_FUNC RxSO2() : complex_(Scalar(1), Scalar(0)) {}

  SOPHUS_FUNC RxSO2(RxSO2 const& other) = default;

  template <class OtherDerived>
  SOPHUS_FUNC RxSO2(RxSO2Base<OtherDerived> const& other)
      : complex_(other.complex()) {}

  SOPHUS_FUNC explicit RxSO2(Transformation const& sR) {
    this->setScaledRotationMatrix(sR);
  }

  SOPHUS_FUNC RxSO2(Scalar const& scale, Transformation const& R)
      : RxSO2((scale * SO2<Scalar>(R).unit_complex()).eval()) {}

  SOPHUS_FUNC RxSO2(Scalar const& scale, SO2<Scalar> const& so2)
      : RxSO2((scale * so2.unit_complex()).eval()) {}

  SOPHUS_FUNC explicit RxSO2(Vector2<Scalar> const& z) : complex_(z) {
    SOPHUS_ENSURE(complex_.squaredNorm() >= Constants<Scalar>::epsilon() *
                                                Constants<Scalar>::epsilon(),
                  "Scale factor must be greater-equal epsilon: {} vs {}",
                  SOPHUS_FMT_ARG(complex_.squaredNorm()),
                  SOPHUS_FMT_ARG(Constants<Scalar>::epsilon() *
                                 Constants<Scalar>::epsilon()));
    SOPHUS_ENSURE(
        complex_.squaredNorm() <= Scalar(1.) / (Constants<Scalar>::epsilon() *
                                                Constants<Scalar>::epsilon()),
        "Inverse scale factor must be greater-equal epsilon: {} vs {}",
        SOPHUS_FMT_ARG(Scalar(1.) / complex_.squaredNorm()),
        SOPHUS_FMT_ARG(Constants<Scalar>::epsilon() *
                       Constants<Scalar>::epsilon()));
  }

  SOPHUS_FUNC explicit RxSO2(Scalar const& real, Scalar const& imag)
      : RxSO2(Vector2<Scalar>(real, imag)) {}

  SOPHUS_FUNC ComplexMember const& complex() const { return complex_; }

  SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF> Dx_exp_x(
      Tangent const& a) {
    using std::cos;
    using std::exp;
    using std::sin;
    Scalar const theta = a[0];
    Scalar const sigma = a[1];

    Sophus::Matrix<Scalar, num_parameters, DoF> J;
    J << -sin(theta), cos(theta), cos(theta), sin(theta);
    return J * exp(sigma);
  }

  SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF>
  Dx_exp_x_at_0() {
    Sophus::Matrix<Scalar, num_parameters, DoF> J;
    static Scalar const i(1.);
    static Scalar const o(0.);
    J << o, i, i, o;
    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::SO2<Scalar>::Dx_exp_x_times_point_at_0(point), point;
    return j;
  }

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

  SOPHUS_FUNC static RxSO2<Scalar> exp(Tangent const& a) {
    using std::exp;
    using std::max;
    using std::min;

    Scalar const theta = a[0];
    Scalar const sigma = a[1];
    Scalar s = exp(sigma);
    // Ensuring proper scale
    s = max(s, Constants<Scalar>::epsilonPlus());
    s = min(s, Scalar(1.) / Constants<Scalar>::epsilonPlus());
    Vector2<Scalar> z = SO2<Scalar>::exp(theta).unit_complex();
    z *= s;
    return RxSO2<Scalar>(z);
  }

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

  SOPHUS_FUNC static Transformation hat(Tangent const& a) {
    Transformation A;
    // clang-format off
    A << a(1), -a(0),
         a(0),  a(1);
    // clang-format on
    return A;
  }

  SOPHUS_FUNC static Tangent lieBracket(Tangent const&, Tangent const&) {
    Vector2<Scalar> res;
    res.setZero();
    return res;
  }

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

  SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
    using std::abs;
    return Tangent(Omega(1, 0), Omega(0, 0));
  }

 protected:
  SOPHUS_FUNC ComplexMember& complex_nonconst() { return complex_; }

  ComplexMember complex_;
};

}  // namespace Sophus

namespace Eigen {

template <class Scalar_, int Options>
class Map<Sophus::RxSO2<Scalar_>, Options>
    : public Sophus::RxSO2Base<Map<Sophus::RxSO2<Scalar_>, Options>> {
  using Base = Sophus::RxSO2Base<Map<Sophus::RxSO2<Scalar_>, 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;

  friend class Sophus::RxSO2Base<Map<Sophus::RxSO2<Scalar_>, Options>>;

  using Base::operator=;
  using Base::operator*=;
  using Base::operator*;

  SOPHUS_FUNC explicit Map(Scalar* coeffs) : complex_(coeffs) {}

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

 protected:
  SOPHUS_FUNC Map<Sophus::Vector2<Scalar>, Options>& complex_nonconst() {
    return complex_;
  }

  Map<Sophus::Vector2<Scalar>, Options> complex_;
};

template <class Scalar_, int Options>
class Map<Sophus::RxSO2<Scalar_> const, Options>
    : public Sophus::RxSO2Base<Map<Sophus::RxSO2<Scalar_> const, Options>> {
 public:
  using Base = Sophus::RxSO2Base<Map<Sophus::RxSO2<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) : complex_(coeffs) {}

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

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