.. _program_listing_file_sophus_cartesian.hpp:

Program Listing for File cartesian.hpp
======================================

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

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

.. code-block:: cpp

   
   #pragma once
   #include <sophus/types.hpp>
   
   namespace Sophus {
   template <class Scalar_, int M, int Options = 0>
   class Cartesian;
   
   template <class Scalar_>
   using Cartesian2 = Cartesian<Scalar_, 2>;
   
   template <class Scalar_>
   using Cartesian3 = Cartesian<Scalar_, 3>;
   
   using Cartesian2d = Cartesian2<double>;
   using Cartesian3d = Cartesian3<double>;
   
   }  // namespace Sophus
   
   namespace Eigen {
   namespace internal {
   
   template <class Scalar_, int M, int Options>
   struct traits<Sophus::Cartesian<Scalar_, M, Options>> {
     using Scalar = Scalar_;
     using ParamsType = Sophus::Vector<Scalar, M, Options>;
   };
   
   template <class Scalar_, int M, int Options>
   struct traits<Map<Sophus::Cartesian<Scalar_, M>, Options>>
       : traits<Sophus::Cartesian<Scalar_, M, Options>> {
     using Scalar = Scalar_;
     using ParamsType = Map<Sophus::Vector<Scalar, M>, Options>;
   };
   
   template <class Scalar_, int M, int Options>
   struct traits<Map<Sophus::Cartesian<Scalar_, M> const, Options>>
       : traits<Sophus::Cartesian<Scalar_, M, Options> const> {
     using Scalar = Scalar_;
     using ParamsType = Map<Sophus::Vector<Scalar, M> const, Options>;
   };
   }  // namespace internal
   }  // namespace Eigen
   
   namespace Sophus {
   
   //  (M+1) x (M+1) homogeneous matrix:
   template <class Derived, int M>
   class CartesianBase {
    public:
     using Scalar = typename Eigen::internal::traits<Derived>::Scalar;
     using ParamsType = typename Eigen::internal::traits<Derived>::ParamsType;
     static int constexpr DoF = M;
     static int constexpr num_parameters = M;
     static int constexpr N = M + 1;
     static int constexpr Dim = M;
   
     using Transformation = Sophus::Matrix<Scalar, N, N>;
     using Point = Sophus::Vector<Scalar, M>;
     using HomogeneousPoint = Sophus::Vector<Scalar, N>;
     using Line = ParametrizedLine<Scalar, M>;
     using Hyperplane = Eigen::Hyperplane<Scalar, M>;
     using Tangent = Sophus::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 CartesianSum = Cartesian<ReturnScalar<OtherDerived>, M>;
   
     template <typename PointDerived>
     using PointProduct = Sophus::Vector<ReturnScalar<PointDerived>, M>;
   
     template <typename HPointDerived>
     using HomogeneousPointProduct =
         Sophus::Vector<ReturnScalar<HPointDerived>, N>;
   
     SOPHUS_FUNC Adjoint Adj() const { return Adjoint::Identity(); }
   
     template <class NewScalarType>
     SOPHUS_FUNC Cartesian<NewScalarType, M> cast() const {
       return Cartesian<NewScalarType, M>(params().template cast<NewScalarType>());
     }
   
     SOPHUS_FUNC Matrix<Scalar, num_parameters, DoF> Dx_this_mul_exp_x_at_0()
         const {
       Sophus::Matrix<Scalar, num_parameters, DoF> m;
       m.setIdentity();
       return m;
     }
   
     SOPHUS_FUNC Matrix<Scalar, num_parameters, DoF> Dx_log_this_inv_by_x_at_this()
         const {
       Matrix<Scalar, DoF, num_parameters> m;
       m.setIdentity();
       return m;
     }
   
     SOPHUS_FUNC Cartesian<Scalar, M> inverse() const {
       return Cartesian<Scalar, M>(-params());
     }
   
     SOPHUS_FUNC Tangent log() const { return params(); }
   
     SOPHUS_FUNC Transformation matrix() const {
       Sophus::Matrix<Scalar, N, N> matrix;
       matrix.setIdentity();
       matrix.col(M).template head<M>() = params();
       return matrix;
     }
   
     template <class OtherDerived>
     SOPHUS_FUNC CartesianBase<Derived, M>& operator=(
         CartesianBase<OtherDerived, M> const& other) {
       params() = other.params();
       return *this;
     }
   
     template <typename OtherDerived>
     SOPHUS_FUNC CartesianSum<OtherDerived> operator*(
         CartesianBase<OtherDerived, M> const& other) const {
       return CartesianSum<OtherDerived>(params() + other.params());
     }
   
     template <typename PointDerived,
               typename = typename std::enable_if<
                   IsFixedSizeVector<PointDerived, M>::value>::type>
     SOPHUS_FUNC PointProduct<PointDerived> operator*(
         Eigen::MatrixBase<PointDerived> const& p) const {
       return PointProduct<PointDerived>(params() + p);
     }
   
     template <typename HPointDerived,
               typename = typename std::enable_if<
                   IsFixedSizeVector<HPointDerived, N>::value>::type>
     SOPHUS_FUNC HomogeneousPointProduct<HPointDerived> operator*(
         Eigen::MatrixBase<HPointDerived> const& p) const {
       const auto rp = *this * p.template head<M>();
       HomogeneousPointProduct<HPointDerived> r;
       r << rp, p(M);
       return r;
     }
   
     SOPHUS_FUNC Line operator*(Line const& l) const {
       return Line((*this) * l.origin(), l.direction());
     }
   
     SOPHUS_FUNC Hyperplane operator*(Hyperplane const& p) const {
       return Hyperplane(p.normal(), p.offset() - params().dot(p.normal()));
     }
   
     template <typename OtherDerived,
               typename = typename std::enable_if<
                   std::is_same<Scalar, ReturnScalar<OtherDerived>>::value>::type>
     SOPHUS_FUNC CartesianBase<Derived, M>& operator*=(
         CartesianBase<OtherDerived, M> const& other) {
       *static_cast<Derived*>(this) = *this * other;
       return *this;
     }
   
     SOPHUS_FUNC ParamsType& params() {
       return static_cast<Derived*>(this)->params();
     }
   
     SOPHUS_FUNC ParamsType const& params() const {
       return static_cast<Derived const*>(this)->params();
     }
   };
   
   template <class Scalar_, int M, int Options>
   class Cartesian : public CartesianBase<Cartesian<Scalar_, M, Options>, M> {
     using Base = CartesianBase<Cartesian<Scalar_, M, Options>, M>;
   
    public:
     static int constexpr DoF = Base::DoF;
     static int constexpr num_parameters = Base::num_parameters;
     static int constexpr N = Base::N;
     static int constexpr Dim = Base::Dim;
   
     using Scalar = Scalar_;
     using Transformation = typename Base::Transformation;
     using Point = typename Base::Point;
     using HomogeneousPoint = typename Base::HomogeneousPoint;
     using Tangent = typename Base::Tangent;
     using ParamsMember = Sophus::Vector<Scalar, M, Options>;
   
     using Base::operator=;
   
     SOPHUS_FUNC Cartesian& operator=(Cartesian const& other) = default;
   
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
   
     SOPHUS_FUNC Cartesian() { params_.setZero(); }
   
     SOPHUS_FUNC Cartesian(Cartesian const& other) = default;
   
     template <class OtherDerived>
     SOPHUS_FUNC Cartesian(CartesianBase<OtherDerived, M> const& other)
         : params_(other.params()) {
       static_assert(std::is_same<typename OtherDerived::Scalar, Scalar>::value,
                     "must be same Scalar type");
     }
   
     template <class D>
     explicit SOPHUS_FUNC Cartesian(Eigen::MatrixBase<D> const& m) {
       static_assert(
           std::is_same<typename Eigen::MatrixBase<D>::Scalar, Scalar>::value, "");
       if (m.rows() == DoF && m.cols() == 1) {
         // trick so this compiles
         params_ = m.template block<M, 1>(0, 0);
       } else if (m.rows() == N && m.cols() == N) {
         params_ = m.template block<M, 1>(0, M);
       } else {
         SOPHUS_ENSURE(false, "{} {}", m.rows(), m.cols());
       }
     }
   
     SOPHUS_FUNC Scalar* data() { return params_.data(); }
   
     SOPHUS_FUNC Scalar const* data() const { return params_.data(); }
   
     SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF>
     Dx_exp_x_at_0() {
       Sophus::Matrix<Scalar, num_parameters, DoF> m;
       m.setIdentity();
       return m;
     }
   
     SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF> Dx_exp_x(
         Tangent const&) {
       return Dx_exp_x_at_0();
     }
   
     SOPHUS_FUNC static Sophus::Matrix<Scalar, Dim, DoF> Dx_exp_x_times_point_at_0(
         Point const&) {
       Sophus::Matrix<Scalar, Dim, DoF> J;
       J.setIdentity();
       return J;
     }
   
     SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) {
       return generator(i);
     }
   
     SOPHUS_FUNC ParamsMember& params() { return params_; }
   
     SOPHUS_FUNC ParamsMember const& params() const { return params_; }
   
     SOPHUS_FUNC static Transformation generator(int i) {
       SOPHUS_ENSURE(i >= 0 && i <= M, "i should be in range [0,M-1].");
       Tangent e;
       e.setZero();
       e[i] = Scalar(1);
       return hat(e);
     }
   
     //  from the M-vector.
     SOPHUS_FUNC static Cartesian<Scalar, M> exp(Tangent const& a) {
       return Cartesian<Scalar, M>(a);
     }
   
     SOPHUS_FUNC static Transformation hat(Tangent const& a) {
       Transformation Omega;
       Omega.setZero();
       Omega.col(M).template head<M>() = a.template head<M>();
       return Omega;
     }
   
     SOPHUS_FUNC static Tangent lieBracket(Tangent const&, Tangent const&) {
       return Tangent::Zero();
     }
   
     template <class UniformRandomBitGenerator>
     static Cartesian sampleUniform(UniformRandomBitGenerator& generator) {
       std::uniform_real_distribution<Scalar> uniform(Scalar(-1), Scalar(1));
       Vector<Scalar, M> v;
       for (int i = 0; i < M; ++i) {
         v[i] = uniform(generator);
       }
       return Cartesian(v);
     }
   
     SOPHUS_FUNC static Tangent vee(Transformation const& m) {
       return m.col(M).template head<M>();
     }
   
    protected:
     ParamsMember params_;
   };
   
   }  // namespace Sophus
   
   namespace Eigen {
   
   template <class Scalar_, int M, int Options>
   class Map<Sophus::Cartesian<Scalar_, M>, Options>
       : public Sophus::CartesianBase<Map<Sophus::Cartesian<Scalar_, M>, Options>,
                                      M> {
    public:
     using Base =
         Sophus::CartesianBase<Map<Sophus::Cartesian<Scalar_, M>, Options>, M>;
     using Scalar = Scalar_;
     using Transformation = typename Base::Transformation;
     using Point = typename Base::Point;
     using HomogeneousPoint = typename Base::HomogeneousPoint;
     using Tangent = typename Base::Tangent;
   
     using Base::operator=;
     using Base::operator*=;
     using Base::operator*;
   
     SOPHUS_FUNC explicit Map(Scalar* coeffs) : params_(coeffs) {}
   
     SOPHUS_FUNC Map<Sophus::Vector<Scalar, M, Options>>& params() {
       return params_;
     }
   
     SOPHUS_FUNC Map<Sophus::Vector<Scalar, M, Options>> const& params() const {
       return params_;
     }
   
    protected:
     Map<Sophus::Vector<Scalar, M>, Options> params_;
   };
   
   template <class Scalar_, int M, int Options>
   class Map<Sophus::Cartesian<Scalar_, M> const, Options>
       : public Sophus::CartesianBase<
             Map<Sophus::Cartesian<Scalar_, M> const, Options>, M> {
    public:
     using Base =
         Sophus::CartesianBase<Map<Sophus::Cartesian<Scalar_, M> const, Options>,
                               M>;
     using Scalar = Scalar_;
     using Transformation = typename Base::Transformation;
     using Point = typename Base::Point;
     using HomogeneousPoint = typename Base::HomogeneousPoint;
     using Tangent = typename Base::Tangent;
   
     using Base::operator*;
   
     SOPHUS_FUNC Map(Scalar const* coeffs) : params_(coeffs) {}
   
     SOPHUS_FUNC Map<Sophus::Vector<Scalar, M> const, Options> const& params()
         const {
       return params_;
     }
   
    protected:
     Map<Sophus::Vector<Scalar, M> const, Options> const params_;
   };
   }  // namespace Eigen