.. _program_listing_file_sophus_sim3.hpp:

Program Listing for File sim3.hpp
=================================

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

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

.. code-block:: cpp

   
   #pragma once
   
   #include "rxso3.hpp"
   #include "sim_details.hpp"
   
   namespace Sophus {
   template <class Scalar_, int Options = 0>
   class Sim3;
   using Sim3d = Sim3<double>;
   using Sim3f = Sim3<float>;
   }  // namespace Sophus
   
   namespace Eigen {
   namespace internal {
   
   template <class Scalar_, int Options>
   struct traits<Sophus::Sim3<Scalar_, Options>> {
     using Scalar = Scalar_;
     using TranslationType = Sophus::Vector3<Scalar, Options>;
     using RxSO3Type = Sophus::RxSO3<Scalar, Options>;
   };
   
   template <class Scalar_, int Options>
   struct traits<Map<Sophus::Sim3<Scalar_>, Options>>
       : traits<Sophus::Sim3<Scalar_, Options>> {
     using Scalar = Scalar_;
     using TranslationType = Map<Sophus::Vector3<Scalar>, Options>;
     using RxSO3Type = Map<Sophus::RxSO3<Scalar>, Options>;
   };
   
   template <class Scalar_, int Options>
   struct traits<Map<Sophus::Sim3<Scalar_> const, Options>>
       : traits<Sophus::Sim3<Scalar_, Options> const> {
     using Scalar = Scalar_;
     using TranslationType = Map<Sophus::Vector3<Scalar> const, Options>;
     using RxSO3Type = Map<Sophus::RxSO3<Scalar> const, Options>;
   };
   }  // namespace internal
   }  // namespace Eigen
   
   namespace Sophus {
   
   template <class Derived>
   class Sim3Base {
    public:
     using Scalar = typename Eigen::internal::traits<Derived>::Scalar;
     using TranslationType =
         typename Eigen::internal::traits<Derived>::TranslationType;
     using RxSO3Type = typename Eigen::internal::traits<Derived>::RxSO3Type;
     using QuaternionType = typename RxSO3Type::QuaternionType;
   
     static int constexpr DoF = 7;
     static int constexpr num_parameters = 7;
     static int constexpr N = 4;
     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>;
   
     template <typename OtherDerived>
     using ReturnScalar = typename Eigen::ScalarBinaryOpTraits<
         Scalar, typename OtherDerived::Scalar>::ReturnType;
   
     template <typename OtherDerived>
     using Sim3Product = Sim3<ReturnScalar<OtherDerived>>;
   
     template <typename PointDerived>
     using PointProduct = Vector3<ReturnScalar<PointDerived>>;
   
     template <typename HPointDerived>
     using HomogeneousPointProduct = Vector4<ReturnScalar<HPointDerived>>;
   
     SOPHUS_FUNC Adjoint Adj() const {
       Matrix3<Scalar> const R = rxso3().rotationMatrix();
       Adjoint res;
       res.setZero();
       res.template block<3, 3>(0, 0) = rxso3().matrix();
       res.template block<3, 3>(0, 3) = SO3<Scalar>::hat(translation()) * R;
       res.template block<3, 1>(0, 6) = -translation();
   
       res.template block<3, 3>(3, 3) = R;
   
       res(6, 6) = Scalar(1);
       return res;
     }
   
     template <class NewScalarType>
     SOPHUS_FUNC Sim3<NewScalarType> cast() const {
       return Sim3<NewScalarType>(rxso3().template cast<NewScalarType>(),
                                  translation().template cast<NewScalarType>());
     }
   
     SOPHUS_FUNC Sim3<Scalar> inverse() const {
       RxSO3<Scalar> invR = rxso3().inverse();
       return Sim3<Scalar>(invR, invR * (translation() * Scalar(-1)));
     }
   
     SOPHUS_FUNC Tangent log() const {
       // The derivation of the closed-form Sim(3) logarithm for is done
       // analogously to the closed-form solution of the SE(3) logarithm, see
       // J. Gallier, D. Xu, "Computing exponentials of skew symmetric matrices
       // and logarithms of orthogonal matrices", IJRA 2002.
       // https:///pdfs.semanticscholar.org/cfe3/e4b39de63c8cabd89bf3feff7f5449fc981d.pdf
       // (Sec. 6., pp. 8)
       Tangent res;
       auto omega_sigma_and_theta = rxso3().logAndTheta();
       Vector3<Scalar> const omega =
           omega_sigma_and_theta.tangent.template head<3>();
       Scalar sigma = omega_sigma_and_theta.tangent[3];
       Matrix3<Scalar> const Omega = SO3<Scalar>::hat(omega);
       Matrix3<Scalar> const W_inv = details::calcWInv<Scalar, 3>(
           Omega, omega_sigma_and_theta.theta, sigma, scale());
   
       res.segment(0, 3) = W_inv * translation();
       res.segment(3, 3) = omega;
       res[6] = sigma;
       return res;
     }
   
     SOPHUS_FUNC Transformation matrix() const {
       Transformation homogeneous_matrix;
       homogeneous_matrix.template topLeftCorner<3, 4>() = matrix3x4();
       homogeneous_matrix.row(3) =
           Matrix<Scalar, 4, 1>(Scalar(0), Scalar(0), Scalar(0), Scalar(1));
       return homogeneous_matrix;
     }
   
     SOPHUS_FUNC Matrix<Scalar, 3, 4> matrix3x4() const {
       Matrix<Scalar, 3, 4> matrix;
       matrix.template topLeftCorner<3, 3>() = rxso3().matrix();
       matrix.col(3) = translation();
       return matrix;
     }
   
     template <class OtherDerived>
     SOPHUS_FUNC Sim3Base<Derived>& operator=(
         Sim3Base<OtherDerived> const& other) {
       rxso3() = other.rxso3();
       translation() = other.translation();
       return *this;
     }
   
     template <typename OtherDerived>
     SOPHUS_FUNC Sim3Product<OtherDerived> operator*(
         Sim3Base<OtherDerived> const& other) const {
       return Sim3Product<OtherDerived>(
           rxso3() * other.rxso3(), translation() + rxso3() * other.translation());
     }
   
     template <typename PointDerived,
               typename = typename std::enable_if<
                   IsFixedSizeVector<PointDerived, 3>::value>::type>
     SOPHUS_FUNC PointProduct<PointDerived> operator*(
         Eigen::MatrixBase<PointDerived> const& p) const {
       return rxso3() * p + translation();
     }
   
     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 PointProduct<HPointDerived> tp =
           rxso3() * p.template head<3>() + p(3) * translation();
       return HomogeneousPointProduct<HPointDerived>(tp(0), tp(1), tp(2), p(3));
     }
   
     SOPHUS_FUNC Line operator*(Line const& l) const {
       Line rotatedLine = rxso3() * l;
       return Line(rotatedLine.origin() + translation(), rotatedLine.direction());
     }
   
     SOPHUS_FUNC Hyperplane operator*(Hyperplane const& p) const {
       Hyperplane const rotated = rxso3() * p;
       return Hyperplane(rotated.normal(),
                         rotated.offset() - translation().dot(rotated.normal()));
     }
   
     template <typename OtherDerived,
               typename = typename std::enable_if<
                   std::is_same<Scalar, ReturnScalar<OtherDerived>>::value>::type>
     SOPHUS_FUNC Sim3Base<Derived>& operator*=(
         Sim3Base<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.template block<4, 3>(0, 0).setZero();
       J.template block<4, 4>(0, 3) = rxso3().Dx_this_mul_exp_x_at_0();
       J.template block<3, 3>(4, 0) = rxso3().matrix();
       J.template block<3, 4>(4, 3).setZero();
   
       return J;
     }
   
     SOPHUS_FUNC Matrix<Scalar, DoF, num_parameters> Dx_log_this_inv_by_x_at_this()
         const {
       Matrix<Scalar, DoF, num_parameters> J;
       J.template block<3, 4>(0, 0).setZero();
       J.template block<3, 3>(0, 4) = rxso3().inverse().matrix();
       J.template block<4, 4>(3, 0) = rxso3().Dx_log_this_inv_by_x_at_this();
       J.template block<4, 3>(3, 4).setZero();
       return J;
     }
   
     SOPHUS_FUNC Sophus::Vector<Scalar, num_parameters> params() const {
       Sophus::Vector<Scalar, num_parameters> p;
       p << rxso3().params(), translation();
       return p;
     }
   
     SOPHUS_FUNC void setQuaternion(Eigen::Quaternion<Scalar> const& quat) {
       rxso3().setQuaternion(quat);
     }
   
     SOPHUS_FUNC QuaternionType const& quaternion() const {
       return rxso3().quaternion();
     }
   
     SOPHUS_FUNC Matrix3<Scalar> rotationMatrix() const {
       return rxso3().rotationMatrix();
     }
   
     SOPHUS_FUNC RxSO3Type& rxso3() {
       return static_cast<Derived*>(this)->rxso3();
     }
   
     SOPHUS_FUNC RxSO3Type const& rxso3() const {
       return static_cast<Derived const*>(this)->rxso3();
     }
   
     SOPHUS_FUNC Scalar scale() const { return rxso3().scale(); }
   
     SOPHUS_FUNC void setRotationMatrix(Matrix3<Scalar>& R) {
       rxso3().setRotationMatrix(R);
     }
   
     SOPHUS_FUNC void setScale(Scalar const& scale) { rxso3().setScale(scale); }
   
     SOPHUS_FUNC void setScaledRotationMatrix(Matrix3<Scalar> const& sR) {
       rxso3().setScaledRotationMatrix(sR);
     }
   
     SOPHUS_FUNC TranslationType& translation() {
       return static_cast<Derived*>(this)->translation();
     }
   
     SOPHUS_FUNC TranslationType const& translation() const {
       return static_cast<Derived const*>(this)->translation();
     }
   };
   
   template <class Scalar_, int Options>
   class Sim3 : public Sim3Base<Sim3<Scalar_, Options>> {
    public:
     using Base = Sim3Base<Sim3<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 RxSo3Member = RxSO3<Scalar, Options>;
     using TranslationMember = Vector3<Scalar, Options>;
   
     using Base::operator=;
   
     SOPHUS_FUNC Sim3& operator=(Sim3 const& other) = default;
   
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
   
     SOPHUS_FUNC Sim3();
   
     SOPHUS_FUNC Sim3(Sim3 const& other) = default;
   
     template <class OtherDerived>
     SOPHUS_FUNC Sim3(Sim3Base<OtherDerived> const& other)
         : rxso3_(other.rxso3()), translation_(other.translation()) {
       static_assert(std::is_same<typename OtherDerived::Scalar, Scalar>::value,
                     "must be same Scalar type");
     }
   
     template <class OtherDerived, class D>
     SOPHUS_FUNC explicit Sim3(RxSO3Base<OtherDerived> const& rxso3,
                               Eigen::MatrixBase<D> const& translation)
         : rxso3_(rxso3), translation_(translation) {
       static_assert(std::is_same<typename OtherDerived::Scalar, Scalar>::value,
                     "must be same Scalar type");
       static_assert(std::is_same<typename D::Scalar, Scalar>::value,
                     "must be same Scalar type");
     }
   
     template <class D1, class D2>
     SOPHUS_FUNC explicit Sim3(Eigen::QuaternionBase<D1> const& quaternion,
                               Eigen::MatrixBase<D2> const& translation)
         : rxso3_(quaternion), translation_(translation) {
       static_assert(std::is_same<typename D1::Scalar, Scalar>::value,
                     "must be same Scalar type");
       static_assert(std::is_same<typename D2::Scalar, Scalar>::value,
                     "must be same Scalar type");
     }
   
     template <class D1, class D2>
     SOPHUS_FUNC explicit Sim3(Scalar const& scale,
                               Eigen::QuaternionBase<D1> const& unit_quaternion,
                               Eigen::MatrixBase<D2> const& translation)
         : Sim3(RxSO3<Scalar>(scale, unit_quaternion), translation) {}
   
     SOPHUS_FUNC explicit Sim3(Matrix<Scalar, 4, 4> const& T)
         : rxso3_(T.template topLeftCorner<3, 3>()),
           translation_(T.template block<3, 1>(0, 3)) {}
   
     SOPHUS_FUNC Scalar* data() {
       // rxso3_ and translation_ are laid out sequentially with no padding
       return rxso3_.data();
     }
   
     SOPHUS_FUNC Scalar const* data() const {
       // rxso3_ and translation_ are laid out sequentially with no padding
       return rxso3_.data();
     }
   
     SOPHUS_FUNC RxSo3Member& rxso3() { return rxso3_; }
   
     SOPHUS_FUNC RxSo3Member const& rxso3() const { return rxso3_; }
   
     SOPHUS_FUNC TranslationMember& translation() { return translation_; }
   
     SOPHUS_FUNC TranslationMember const& translation() const {
       return translation_;
     }
   
     SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF>
     Dx_exp_x_at_0() {
       Sophus::Matrix<Scalar, num_parameters, DoF> J;
       J.template block<4, 3>(0, 0).setZero();
       J.template block<4, 4>(0, 3) = RxSO3<Scalar>::Dx_exp_x_at_0();
       J.template block<3, 3>(4, 0).setIdentity();
       J.template block<3, 4>(4, 3).setZero();
       return J;
     }
   
     SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF> Dx_exp_x(
         const Tangent& a) {
       Sophus::Matrix<Scalar, num_parameters, DoF> J;
   
       static Matrix3<Scalar> const I = Matrix3<Scalar>::Identity();
       Vector3<Scalar> const omega = a.template segment<3>(3);
       Vector3<Scalar> const upsilon = a.template head<3>();
       Scalar const sigma = a[6];
       Scalar const theta = omega.norm();
   
       Matrix3<Scalar> const Omega = SO3<Scalar>::hat(omega);
       Matrix3<Scalar> const Omega2 = Omega * Omega;
       Vector3<Scalar> theta_domega;
       if (theta < Constants<Scalar>::epsilon()) {
         theta_domega = Vector3<Scalar>::Zero();
       } else {
         theta_domega = omega / theta;
       }
       static Matrix3<Scalar> const Omega_domega[3] = {
           SO3<Scalar>::hat(Vector3<Scalar>::Unit(0)),
           SO3<Scalar>::hat(Vector3<Scalar>::Unit(1)),
           SO3<Scalar>::hat(Vector3<Scalar>::Unit(2))};
   
       Matrix3<Scalar> const Omega2_domega[3] = {
           Omega_domega[0] * Omega + Omega * Omega_domega[0],
           Omega_domega[1] * Omega + Omega * Omega_domega[1],
           Omega_domega[2] * Omega + Omega * Omega_domega[2]};
   
       Matrix3<Scalar> const W = details::calcW<Scalar, 3>(Omega, theta, sigma);
   
       J.template block<4, 3>(0, 0).setZero();
       J.template block<4, 4>(0, 3) =
           RxSO3<Scalar>::Dx_exp_x(a.template tail<4>());
       J.template block<3, 4>(4, 3).setZero();
       J.template block<3, 3>(4, 0) = W;
   
       Scalar A, B, C, A_dtheta, B_dtheta, A_dsigma, B_dsigma, C_dsigma;
       details::calcW_derivatives(theta, sigma, A, B, C, A_dsigma, B_dsigma,
                                  C_dsigma, A_dtheta, B_dtheta);
   
       for (int i = 0; i < 3; ++i) {
         J.template block<3, 1>(4, 3 + i) =
             (A_dtheta * theta_domega[i] * Omega + A * Omega_domega[i] +
              B_dtheta * theta_domega[i] * Omega2 + B * Omega2_domega[i]) *
             upsilon;
       }
   
       J.template block<3, 1>(4, 6) =
           (A_dsigma * Omega + B_dsigma * Omega2 + C_dsigma * I) * upsilon;
   
       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::Matrix3<Scalar>::Identity(),
           Sophus::RxSO3<Scalar>::Dx_exp_x_times_point_at_0(point);
       return J;
     }
   
     SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) {
       return generator(i);
     }
   
     SOPHUS_FUNC static Sim3<Scalar> exp(Tangent const& a) {
       // For the derivation of the exponential map of Sim(3) see
       // H. Strasdat, "Local Accuracy and Global Consistency for Efficient Visual
       // SLAM", PhD thesis, 2012.
       // http:///hauke.strasdat.net/files/strasdat_thesis_2012.pdf (A.5, pp. 186)
       Vector3<Scalar> const upsilon = a.segment(0, 3);
       Vector3<Scalar> const omega = a.segment(3, 3);
       Scalar const sigma = a[6];
       Scalar theta;
       RxSO3<Scalar> rxso3 =
           RxSO3<Scalar>::expAndTheta(a.template tail<4>(), &theta);
       Matrix3<Scalar> const Omega = SO3<Scalar>::hat(omega);
   
       Matrix3<Scalar> const W = details::calcW<Scalar, 3>(Omega, theta, sigma);
       return Sim3<Scalar>(rxso3, W * upsilon);
     }
   
     SOPHUS_FUNC static Transformation generator(int i) {
       SOPHUS_ENSURE(i >= 0 || i <= 6, "i should be in range [0,6].");
       Tangent e;
       e.setZero();
       e[i] = Scalar(1);
       return hat(e);
     }
   
     SOPHUS_FUNC static Transformation hat(Tangent const& a) {
       Transformation Omega;
       Omega.template topLeftCorner<3, 3>() =
           RxSO3<Scalar>::hat(a.template tail<4>());
       Omega.col(3).template head<3>() = a.template head<3>();
       Omega.row(3).setZero();
       return Omega;
     }
   
     SOPHUS_FUNC static Tangent lieBracket(Tangent const& a, Tangent const& b) {
       Vector3<Scalar> const upsilon1 = a.template head<3>();
       Vector3<Scalar> const upsilon2 = b.template head<3>();
       Vector3<Scalar> const omega1 = a.template segment<3>(3);
       Vector3<Scalar> const omega2 = b.template segment<3>(3);
       Scalar sigma1 = a[6];
       Scalar sigma2 = b[6];
   
       Tangent res;
       res.template head<3>() = SO3<Scalar>::hat(omega1) * upsilon2 +
                                SO3<Scalar>::hat(upsilon1) * omega2 +
                                sigma1 * upsilon2 - sigma2 * upsilon1;
       res.template segment<3>(3) = omega1.cross(omega2);
       res[6] = Scalar(0);
   
       return res;
     }
   
     template <class UniformRandomBitGenerator>
     static Sim3 sampleUniform(UniformRandomBitGenerator& generator) {
       std::uniform_real_distribution<Scalar> uniform(Scalar(-1), Scalar(1));
       return Sim3(RxSO3<Scalar>::sampleUniform(generator),
                   Vector3<Scalar>(uniform(generator), uniform(generator),
                                   uniform(generator)));
     }
   
     SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
       Tangent upsilon_omega_sigma;
       upsilon_omega_sigma.template head<3>() = Omega.col(3).template head<3>();
       upsilon_omega_sigma.template tail<4>() =
           RxSO3<Scalar>::vee(Omega.template topLeftCorner<3, 3>());
       return upsilon_omega_sigma;
     }
   
    protected:
     RxSo3Member rxso3_;
     TranslationMember translation_;
   };
   
   template <class Scalar, int Options>
   SOPHUS_FUNC Sim3<Scalar, Options>::Sim3()
       : translation_(TranslationMember::Zero()) {
     static_assert(std::is_standard_layout<Sim3>::value,
                   "Assume standard layout for the use of offset of check below.");
     static_assert(
         offsetof(Sim3, rxso3_) + sizeof(Scalar) * RxSO3<Scalar>::num_parameters ==
             offsetof(Sim3, translation_),
         "This class assumes packed storage and hence will only work "
         "correctly depending on the compiler (options) - in "
         "particular when using [this->data(), this-data() + "
         "num_parameters] to access the raw data in a contiguous fashion.");
   }
   
   }  // namespace Sophus
   
   namespace Eigen {
   
   template <class Scalar_, int Options>
   class Map<Sophus::Sim3<Scalar_>, Options>
       : public Sophus::Sim3Base<Map<Sophus::Sim3<Scalar_>, Options>> {
    public:
     using Base = Sophus::Sim3Base<Map<Sophus::Sim3<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 Base::operator=;
     using Base::operator*=;
     using Base::operator*;
   
     SOPHUS_FUNC explicit Map(Scalar* coeffs)
         : rxso3_(coeffs),
           translation_(coeffs + Sophus::RxSO3<Scalar>::num_parameters) {}
   
     SOPHUS_FUNC Map<Sophus::RxSO3<Scalar>, Options>& rxso3() { return rxso3_; }
   
     SOPHUS_FUNC Map<Sophus::RxSO3<Scalar>, Options> const& rxso3() const {
       return rxso3_;
     }
   
     SOPHUS_FUNC Map<Sophus::Vector3<Scalar>, Options>& translation() {
       return translation_;
     }
   
     SOPHUS_FUNC Map<Sophus::Vector3<Scalar>, Options> const& translation() const {
       return translation_;
     }
   
    protected:
     Map<Sophus::RxSO3<Scalar>, Options> rxso3_;
     Map<Sophus::Vector3<Scalar>, Options> translation_;
   };
   
   template <class Scalar_, int Options>
   class Map<Sophus::Sim3<Scalar_> const, Options>
       : public Sophus::Sim3Base<Map<Sophus::Sim3<Scalar_> const, Options>> {
     using Base = Sophus::Sim3Base<Map<Sophus::Sim3<Scalar_> const, 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;
   
     using Base::operator*=;
     using Base::operator*;
   
     SOPHUS_FUNC explicit Map(Scalar const* coeffs)
         : rxso3_(coeffs),
           translation_(coeffs + Sophus::RxSO3<Scalar>::num_parameters) {}
   
     SOPHUS_FUNC Map<Sophus::RxSO3<Scalar> const, Options> const& rxso3() const {
       return rxso3_;
     }
   
     SOPHUS_FUNC Map<Sophus::Vector3<Scalar> const, Options> const& translation()
         const {
       return translation_;
     }
   
    protected:
     Map<Sophus::RxSO3<Scalar> const, Options> const rxso3_;
     Map<Sophus::Vector3<Scalar> const, Options> const translation_;
   };
   }  // namespace Eigen