.. _program_listing_file_sophus_rxso3.hpp:

Program Listing for File rxso3.hpp
==================================

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

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

.. code-block:: cpp

   
   #pragma once
   
   #include "so3.hpp"
   
   namespace Sophus {
   template <class Scalar_, int Options = 0>
   class RxSO3;
   using RxSO3d = RxSO3<double>;
   using RxSO3f = RxSO3<float>;
   }  // namespace Sophus
   
   namespace Eigen {
   namespace internal {
   
   template <class Scalar_, int Options_>
   struct traits<Sophus::RxSO3<Scalar_, Options_>> {
     static constexpr int Options = Options_;
     using Scalar = Scalar_;
     using QuaternionType = Eigen::Quaternion<Scalar, Options>;
   };
   
   template <class Scalar_, int Options_>
   struct traits<Map<Sophus::RxSO3<Scalar_>, Options_>>
       : traits<Sophus::RxSO3<Scalar_, Options_>> {
     static constexpr int Options = Options_;
     using Scalar = Scalar_;
     using QuaternionType = Map<Eigen::Quaternion<Scalar>, Options>;
   };
   
   template <class Scalar_, int Options_>
   struct traits<Map<Sophus::RxSO3<Scalar_> const, Options_>>
       : traits<Sophus::RxSO3<Scalar_, Options_> const> {
     static constexpr int Options = Options_;
     using Scalar = Scalar_;
     using QuaternionType = Map<Eigen::Quaternion<Scalar> const, Options>;
   };
   }  // namespace internal
   }  // namespace Eigen
   
   namespace Sophus {
   
   template <class Derived>
   class RxSO3Base {
    public:
     static constexpr int Options = Eigen::internal::traits<Derived>::Options;
     using Scalar = typename Eigen::internal::traits<Derived>::Scalar;
     using QuaternionType =
         typename Eigen::internal::traits<Derived>::QuaternionType;
     using QuaternionTemporaryType = Eigen::Quaternion<Scalar, Options>;
   
     static int constexpr DoF = 4;
     static int constexpr num_parameters = 4;
     static int constexpr N = 3;
     static int constexpr Dim = 3;
     using Transformation = Matrix<Scalar, N, N>;
     using Point = Vector3<Scalar>;
     using HomogeneousPoint = Vector4<Scalar>;
     using Line = ParametrizedLine3<Scalar>;
     using Hyperplane = Hyperplane3<Scalar>;
     using Tangent = Vector<Scalar, DoF>;
     using Adjoint = Matrix<Scalar, DoF, DoF>;
   
     struct TangentAndTheta {
       EIGEN_MAKE_ALIGNED_OPERATOR_NEW
   
       Tangent tangent;
       Scalar theta;
     };
   
     template <typename OtherDerived>
     using ReturnScalar = typename Eigen::ScalarBinaryOpTraits<
         Scalar, typename OtherDerived::Scalar>::ReturnType;
   
     template <typename OtherDerived>
     using RxSO3Product = RxSO3<ReturnScalar<OtherDerived>>;
   
     template <typename PointDerived>
     using PointProduct = Vector3<ReturnScalar<PointDerived>>;
   
     template <typename HPointDerived>
     using HomogeneousPointProduct = Vector4<ReturnScalar<HPointDerived>>;
   
     SOPHUS_FUNC Adjoint Adj() const {
       Adjoint res;
       res.setIdentity();
       res.template topLeftCorner<3, 3>() = rotationMatrix();
       return res;
     }
   
     template <class NewScalarType>
     SOPHUS_FUNC RxSO3<NewScalarType> cast() const {
       return RxSO3<NewScalarType>(quaternion().template cast<NewScalarType>());
     }
   
     SOPHUS_FUNC Scalar* data() { return quaternion_nonconst().coeffs().data(); }
   
     SOPHUS_FUNC Scalar const* data() const {
       return quaternion().coeffs().data();
     }
   
     SOPHUS_FUNC RxSO3<Scalar> inverse() const {
       return RxSO3<Scalar>(quaternion().inverse());
     }
   
     SOPHUS_FUNC Tangent log() const { return logAndTheta().tangent; }
   
     SOPHUS_FUNC TangentAndTheta logAndTheta() const {
       using std::log;
   
       Scalar scale = quaternion().squaredNorm();
       TangentAndTheta result;
       result.tangent[3] = log(scale);
       auto omega_and_theta = SO3<Scalar>(quaternion()).logAndTheta();
       result.tangent.template head<3>() = omega_and_theta.tangent;
       result.theta = omega_and_theta.theta;
       return result;
     }
     SOPHUS_FUNC Transformation matrix() const {
       Transformation sR;
   
       Scalar const vx_sq = quaternion().vec().x() * quaternion().vec().x();
       Scalar const vy_sq = quaternion().vec().y() * quaternion().vec().y();
       Scalar const vz_sq = quaternion().vec().z() * quaternion().vec().z();
       Scalar const w_sq = quaternion().w() * quaternion().w();
       Scalar const two_vx = Scalar(2) * quaternion().vec().x();
       Scalar const two_vy = Scalar(2) * quaternion().vec().y();
       Scalar const two_vz = Scalar(2) * quaternion().vec().z();
       Scalar const two_vx_vy = two_vx * quaternion().vec().y();
       Scalar const two_vx_vz = two_vx * quaternion().vec().z();
       Scalar const two_vx_w = two_vx * quaternion().w();
       Scalar const two_vy_vz = two_vy * quaternion().vec().z();
       Scalar const two_vy_w = two_vy * quaternion().w();
       Scalar const two_vz_w = two_vz * quaternion().w();
   
       sR(0, 0) = vx_sq - vy_sq - vz_sq + w_sq;
       sR(1, 0) = two_vx_vy + two_vz_w;
       sR(2, 0) = two_vx_vz - two_vy_w;
   
       sR(0, 1) = two_vx_vy - two_vz_w;
       sR(1, 1) = -vx_sq + vy_sq - vz_sq + w_sq;
       sR(2, 1) = two_vx_w + two_vy_vz;
   
       sR(0, 2) = two_vx_vz + two_vy_w;
       sR(1, 2) = -two_vx_w + two_vy_vz;
       sR(2, 2) = -vx_sq - vy_sq + vz_sq + w_sq;
       return sR;
     }
   
     template <class OtherDerived>
     SOPHUS_FUNC RxSO3Base<Derived>& operator=(
         RxSO3Base<OtherDerived> const& other) {
       quaternion_nonconst() = other.quaternion();
       return *this;
     }
   
     template <typename OtherDerived>
     SOPHUS_FUNC RxSO3Product<OtherDerived> operator*(
         RxSO3Base<OtherDerived> const& other) const {
       using std::sqrt;
       using ResultT = ReturnScalar<OtherDerived>;
       using QuaternionProductType =
           typename RxSO3Product<OtherDerived>::QuaternionType;
   
       QuaternionProductType result_quaternion(
           Sophus::SO3<double>::QuaternionProduct<QuaternionProductType>(
               quaternion(), other.quaternion()));
   
       ResultT scale = result_quaternion.squaredNorm();
       if (scale < Constants<ResultT>::epsilon()) {
         SOPHUS_ENSURE(scale > ResultT(0), "Scale must be greater zero.");
         result_quaternion.normalize();
         result_quaternion.coeffs() *= sqrt(Constants<ResultT>::epsilonPlus());
       }
       if (scale > ResultT(1.) / Constants<ResultT>::epsilon()) {
         result_quaternion.normalize();
         result_quaternion.coeffs() /= sqrt(Constants<ResultT>::epsilonPlus());
       }
       return RxSO3Product<OtherDerived>(result_quaternion);
     }
   
     template <typename PointDerived,
               typename = typename std::enable_if<
                   IsFixedSizeVector<PointDerived, 3>::value>::type>
     SOPHUS_FUNC PointProduct<PointDerived> operator*(
         Eigen::MatrixBase<PointDerived> const& p) const {
       // Follows http:///eigen.tuxfamily.org/bz/show_bug.cgi?id=459
       Scalar scale = quaternion().squaredNorm();
       PointProduct<PointDerived> two_vec_cross_p = quaternion().vec().cross(p);
       two_vec_cross_p += two_vec_cross_p;
       return scale * p + (quaternion().w() * two_vec_cross_p +
                           quaternion().vec().cross(two_vec_cross_p));
     }
   
     template <typename HPointDerived,
               typename = typename std::enable_if<
                   IsFixedSizeVector<HPointDerived, 4>::value>::type>
     SOPHUS_FUNC HomogeneousPointProduct<HPointDerived> operator*(
         Eigen::MatrixBase<HPointDerived> const& p) const {
       const auto rsp = *this * p.template head<3>();
       return HomogeneousPointProduct<HPointDerived>(rsp(0), rsp(1), rsp(2), p(3));
     }
   
     SOPHUS_FUNC Line operator*(Line const& l) const {
       return Line((*this) * l.origin(),
                   (*this) * l.direction() / quaternion().squaredNorm());
     }
   
     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 RxSO3Base<Derived>& operator*=(
         RxSO3Base<OtherDerived> const& other) {
       *static_cast<Derived*>(this) = *this * other;
       return *this;
     }
   
     SOPHUS_FUNC Sophus::Vector<Scalar, num_parameters> params() const {
       return quaternion().coeffs();
     }
   
     SOPHUS_FUNC void setQuaternion(Eigen::Quaternion<Scalar> const& quat) {
       SOPHUS_ENSURE(quat.squaredNorm() > Constants<Scalar>::epsilon() *
                                              Constants<Scalar>::epsilon(),
                     "Scale factor must be greater-equal epsilon.");
       SOPHUS_ENSURE(
           quat.squaredNorm() < Scalar(1.) / (Constants<Scalar>::epsilon() *
                                              Constants<Scalar>::epsilon()),
           "Inverse scale factor must be greater-equal epsilon.");
       static_cast<Derived*>(this)->quaternion_nonconst() = quat;
     }
   
     SOPHUS_FUNC QuaternionType const& quaternion() const {
       return static_cast<Derived const*>(this)->quaternion();
     }
   
     SOPHUS_FUNC Transformation rotationMatrix() const {
       QuaternionTemporaryType norm_quad = quaternion();
       norm_quad.normalize();
       return norm_quad.toRotationMatrix();
     }
   
     SOPHUS_FUNC
     Scalar scale() const { return quaternion().squaredNorm(); }
   
     SOPHUS_FUNC void setRotationMatrix(Transformation const& R) {
       using std::sqrt;
       Scalar saved_scale = scale();
       quaternion_nonconst() = R;
       quaternion_nonconst().coeffs() *= sqrt(saved_scale);
     }
   
     SOPHUS_FUNC
     void setScale(Scalar const& scale) {
       using std::sqrt;
       quaternion_nonconst().normalize();
       quaternion_nonconst().coeffs() *= sqrt(scale);
     }
   
     SOPHUS_FUNC void setScaledRotationMatrix(Transformation const& sR) {
       Transformation squared_sR = sR * sR.transpose();
       Scalar squared_scale =
           Scalar(1. / 3.) *
           (squared_sR(0, 0) + squared_sR(1, 1) + squared_sR(2, 2));
       SOPHUS_ENSURE(squared_scale >= Constants<Scalar>::epsilon() *
                                          Constants<Scalar>::epsilon(),
                     "Scale factor must be greater-equal epsilon.");
       SOPHUS_ENSURE(squared_scale < Scalar(1.) / (Constants<Scalar>::epsilon() *
                                                   Constants<Scalar>::epsilon()),
                     "Inverse scale factor must be greater-equal epsilon.");
       Scalar scale = sqrt(squared_scale);
       quaternion_nonconst() = sR / scale;
       quaternion_nonconst().coeffs() *= sqrt(scale);
     }
   
     SOPHUS_FUNC void setSO3(SO3<Scalar> const& so3) {
       using std::sqrt;
       Scalar saved_scale = scale();
       quaternion_nonconst() = so3.unit_quaternion();
       quaternion_nonconst().coeffs() *= sqrt(saved_scale);
     }
   
     SOPHUS_FUNC SO3<Scalar> so3() const { return SO3<Scalar>(quaternion()); }
   
     SOPHUS_FUNC Matrix<Scalar, num_parameters, DoF> Dx_this_mul_exp_x_at_0()
         const {
       Matrix<Scalar, num_parameters, DoF> J;
       Eigen::Quaternion<Scalar> const q = quaternion();
       J.col(3) = q.coeffs() * Scalar(0.5);
       Scalar const c0 = Scalar(0.5) * q.w();
       Scalar const c1 = Scalar(0.5) * q.z();
       Scalar const c2 = -c1;
       Scalar const c3 = Scalar(0.5) * q.y();
       Scalar const c4 = Scalar(0.5) * q.x();
       Scalar const c5 = -c4;
       Scalar const c6 = -c3;
       J(0, 0) = c0;
       J(0, 1) = c2;
       J(0, 2) = c3;
       J(1, 0) = c1;
       J(1, 1) = c0;
       J(1, 2) = c5;
       J(2, 0) = c6;
       J(2, 1) = c4;
       J(2, 2) = c0;
       J(3, 0) = c5;
       J(3, 1) = c6;
       J(3, 2) = c2;
       return J;
     }
   
     SOPHUS_FUNC Matrix<Scalar, DoF, num_parameters> Dx_log_this_inv_by_x_at_this()
         const {
       auto& q = quaternion();
       Matrix<Scalar, DoF, num_parameters> J;
       // clang-format off
       J << q.w(),  q.z(), -q.y(), -q.x(),
           -q.z(),  q.w(),  q.x(), -q.y(),
            q.y(), -q.x(),  q.w(), -q.z(),
            q.x(),  q.y(),  q.z(),  q.w();
       // clang-format on
       const Scalar scaler = Scalar(2.) / q.squaredNorm();
       return J * scaler;
     }
   
    private:
     SOPHUS_FUNC QuaternionType& quaternion_nonconst() {
       return static_cast<Derived*>(this)->quaternion_nonconst();
     }
   };
   
   template <class Scalar_, int Options>
   class RxSO3 : public RxSO3Base<RxSO3<Scalar_, Options>> {
    public:
     using Base = RxSO3Base<RxSO3<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 QuaternionMember = Eigen::Quaternion<Scalar, Options>;
   
     friend class RxSO3Base<RxSO3<Scalar_, Options>>;
   
     using Base::operator=;
   
     SOPHUS_FUNC RxSO3& operator=(RxSO3 const& other) = default;
   
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
   
     SOPHUS_FUNC RxSO3()
         : quaternion_(Scalar(1), Scalar(0), Scalar(0), Scalar(0)) {}
   
     SOPHUS_FUNC RxSO3(RxSO3 const& other) = default;
   
     template <class OtherDerived>
     SOPHUS_FUNC RxSO3(RxSO3Base<OtherDerived> const& other)
         : quaternion_(other.quaternion()) {}
   
     SOPHUS_FUNC explicit RxSO3(Transformation const& sR) {
       this->setScaledRotationMatrix(sR);
     }
   
     SOPHUS_FUNC RxSO3(Scalar const& scale, Transformation const& R)
         : quaternion_(R) {
       SOPHUS_ENSURE(scale >= Constants<Scalar>::epsilon(),
                     "Scale factor must be greater-equal epsilon.");
       SOPHUS_ENSURE(scale < Scalar(1.) / Constants<Scalar>::epsilon(),
                     "Inverse scale factor must be greater-equal epsilon.");
       using std::sqrt;
       quaternion_.coeffs() *= sqrt(scale);
     }
   
     SOPHUS_FUNC RxSO3(Scalar const& scale, SO3<Scalar> const& so3)
         : quaternion_(so3.unit_quaternion()) {
       SOPHUS_ENSURE(scale >= Constants<Scalar>::epsilon(),
                     "Scale factor must be greater-equal epsilon.");
       SOPHUS_ENSURE(scale < Scalar(1.) / Constants<Scalar>::epsilon(),
                     "Inverse scale factor must be greater-equal epsilon.");
       using std::sqrt;
       quaternion_.coeffs() *= sqrt(scale);
     }
   
     template <class D>
     SOPHUS_FUNC explicit RxSO3(Eigen::QuaternionBase<D> const& quat)
         : quaternion_(quat) {
       static_assert(std::is_same<typename D::Scalar, Scalar>::value,
                     "must be same Scalar type.");
       SOPHUS_ENSURE(quaternion_.squaredNorm() >= Constants<Scalar>::epsilon(),
                     "Scale factor must be greater-equal epsilon.");
       SOPHUS_ENSURE(
           quat.squaredNorm() < Scalar(1.) / Constants<Scalar>::epsilon(),
           "Inverse scale factor must be greater-equal epsilon.");
     }
   
     template <class D>
     SOPHUS_FUNC explicit RxSO3(Scalar const& scale,
                                Eigen::QuaternionBase<D> const& unit_quat)
         : RxSO3(scale, SO3<Scalar>(unit_quat)) {}
   
     SOPHUS_FUNC QuaternionMember const& quaternion() const { return quaternion_; }
   
     SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF>
     Dx_exp_x_at_0() {
       static Scalar const h(0.5);
       return h * Sophus::Matrix<Scalar, num_parameters, DoF>::Identity();
     }
   
     SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF> Dx_exp_x(
         const Tangent& a) {
       using std::exp;
       using std::sqrt;
       Sophus::Matrix<Scalar, num_parameters, DoF> J;
       Vector3<Scalar> const omega = a.template head<3>();
       Scalar const sigma = a[3];
       Eigen::Quaternion<Scalar> quat = SO3<Scalar>::exp(omega).unit_quaternion();
       Scalar const scale = sqrt(exp(sigma));
       Scalar const scale_half = scale * Scalar(0.5);
   
       J.template block<4, 3>(0, 0) = SO3<Scalar>::Dx_exp_x(omega) * scale;
       J.col(3) = quat.coeffs() * scale_half;
       return J;
     }
   
     SOPHUS_FUNC static Sophus::Matrix<Scalar, 3, DoF> Dx_exp_x_times_point_at_0(
         Point const& point) {
       Sophus::Matrix<Scalar, 3, DoF> j;
       j << Sophus::SO3<Scalar>::hat(-point), point;
       return j;
     }
   
     SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) {
       return generator(i);
     }
     SOPHUS_FUNC static RxSO3<Scalar> exp(Tangent const& a) {
       Scalar theta;
       return expAndTheta(a, &theta);
     }
   
     SOPHUS_FUNC static RxSO3<Scalar> expAndTheta(Tangent const& a,
                                                  Scalar* theta) {
       SOPHUS_ENSURE(theta != nullptr, "must not be nullptr.");
       using std::exp;
       using std::max;
       using std::min;
       using std::sqrt;
   
       Vector3<Scalar> const omega = a.template head<3>();
       Scalar sigma = a[3];
       Scalar scale = exp(sigma);
       // Ensure that scale-factor constraint is always valid
       scale = max(scale, Constants<Scalar>::epsilonPlus());
       scale = min(scale, Scalar(1.) / Constants<Scalar>::epsilonPlus());
       Scalar sqrt_scale = sqrt(scale);
       Eigen::Quaternion<Scalar> quat =
           SO3<Scalar>::expAndTheta(omega, theta).unit_quaternion();
       quat.coeffs() *= sqrt_scale;
       return RxSO3<Scalar>(quat);
     }
   
     SOPHUS_FUNC static Transformation generator(int i) {
       SOPHUS_ENSURE(i >= 0 && i <= 3, "i should be in range [0,3].");
       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(3), -a(2),  a(1),
             a(2),  a(3), -a(0),
            -a(1),  a(0),  a(3);
       // clang-format on
       return A;
     }
   
     SOPHUS_FUNC static Tangent lieBracket(Tangent const& a, Tangent const& b) {
       Vector3<Scalar> const omega1 = a.template head<3>();
       Vector3<Scalar> const omega2 = b.template head<3>();
       Vector4<Scalar> res;
       res.template head<3>() = omega1.cross(omega2);
       res[3] = Scalar(0);
       return res;
     }
   
     template <class UniformRandomBitGenerator>
     static RxSO3 sampleUniform(UniformRandomBitGenerator& generator) {
       std::uniform_real_distribution<Scalar> uniform(Scalar(-1), Scalar(1));
       using std::exp2;
       return RxSO3(exp2(uniform(generator)),
                    SO3<Scalar>::sampleUniform(generator));
     }
   
     SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
       using std::abs;
       return Tangent(Omega(2, 1), Omega(0, 2), Omega(1, 0), Omega(0, 0));
     }
   
    protected:
     SOPHUS_FUNC QuaternionMember& quaternion_nonconst() { return quaternion_; }
   
     QuaternionMember quaternion_;
   };
   
   }  // namespace Sophus
   
   namespace Eigen {
   
   template <class Scalar_, int Options>
   class Map<Sophus::RxSO3<Scalar_>, Options>
       : public Sophus::RxSO3Base<Map<Sophus::RxSO3<Scalar_>, Options>> {
    public:
     using Base = Sophus::RxSO3Base<Map<Sophus::RxSO3<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::RxSO3Base<Map<Sophus::RxSO3<Scalar_>, Options>>;
   
     using Base::operator=;
     using Base::operator*=;
     using Base::operator*;
   
     SOPHUS_FUNC explicit Map(Scalar* coeffs) : quaternion_(coeffs) {}
   
     SOPHUS_FUNC
     Map<Eigen::Quaternion<Scalar>, Options> const& quaternion() const {
       return quaternion_;
     }
   
    protected:
     SOPHUS_FUNC Map<Eigen::Quaternion<Scalar>, Options>& quaternion_nonconst() {
       return quaternion_;
     }
   
     Map<Eigen::Quaternion<Scalar>, Options> quaternion_;
   };
   
   template <class Scalar_, int Options>
   class Map<Sophus::RxSO3<Scalar_> const, Options>
       : public Sophus::RxSO3Base<Map<Sophus::RxSO3<Scalar_> const, Options>> {
    public:
     using Base = Sophus::RxSO3Base<Map<Sophus::RxSO3<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) : quaternion_(coeffs) {}
   
     SOPHUS_FUNC
     Map<Eigen::Quaternion<Scalar> const, Options> const& quaternion() const {
       return quaternion_;
     }
   
    protected:
     Map<Eigen::Quaternion<Scalar> const, Options> const quaternion_;
   };
   }  // namespace Eigen