.. _program_listing_file_sophus_rxso2.hpp:

Program Listing for File rxso2.hpp
==================================

|exhale_lsh| :ref:`Return to documentation for file <file_sophus_rxso2.hpp>` (``sophus/rxso2.hpp``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   
   #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