rxso2.hpp

Go to the documentation of this file.

 
#ifndef SOPHUS_RXSO2_HPP
#define SOPHUS_RXSO2_HPP
 
#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>> {
  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>> {
  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> {
  using Scalar = Scalar_;
  using ComplexType = Map<Sophus::Vector2<Scalar> const, Options>;
};
}  // namespace internal
}  // namespace Eigen
 
namespace Sophus {
 
template <class Derived>
class RxSO2Base {
 public:
  using Scalar = typename Eigen::internal::traits<Derived>::Scalar;
  using ComplexType = typename Eigen::internal::traits<Derived>::ComplexType;
 
  static int constexpr DoF = 2;
  static int constexpr num_parameters = 2;
  static int constexpr N = 2;
  using Transformation = Matrix<Scalar, N, N>;
  using Point = Vector2<Scalar>;
  using HomogeneousPoint = Vector3<Scalar>;
  using Line = ParametrizedLine2<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 {
    return RxSO2<NewScalarType>(complex().template cast<NewScalarType>());
  }
 
  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();
    return RxSO2<Scalar>(complex().x() / squared_scale,
                         -complex().y() / squared_scale);
  }
 
  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;
  }
 
  SOPHUS_FUNC RxSO2Base& operator=(RxSO2Base const& other) = default;
 
  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>::epsilon();
    }
    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());
  }
 
  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 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.");
    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 {
    ComplexType norm_quad = complex();
    norm_quad.normalize();
    return SO2<Scalar>(norm_quad).matrix();
  }
 
  SOPHUS_FUNC
  Scalar scale() const { return complex().norm(); }
 
  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 %", 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()); }
 
 protected:
  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>>;
 
  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 %",
                  complex_.squaredNorm(),
                  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 Transformation Dxi_exp_x_matrix_at_0(int i) {
    return generator(i);
  }
  SOPHUS_FUNC static RxSO2<Scalar> exp(Tangent const& a) {
    using std::exp;
 
    Scalar const theta = a[0];
    Scalar const sigma = a[1];
    Scalar s = exp(sigma);
    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>>;
 
  // LCOV_EXCL_START
  EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map);
  // LCOV_EXCL_STOP
  using Base::operator*=;
  using Base::operator*;
 
  SOPHUS_FUNC 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
  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
 
#endif