.. _program_listing_file_sophus_so3.hpp:

Program Listing for File so3.hpp
================================

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

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

.. code-block:: cpp

   
   #pragma once
   
   #include "rotation_matrix.hpp"
   #include "so2.hpp"
   #include "types.hpp"
   
   // Include only the selective set of Eigen headers that we need.
   // This helps when using Sophus with unusual compilers, like nvcc.
   #include <Eigen/src/Geometry/OrthoMethods.h>
   #include <Eigen/src/Geometry/Quaternion.h>
   #include <Eigen/src/Geometry/RotationBase.h>
   
   namespace Sophus {
   template <class Scalar_, int Options = 0>
   class SO3;
   using SO3d = SO3<double>;
   using SO3f = SO3<float>;
   }  // namespace Sophus
   
   namespace Eigen {
   namespace internal {
   
   template <class Scalar_, int Options_>
   struct traits<Sophus::SO3<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::SO3<Scalar_>, Options_>>
       : traits<Sophus::SO3<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::SO3<Scalar_> const, Options_>>
       : traits<Sophus::SO3<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 SO3Base {
    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 = 3;
     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 SO3Product = SO3<ReturnScalar<OtherDerived>>;
   
     template <typename PointDerived>
     using PointProduct = Vector3<ReturnScalar<PointDerived>>;
   
     template <typename HPointDerived>
     using HomogeneousPointProduct = Vector4<ReturnScalar<HPointDerived>>;
   
     //
     //
     SOPHUS_FUNC Adjoint Adj() const { return matrix(); }
   
     template <class S = Scalar>
     SOPHUS_FUNC enable_if_t<std::is_floating_point<S>::value, S> angleX() const {
       Sophus::Matrix3<Scalar> R = matrix();
       Sophus::Matrix2<Scalar> Rx = R.template block<2, 2>(1, 1);
       return SO2<Scalar>(makeRotationMatrix(Rx)).log();
     }
   
     template <class S = Scalar>
     SOPHUS_FUNC enable_if_t<std::is_floating_point<S>::value, S> angleY() const {
       Sophus::Matrix3<Scalar> R = matrix();
       Sophus::Matrix2<Scalar> Ry;
       // clang-format off
       Ry <<
         R(0, 0), R(2, 0),
         R(0, 2), R(2, 2);
       // clang-format on
       return SO2<Scalar>(makeRotationMatrix(Ry)).log();
     }
   
     template <class S = Scalar>
     SOPHUS_FUNC enable_if_t<std::is_floating_point<S>::value, S> angleZ() const {
       Sophus::Matrix3<Scalar> R = matrix();
       Sophus::Matrix2<Scalar> Rz = R.template block<2, 2>(0, 0);
       return SO2<Scalar>(makeRotationMatrix(Rz)).log();
     }
   
     template <class NewScalarType>
     SOPHUS_FUNC SO3<NewScalarType> cast() const {
       return SO3<NewScalarType>(unit_quaternion().template cast<NewScalarType>());
     }
   
     SOPHUS_FUNC Scalar* data() {
       return unit_quaternion_nonconst().coeffs().data();
     }
   
     SOPHUS_FUNC Scalar const* data() const {
       return unit_quaternion().coeffs().data();
     }
   
     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 = unit_quaternion();
       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 = unit_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();
       // clang-format on
       return J * Scalar(2.);
     }
   
     SOPHUS_FUNC Sophus::Vector<Scalar, num_parameters> params() const {
       return unit_quaternion().coeffs();
     }
   
     SOPHUS_FUNC SO3<Scalar> inverse() const {
       return SO3<Scalar>(unit_quaternion().conjugate());
     }
   
     SOPHUS_FUNC Tangent log() const { return logAndTheta().tangent; }
   
     SOPHUS_FUNC TangentAndTheta logAndTheta() const {
       TangentAndTheta J;
       using std::abs;
       using std::atan2;
       using std::sqrt;
       Scalar squared_n = unit_quaternion().vec().squaredNorm();
       Scalar w = unit_quaternion().w();
   
       Scalar two_atan_nbyw_by_n;
   
   
       if (squared_n <
           Constants<Scalar>::epsilon() * Constants<Scalar>::epsilon()) {
         // If quaternion is normalized and n=0, then w should be 1;
         // w=0 should never happen here!
         SOPHUS_ENSURE(abs(w) >= Constants<Scalar>::epsilon(),
                       "Quaternion ({}) should be normalized!",
                       SOPHUS_FMT_ARG(unit_quaternion().coeffs().transpose()));
         Scalar squared_w = w * w;
         two_atan_nbyw_by_n =
             Scalar(2) / w - Scalar(2.0 / 3.0) * (squared_n) / (w * squared_w);
         J.theta = Scalar(2) * squared_n / w;
       } else {
         Scalar n = sqrt(squared_n);
   
         // w < 0 ==> cos(theta/2) < 0 ==> theta > pi
         //
         // By convention, the condition |theta| < pi is imposed by wrapping theta
         // to pi; The wrap operation can be folded inside evaluation of atan2
         //
         // theta - pi = atan(sin(theta - pi), cos(theta - pi))
         //            = atan(-sin(theta), -cos(theta))
         //
         Scalar atan_nbyw =
             (w < Scalar(0)) ? Scalar(atan2(-n, -w)) : Scalar(atan2(n, w));
         two_atan_nbyw_by_n = Scalar(2) * atan_nbyw / n;
         J.theta = two_atan_nbyw_by_n * n;
       }
   
       J.tangent = two_atan_nbyw_by_n * unit_quaternion().vec();
       return J;
     }
   
     SOPHUS_FUNC void normalize() {
       Scalar length = unit_quaternion_nonconst().norm();
       SOPHUS_ENSURE(
           length >= Constants<Scalar>::epsilon(),
           "Quaternion ({}) should not be close to zero!",
           SOPHUS_FMT_ARG(unit_quaternion_nonconst().coeffs().transpose()));
       unit_quaternion_nonconst().coeffs() /= length;
     }
   
     SOPHUS_FUNC Transformation matrix() const {
       return unit_quaternion().toRotationMatrix();
     }
   
     template <class OtherDerived>
     SOPHUS_FUNC SO3Base<Derived>& operator=(SO3Base<OtherDerived> const& other) {
       unit_quaternion_nonconst() = other.unit_quaternion();
       return *this;
     }
   
     template <typename QuaternionProductType, typename QuaternionTypeA,
               typename QuaternionTypeB>
     SOPHUS_FUNC static QuaternionProductType QuaternionProduct(
         const QuaternionTypeA& a, const QuaternionTypeB& b) {
       return QuaternionProductType(
           a.w() * b.w() - a.x() * b.x() - a.y() * b.y() - a.z() * b.z(),
           a.w() * b.x() + a.x() * b.w() + a.y() * b.z() - a.z() * b.y(),
           a.w() * b.y() + a.y() * b.w() + a.z() * b.x() - a.x() * b.z(),
           a.w() * b.z() + a.z() * b.w() + a.x() * b.y() - a.y() * b.x());
     }
   
     template <typename OtherDerived>
     SOPHUS_FUNC SO3Product<OtherDerived> operator*(
         SO3Base<OtherDerived> const& other) const {
       using QuaternionProductType =
           typename SO3Product<OtherDerived>::QuaternionType;
       const QuaternionType& a = unit_quaternion();
       const typename OtherDerived::QuaternionType& b = other.unit_quaternion();
       return SO3Product<OtherDerived>(
           QuaternionProduct<QuaternionProductType>(a, b));
     }
   
     template <typename PointDerived,
               typename = typename std::enable_if<
                   IsFixedSizeVector<PointDerived, 3>::value>::type>
     SOPHUS_FUNC PointProduct<PointDerived> operator*(
         Eigen::MatrixBase<PointDerived> const& p) const {
       const QuaternionType& q = unit_quaternion();
       PointProduct<PointDerived> uv = q.vec().cross(p);
       uv += uv;
       return p + q.w() * uv + q.vec().cross(uv);
     }
   
     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 rp = *this * p.template head<3>();
       return HomogeneousPointProduct<HPointDerived>(rp(0), rp(1), rp(2), p(3));
     }
   
     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 SO3Base<Derived>& operator*=(SO3Base<OtherDerived> const& other) {
       *static_cast<Derived*>(this) = *this * other;
       return *this;
     }
   
     SOPHUS_FUNC void setQuaternion(Eigen::Quaternion<Scalar> const& quaternion) {
       unit_quaternion_nonconst() = quaternion;
       normalize();
     }
   
     SOPHUS_FUNC QuaternionType const& unit_quaternion() const {
       return static_cast<Derived const*>(this)->unit_quaternion();
     }
   
    private:
     SOPHUS_FUNC QuaternionType& unit_quaternion_nonconst() {
       return static_cast<Derived*>(this)->unit_quaternion_nonconst();
     }
   };
   
   template <class Scalar_, int Options>
   class SO3 : public SO3Base<SO3<Scalar_, Options>> {
    public:
     using Base = SO3Base<SO3<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 SO3Base<SO3<Scalar, Options>>;
   
     using Base::operator=;
   
     SOPHUS_FUNC SO3& operator=(SO3 const& other) = default;
   
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
   
     SOPHUS_FUNC SO3()
         : unit_quaternion_(Scalar(1), Scalar(0), Scalar(0), Scalar(0)) {}
   
     SOPHUS_FUNC SO3(SO3 const& other) = default;
   
     template <class OtherDerived>
     SOPHUS_FUNC SO3(SO3Base<OtherDerived> const& other)
         : unit_quaternion_(other.unit_quaternion()) {}
   
     SOPHUS_FUNC SO3(Transformation const& R) : unit_quaternion_(R) {
       SOPHUS_ENSURE(isOrthogonal(R), "R is not orthogonal:\n {}",
                     SOPHUS_FMT_ARG(R * R.transpose()));
       SOPHUS_ENSURE(R.determinant() > Scalar(0), "det(R) is not positive: {}",
                     SOPHUS_FMT_ARG(R.determinant()));
     }
   
     template <class D>
     SOPHUS_FUNC explicit SO3(Eigen::QuaternionBase<D> const& quat)
         : unit_quaternion_(quat) {
       static_assert(
           std::is_same<typename Eigen::QuaternionBase<D>::Scalar, Scalar>::value,
           "Input must be of same scalar type");
       Base::normalize();
     }
   
     SOPHUS_FUNC QuaternionMember const& unit_quaternion() const {
       return unit_quaternion_;
     }
   
     SOPHUS_FUNC static Sophus::Matrix<Scalar, DoF, DoF> leftJacobian(
         Tangent const& omega, optional<Scalar> const& theta_o = nullopt) {
       using std::cos;
       using std::sin;
       using std::sqrt;
   
       Scalar const theta_sq = theta_o ? *theta_o * *theta_o : omega.squaredNorm();
       Matrix3<Scalar> const Omega = SO3<Scalar>::hat(omega);
       Matrix3<Scalar> const Omega_sq = Omega * Omega;
       Matrix3<Scalar> V;
   
       if (theta_sq <
           Constants<Scalar>::epsilon() * Constants<Scalar>::epsilon()) {
         V = Matrix3<Scalar>::Identity() + Scalar(0.5) * Omega;
       } else {
         Scalar theta = theta_o ? *theta_o : sqrt(theta_sq);
         V = Matrix3<Scalar>::Identity() +
             (Scalar(1) - cos(theta)) / theta_sq * Omega +
             (theta - sin(theta)) / (theta_sq * theta) * Omega_sq;
       }
       return V;
     }
   
     SOPHUS_FUNC static Sophus::Matrix<Scalar, DoF, DoF> leftJacobianInverse(
         Tangent const& omega, optional<Scalar> const& theta_o = nullopt) {
       using std::cos;
       using std::sin;
       using std::sqrt;
       Scalar const theta_sq = theta_o ? *theta_o * *theta_o : omega.squaredNorm();
       Matrix3<Scalar> const Omega = SO3<Scalar>::hat(omega);
   
       Matrix3<Scalar> V_inv;
       if (theta_sq <
           Constants<Scalar>::epsilon() * Constants<Scalar>::epsilon()) {
         V_inv = Matrix3<Scalar>::Identity() - Scalar(0.5) * Omega +
                 Scalar(1. / 12.) * (Omega * Omega);
   
       } else {
         Scalar const theta = theta_o ? *theta_o : sqrt(theta_sq);
         Scalar const half_theta = Scalar(0.5) * theta;
   
         V_inv = Matrix3<Scalar>::Identity() - Scalar(0.5) * Omega +
                 (Scalar(1) -
                  Scalar(0.5) * theta * cos(half_theta) / sin(half_theta)) /
                     (theta * theta) * (Omega * Omega);
       }
       return V_inv;
     }
   
     SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF> Dx_exp_x(
         Tangent const& omega) {
       using std::cos;
       using std::exp;
       using std::sin;
       using std::sqrt;
       Scalar const c0 = omega[0] * omega[0];
       Scalar const c1 = omega[1] * omega[1];
       Scalar const c2 = omega[2] * omega[2];
       Scalar const c3 = c0 + c1 + c2;
   
       if (c3 < Constants<Scalar>::epsilon()) {
         return Dx_exp_x_at_0();
       }
   
       Scalar const c4 = sqrt(c3);
       Scalar const c5 = 1.0 / c4;
       Scalar const c6 = 0.5 * c4;
       Scalar const c7 = sin(c6);
       Scalar const c8 = c5 * c7;
       Scalar const c9 = pow(c3, -3.0L / 2.0L);
       Scalar const c10 = c7 * c9;
       Scalar const c11 = Scalar(1.0) / c3;
       Scalar const c12 = cos(c6);
       Scalar const c13 = Scalar(0.5) * c11 * c12;
       Scalar const c14 = c7 * c9 * omega[0];
       Scalar const c15 = Scalar(0.5) * c11 * c12 * omega[0];
       Scalar const c16 = -c14 * omega[1] + c15 * omega[1];
       Scalar const c17 = -c14 * omega[2] + c15 * omega[2];
       Scalar const c18 = omega[1] * omega[2];
       Scalar const c19 = -c10 * c18 + c13 * c18;
       Scalar const c20 = Scalar(0.5) * c5 * c7;
       Sophus::Matrix<Scalar, num_parameters, DoF> J;
       J(0, 0) = -c0 * c10 + c0 * c13 + c8;
       J(0, 1) = c16;
       J(0, 2) = c17;
       J(1, 0) = c16;
       J(1, 1) = -c1 * c10 + c1 * c13 + c8;
       J(1, 2) = c19;
       J(2, 0) = c17;
       J(2, 1) = c19;
       J(2, 2) = -c10 * c2 + c13 * c2 + c8;
       J(3, 0) = -c20 * omega[0];
       J(3, 1) = -c20 * omega[1];
       J(3, 2) = -c20 * omega[2];
       return J;
     }
   
     SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF>
     Dx_exp_x_at_0() {
       Sophus::Matrix<Scalar, num_parameters, DoF> J;
       // clang-format off
       J <<  Scalar(0.5),   Scalar(0),   Scalar(0),
               Scalar(0), Scalar(0.5),   Scalar(0),
               Scalar(0),   Scalar(0), Scalar(0.5),
               Scalar(0),   Scalar(0),   Scalar(0);
       // clang-format on
       return J;
     }
   
     SOPHUS_FUNC static Sophus::Matrix<Scalar, 3, DoF> Dx_exp_x_times_point_at_0(
         Point const& point) {
       return hat(-point);
     }
   
     SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) {
       return generator(i);
     }
   
     SOPHUS_FUNC static SO3<Scalar> exp(Tangent const& omega) {
       Scalar theta;
       return expAndTheta(omega, &theta);
     }
   
     SOPHUS_FUNC static SO3<Scalar> expAndTheta(Tangent const& omega,
                                                Scalar* theta) {
       SOPHUS_ENSURE(theta != nullptr, "%s", "must not be nullptr.");
       using std::abs;
       using std::cos;
       using std::sin;
       using std::sqrt;
       Scalar theta_sq = omega.squaredNorm();
   
       Scalar imag_factor;
       Scalar real_factor;
       if (theta_sq <
           Constants<Scalar>::epsilon() * Constants<Scalar>::epsilon()) {
         *theta = Scalar(0);
         Scalar theta_po4 = theta_sq * theta_sq;
         imag_factor = Scalar(0.5) - Scalar(1.0 / 48.0) * theta_sq +
                       Scalar(1.0 / 3840.0) * theta_po4;
         real_factor = Scalar(1) - Scalar(1.0 / 8.0) * theta_sq +
                       Scalar(1.0 / 384.0) * theta_po4;
       } else {
         *theta = sqrt(theta_sq);
         Scalar half_theta = Scalar(0.5) * (*theta);
         Scalar sin_half_theta = sin(half_theta);
         imag_factor = sin_half_theta / (*theta);
         real_factor = cos(half_theta);
       }
   
       SO3 q;
       q.unit_quaternion_nonconst() =
           QuaternionMember(real_factor, imag_factor * omega.x(),
                            imag_factor * omega.y(), imag_factor * omega.z());
       SOPHUS_ENSURE(abs(q.unit_quaternion().squaredNorm() - Scalar(1)) <
                         Sophus::Constants<Scalar>::epsilon(),
                     "SO3::exp failed! omega: {}, real: {}, img: {}",
                     SOPHUS_FMT_ARG(omega.transpose()),
                     SOPHUS_FMT_ARG(real_factor), SOPHUS_FMT_ARG(imag_factor));
       return q;
     }
   
     template <class S = Scalar>
     static SOPHUS_FUNC enable_if_t<std::is_floating_point<S>::value, SO3>
     fitToSO3(Transformation const& R) {
       return SO3(::Sophus::makeRotationMatrix(R));
     }
   
     SOPHUS_FUNC static Transformation generator(int i) {
       SOPHUS_ENSURE(i >= 0 && i <= 2, "%s", "i should be in range [0,2].");
       Tangent e;
       e.setZero();
       e[i] = Scalar(1);
       return hat(e);
     }
   
     SOPHUS_FUNC static Transformation hat(Tangent const& omega) {
       Transformation Omega;
       // clang-format off
       Omega <<
           Scalar(0), -omega(2),  omega(1),
            omega(2), Scalar(0), -omega(0),
           -omega(1),  omega(0), Scalar(0);
       // clang-format on
       return Omega;
     }
   
     SOPHUS_FUNC static Tangent lieBracket(Tangent const& omega1,
                                           Tangent const& omega2) {
       return omega1.cross(omega2);
     }
   
     static SOPHUS_FUNC SO3 rotX(Scalar const& x) {
       return SO3::exp(Sophus::Vector3<Scalar>(x, Scalar(0), Scalar(0)));
     }
   
     static SOPHUS_FUNC SO3 rotY(Scalar const& y) {
       return SO3::exp(Sophus::Vector3<Scalar>(Scalar(0), y, Scalar(0)));
     }
   
     static SOPHUS_FUNC SO3 rotZ(Scalar const& z) {
       return SO3::exp(Sophus::Vector3<Scalar>(Scalar(0), Scalar(0), z));
     }
   
     template <class UniformRandomBitGenerator>
     static SO3 sampleUniform(UniformRandomBitGenerator& generator) {
       static_assert(IsUniformRandomBitGenerator<UniformRandomBitGenerator>::value,
                     "generator must meet the UniformRandomBitGenerator concept");
   
       std::uniform_real_distribution<Scalar> uniform(Scalar(0), Scalar(1));
       std::uniform_real_distribution<Scalar> uniform_twopi(
           Scalar(0), 2 * Constants<Scalar>::pi());
   
       const Scalar u1 = uniform(generator);
       const Scalar u2 = uniform_twopi(generator);
       const Scalar u3 = uniform_twopi(generator);
   
       const Scalar a = sqrt(1 - u1);
       const Scalar b = sqrt(u1);
   
       return SO3(
           QuaternionMember(a * sin(u2), a * cos(u2), b * sin(u3), b * cos(u3)));
     }
   
     SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
       return Tangent(Omega(2, 1), Omega(0, 2), Omega(1, 0));
     }
   
    protected:
     SOPHUS_FUNC QuaternionMember& unit_quaternion_nonconst() {
       return unit_quaternion_;
     }
   
     QuaternionMember unit_quaternion_;
   };
   
   }  // namespace Sophus
   
   namespace Eigen {
   template <class Scalar_, int Options>
   class Map<Sophus::SO3<Scalar_>, Options>
       : public Sophus::SO3Base<Map<Sophus::SO3<Scalar_>, Options>> {
    public:
     using Base = Sophus::SO3Base<Map<Sophus::SO3<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::SO3Base<Map<Sophus::SO3<Scalar_>, Options>>;
   
     using Base::operator=;
     using Base::operator*=;
     using Base::operator*;
   
     SOPHUS_FUNC explicit Map(Scalar* coeffs) : unit_quaternion_(coeffs) {}
   
     SOPHUS_FUNC Map<Eigen::Quaternion<Scalar>, Options> const& unit_quaternion()
         const {
       return unit_quaternion_;
     }
   
    protected:
     SOPHUS_FUNC Map<Eigen::Quaternion<Scalar>, Options>&
     unit_quaternion_nonconst() {
       return unit_quaternion_;
     }
   
     Map<Eigen::Quaternion<Scalar>, Options> unit_quaternion_;
   };
   
   template <class Scalar_, int Options>
   class Map<Sophus::SO3<Scalar_> const, Options>
       : public Sophus::SO3Base<Map<Sophus::SO3<Scalar_> const, Options>> {
    public:
     using Base = Sophus::SO3Base<Map<Sophus::SO3<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_quaternion_(coeffs) {}
   
     SOPHUS_FUNC Map<Eigen::Quaternion<Scalar> const, Options> const&
     unit_quaternion() const {
       return unit_quaternion_;
     }
   
    protected:
     Map<Eigen::Quaternion<Scalar> const, Options> const unit_quaternion_;
   };
   }  // namespace Eigen