.. _program_listing_file_sophus_so2.hpp:

Program Listing for File so2.hpp
================================

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

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

.. code-block:: cpp

   
   #pragma once
   
   #include <type_traits>
   
   // Include only the selective set of Eigen headers that we need.
   // This helps when using Sophus with unusual compilers, like nvcc.
   #include <Eigen/LU>
   
   #include "rotation_matrix.hpp"
   #include "types.hpp"
   
   namespace Sophus {
   template <class Scalar_, int Options = 0>
   class SO2;
   using SO2d = SO2<double>;
   using SO2f = SO2<float>;
   }  // namespace Sophus
   
   namespace Eigen {
   namespace internal {
   
   template <class Scalar_, int Options_>
   struct traits<Sophus::SO2<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::SO2<Scalar_>, Options_>>
       : traits<Sophus::SO2<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::SO2<Scalar_> const, Options_>>
       : traits<Sophus::SO2<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 SO2Base {
    public:
     static constexpr int Options = Eigen::internal::traits<Derived>::Options;
     using Scalar = typename Eigen::internal::traits<Derived>::Scalar;
     using ComplexT = typename Eigen::internal::traits<Derived>::ComplexType;
     using ComplexTemporaryType = Sophus::Vector2<Scalar, Options>;
   
     static int constexpr DoF = 1;
     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 = Scalar;
     using Adjoint = Scalar;
   
     template <typename OtherDerived>
     using ReturnScalar = typename Eigen::ScalarBinaryOpTraits<
         Scalar, typename OtherDerived::Scalar>::ReturnType;
   
     template <typename OtherDerived>
     using SO2Product = SO2<ReturnScalar<OtherDerived>>;
   
     template <typename PointDerived>
     using PointProduct = Vector2<ReturnScalar<PointDerived>>;
   
     template <typename HPointDerived>
     using HomogeneousPointProduct = Vector3<ReturnScalar<HPointDerived>>;
   
     SOPHUS_FUNC Adjoint Adj() const { return Scalar(1); }
   
     template <class NewScalarType>
     SOPHUS_FUNC SO2<NewScalarType> cast() const {
       return SO2<NewScalarType>(unit_complex().template cast<NewScalarType>());
     }
   
     SOPHUS_FUNC Scalar* data() { return unit_complex_nonconst().data(); }
   
     SOPHUS_FUNC Scalar const* data() const { return unit_complex().data(); }
   
     SOPHUS_FUNC SO2<Scalar> inverse() const {
       return SO2<Scalar>(unit_complex().x(), -unit_complex().y());
     }
   
     SOPHUS_FUNC Scalar log() const {
       using std::atan2;
       return atan2(unit_complex().y(), unit_complex().x());
     }
   
     SOPHUS_FUNC void normalize() {
       using std::hypot;
       // Avoid under/overflows for higher precision
       Scalar length = hypot(unit_complex().x(), unit_complex().y());
       SOPHUS_ENSURE(length >= Constants<Scalar>::epsilon(), "%s",
                     "Complex number should not be close to zero!");
       unit_complex_nonconst() /= length;
     }
   
     SOPHUS_FUNC Transformation matrix() const {
       Scalar const& real = unit_complex().x();
       Scalar const& imag = unit_complex().y();
       Transformation R;
       // clang-format off
       R <<
         real, -imag,
         imag,  real;
       // clang-format on
       return R;
     }
   
     template <class OtherDerived>
     SOPHUS_FUNC SO2Base<Derived>& operator=(SO2Base<OtherDerived> const& other) {
       unit_complex_nonconst() = other.unit_complex();
       return *this;
     }
   
     template <typename OtherDerived>
     SOPHUS_FUNC SO2Product<OtherDerived> operator*(
         SO2Base<OtherDerived> const& other) const {
       using ResultT = ReturnScalar<OtherDerived>;
       Scalar const lhs_real = unit_complex().x();
       Scalar const lhs_imag = unit_complex().y();
       typename OtherDerived::Scalar const& rhs_real = other.unit_complex().x();
       typename OtherDerived::Scalar const& rhs_imag = other.unit_complex().y();
       // complex multiplication
       ResultT const result_real = lhs_real * rhs_real - lhs_imag * rhs_imag;
       ResultT const result_imag = lhs_real * rhs_imag + lhs_imag * rhs_real;
   
       ResultT const squared_norm =
           result_real * result_real + result_imag * result_imag;
       // We can assume that the squared-norm is close to 1 since we deal with a
       // unit complex number. Due to numerical precision issues, there might
       // be a small drift after pose concatenation. Hence, we need to renormalizes
       // the complex number here.
       // Since squared-norm is close to 1, we do not need to calculate the costly
       // square-root, but can use an approximation around 1 (see
       // http://stackoverflow.com/a/12934750 for details).
       if (squared_norm != ResultT(1.0)) {
         ResultT const scale = ResultT(2.0) / (ResultT(1.0) + squared_norm);
         return SO2Product<OtherDerived>(result_real * scale, result_imag * scale);
       }
       return SO2Product<OtherDerived>(result_real, result_imag);
     }
   
     template <typename PointDerived,
               typename = typename std::enable_if<
                   IsFixedSizeVector<PointDerived, 2>::value>::type>
     SOPHUS_FUNC PointProduct<PointDerived> operator*(
         Eigen::MatrixBase<PointDerived> const& p) const {
       Scalar const& real = unit_complex().x();
       Scalar const& imag = unit_complex().y();
       return PointProduct<PointDerived>(real * p[0] - imag * p[1],
                                         imag * p[0] + real * p[1]);
     }
   
     template <typename HPointDerived,
               typename = typename std::enable_if<
                   IsFixedSizeVector<HPointDerived, 3>::value>::type>
     SOPHUS_FUNC HomogeneousPointProduct<HPointDerived> operator*(
         Eigen::MatrixBase<HPointDerived> const& p) const {
       Scalar const& real = unit_complex().x();
       Scalar const& imag = unit_complex().y();
       return HomogeneousPointProduct<HPointDerived>(
           real * p[0] - imag * p[1], imag * p[0] + real * p[1], p[2]);
     }
   
     SOPHUS_FUNC Line operator*(Line const& l) const {
       return Line((*this) * l.origin(), (*this) * l.direction());
     }
   
     SOPHUS_FUNC Hyperplane operator*(Hyperplane const& p) const {
       return Hyperplane((*this) * p.normal(), p.offset());
     }
   
     template <typename OtherDerived,
               typename = typename std::enable_if<
                   std::is_same<Scalar, ReturnScalar<OtherDerived>>::value>::type>
     SOPHUS_FUNC SO2Base<Derived> operator*=(SO2Base<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 {
       return Matrix<Scalar, num_parameters, DoF>(-unit_complex()[1],
                                                  unit_complex()[0]);
     }
   
     SOPHUS_FUNC Sophus::Vector<Scalar, num_parameters> params() const {
       return unit_complex();
     }
   
     SOPHUS_FUNC Matrix<Scalar, DoF, num_parameters> Dx_log_this_inv_by_x_at_this()
         const {
       return Matrix<Scalar, DoF, num_parameters>(-unit_complex()[1],
                                                  unit_complex()[0]);
     }
   
     SOPHUS_FUNC void setComplex(Point const& complex) {
       unit_complex_nonconst() = complex;
       normalize();
     }
   
     SOPHUS_FUNC
     ComplexT const& unit_complex() const {
       return static_cast<Derived const*>(this)->unit_complex();
     }
   
    private:
     SOPHUS_FUNC
     ComplexT& unit_complex_nonconst() {
       return static_cast<Derived*>(this)->unit_complex_nonconst();
     }
   };
   
   template <class Scalar_, int Options>
   class SO2 : public SO2Base<SO2<Scalar_, Options>> {
    public:
     using Base = SO2Base<SO2<Scalar_, Options>>;
     static int constexpr DoF = Base::DoF;
     static int constexpr num_parameters = Base::num_parameters;
   
     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 = Vector2<Scalar, Options>;
   
     friend class SO2Base<SO2<Scalar, Options>>;
   
     using Base::operator=;
   
     SOPHUS_FUNC SO2& operator=(SO2 const& other) = default;
   
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
   
     SOPHUS_FUNC SO2() : unit_complex_(Scalar(1), Scalar(0)) {}
   
     SOPHUS_FUNC SO2(SO2 const& other) = default;
   
     template <class OtherDerived>
     SOPHUS_FUNC SO2(SO2Base<OtherDerived> const& other)
         : unit_complex_(other.unit_complex()) {}
   
     SOPHUS_FUNC explicit SO2(Transformation const& R)
         : unit_complex_(Scalar(0.5) * (R(0, 0) + R(1, 1)),
                         Scalar(0.5) * (R(1, 0) - R(0, 1))) {
       SOPHUS_ENSURE(isOrthogonal(R), "R is not orthogonal:\n {}",
                     SOPHUS_FMT_ARG(R));
       SOPHUS_ENSURE(R.determinant() > Scalar(0), "det(R) is not positive: {}",
                     SOPHUS_FMT_ARG(R.determinant()));
     }
   
     SOPHUS_FUNC SO2(Scalar const& real, Scalar const& imag)
         : unit_complex_(real, imag) {
       Base::normalize();
     }
   
     template <class D>
     SOPHUS_FUNC explicit SO2(Eigen::MatrixBase<D> const& complex)
         : unit_complex_(complex) {
       static_assert(std::is_same<typename D::Scalar, Scalar>::value,
                     "must be same Scalar type");
       Base::normalize();
     }
   
     SOPHUS_FUNC explicit SO2(Scalar theta) {
       unit_complex_nonconst() = SO2<Scalar>::exp(theta).unit_complex();
     }
   
     SOPHUS_FUNC ComplexMember const& unit_complex() const {
       return unit_complex_;
     }
   
     SOPHUS_FUNC static SO2<Scalar> exp(Tangent const& theta) {
       using std::cos;
       using std::sin;
       return SO2<Scalar>(cos(theta), sin(theta));
     }
   
     SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF> Dx_exp_x(
         Tangent const& theta) {
       using std::cos;
       using std::sin;
       return Sophus::Matrix<Scalar, num_parameters, DoF>(-sin(theta), cos(theta));
     }
   
     SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF>
     Dx_exp_x_at_0() {
       return Sophus::Matrix<Scalar, num_parameters, DoF>(Scalar(0), Scalar(1));
     }
   
     SOPHUS_FUNC static Sophus::Matrix<Scalar, 2, DoF> Dx_exp_x_times_point_at_0(
         Point const& point) {
       return Point(-point.y(), point.x());
     }
   
     SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int) {
       return generator();
     }
   
     SOPHUS_FUNC static Transformation generator() { return hat(Scalar(1)); }
   
     SOPHUS_FUNC static Transformation hat(Tangent const& theta) {
       Transformation Omega;
       // clang-format off
       Omega <<
           Scalar(0),   -theta,
               theta, Scalar(0);
       // clang-format on
       return Omega;
     }
   
     template <class S = Scalar>
     static SOPHUS_FUNC enable_if_t<std::is_floating_point<S>::value, SO2>
     fitToSO2(Transformation const& R) {
       return SO2(makeRotationMatrix(R));
     }
   
     SOPHUS_FUNC static Tangent lieBracket(Tangent const&, Tangent const&) {
       return Scalar(0);
     }
   
     template <class UniformRandomBitGenerator>
     static SO2 sampleUniform(UniformRandomBitGenerator& generator) {
       static_assert(IsUniformRandomBitGenerator<UniformRandomBitGenerator>::value,
                     "generator must meet the UniformRandomBitGenerator concept");
       std::uniform_real_distribution<Scalar> uniform(-Constants<Scalar>::pi(),
                                                      Constants<Scalar>::pi());
       return SO2(uniform(generator));
     }
   
     SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
       using std::abs;
       return Omega(1, 0);
     }
   
    protected:
     SOPHUS_FUNC ComplexMember& unit_complex_nonconst() { return unit_complex_; }
   
     ComplexMember unit_complex_;
   };
   
   }  // namespace Sophus
   
   namespace Eigen {
   
   template <class Scalar_, int Options>
   class Map<Sophus::SO2<Scalar_>, Options>
       : public Sophus::SO2Base<Map<Sophus::SO2<Scalar_>, Options>> {
    public:
     using Base = Sophus::SO2Base<Map<Sophus::SO2<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;
   
     friend class Sophus::SO2Base<Map<Sophus::SO2<Scalar_>, Options>>;
   
     using Base::operator=;
     using Base::operator*=;
     using Base::operator*;
   
     SOPHUS_FUNC
     explicit Map(Scalar* coeffs) : unit_complex_(coeffs) {}
   
     SOPHUS_FUNC
     Map<Sophus::Vector2<Scalar>, Options> const& unit_complex() const {
       return unit_complex_;
     }
   
    protected:
     SOPHUS_FUNC
     Map<Sophus::Vector2<Scalar>, Options>& unit_complex_nonconst() {
       return unit_complex_;
     }
   
     Map<Matrix<Scalar, 2, 1>, Options> unit_complex_;
   };
   
   template <class Scalar_, int Options>
   class Map<Sophus::SO2<Scalar_> const, Options>
       : public Sophus::SO2Base<Map<Sophus::SO2<Scalar_> const, Options>> {
    public:
     using Base = Sophus::SO2Base<Map<Sophus::SO2<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) : unit_complex_(coeffs) {}
   
     SOPHUS_FUNC Map<Sophus::Vector2<Scalar> const, Options> const& unit_complex()
         const {
       return unit_complex_;
     }
   
    protected:
     Map<Matrix<Scalar, 2, 1> const, Options> const unit_complex_;
   };
   }  // namespace Eigen