1 #ifndef SOPHUS_SIM3_HPP 2 #define SOPHUS_SIM3_HPP 7 template <
class Scalar_,
int Options = 0>
16 template <
class Scalar_,
int Options>
17 struct traits<
Sophus::Sim3<Scalar_, Options>> {
23 template <
class Scalar_,
int Options>
24 struct traits<Map<
Sophus::Sim3<Scalar_>, Options>>
25 : traits<Sophus::Sim3<Scalar_, Options>> {
28 using RxSO3Type = Map<Sophus::RxSO3<Scalar>, Options>;
31 template <
class Scalar_,
int Options>
32 struct traits<Map<
Sophus::Sim3<Scalar_> const, Options>>
33 : traits<Sophus::Sim3<Scalar_, Options> const> {
36 using RxSO3Type = Map<Sophus::RxSO3<Scalar>
const, Options>;
55 template <
class Derived>
58 using Scalar =
typename Eigen::internal::traits<Derived>::Scalar;
60 typename Eigen::internal::traits<Derived>::TranslationType;
61 using RxSO3Type =
typename Eigen::internal::traits<Derived>::RxSO3Type;
66 static int constexpr DoF = 7;
69 static int constexpr num_parameters = 7;
71 static int constexpr N = 4;
87 res.block(0, 0, 3, 3) = scale() * R;
89 res.block(0, 6, 3, 1) = -translation();
90 res.block(3, 3, 3, 3) = R;
97 template <
class NewScalarType>
100 translation().template cast<NewScalarType>());
128 homogenious_matrix.template topLeftCorner<3, 4>() = matrix3x4();
129 homogenious_matrix.row(3) =
131 return homogenious_matrix;
138 matrix.template topLeftCorner<3, 3>() = rxso3().matrix();
139 matrix.col(3) = translation();
145 template <
class OtherDerived>
148 rxso3() = other.
rxso3();
173 return rxso3() * p + translation();
180 rxso3() *= other.
rxso3();
189 rxso3().setQuaternion(quat);
195 return rxso3().quaternion();
201 return rxso3().rotationMatrix();
207 return static_cast<Derived*
>(
this)->rxso3();
213 return static_cast<Derived const*
>(
this)->rxso3();
223 rxso3().setRotationMatrix(R);
239 rxso3().setScaledRotationMatrix(sR);
245 return static_cast<Derived*
>(
this)->translation();
251 return static_cast<Derived const*
>(
this)->translation();
268 Scalar const sigma2 = b[6];
272 res.template topLeftCorner<3, 3>() =
275 res.template topRightCorner<3, 1>() = upsilon2;
300 Scalar const sigma = a[6];
310 static Scalar const one(1);
311 static Scalar const half(0.5);
321 Scalar theta_sq = theta * theta;
322 A = (one - cos(theta)) / theta_sq;
323 B = (theta - sin(theta)) / (theta_sq * theta);
326 C = (scale - one) / sigma;
328 Scalar sigma_sq = sigma * sigma;
329 A = ((sigma - one) * scale + one) / sigma_sq;
330 B = ((half * sigma * sigma - sigma + one) * scale) / (sigma_sq * sigma);
332 Scalar theta_sq = theta * theta;
333 Scalar a = scale * sin(theta);
334 Scalar b = scale * cos(theta);
335 Scalar c = theta_sq + sigma * sigma;
336 A = (a * sigma + (one - b) * theta) / (theta * c);
337 B = (C - ((b - one) * sigma + a * theta) / (c)) * one / (theta_sq);
386 SOPHUS_ENSURE(i >= 0 || i <= 6,
"i should be in range [0,6].");
406 Omega.template topLeftCorner<3, 3>() =
408 Omega.col(3).template head<3>() = a.template head<3>();
409 Omega.row(3).setZero();
433 sigma1 * upsilon2 - sigma2 * upsilon1;
434 res.template segment<3>(3) = omega1.cross(omega2);
463 Scalar sigma = omega_sigma[3];
469 static Scalar const half(0.5);
470 static Scalar const one(1);
471 static Scalar const two(2);
475 Scalar const scale_sq = scale * scale;
476 Scalar const theta_sq = theta * theta;
477 Scalar const sin_theta = sin(theta);
478 Scalar const cos_theta = cos(theta);
482 c = one - half * sigma;
487 b = (theta * sin_theta + two * cos_theta - two) /
488 (two * theta_sq * (cos_theta - one));
491 Scalar const scale_cu = scale_sq * scale;
492 c = sigma / (scale - one);
494 a = (-sigma * scale + scale - one) / ((scale - one) * (scale - one));
495 b = (scale_sq * sigma - two * scale_sq + scale * sigma + two * scale) /
496 (two * scale_cu -
Scalar(6) * scale_sq +
Scalar(6) * scale - two);
498 Scalar const s_sin_theta = scale * sin_theta;
499 Scalar const s_cos_theta = scale * cos_theta;
500 a = (theta * s_cos_theta - theta - sigma * s_sin_theta) /
501 (theta * (scale_sq - two * s_cos_theta + one));
503 (theta * s_sin_theta - theta * sin_theta + sigma * s_cos_theta -
504 scale * sigma + sigma * cos_theta - sigma) /
505 (theta_sq * (scale_cu - two * scale * s_cos_theta - scale_sq +
506 two * s_cos_theta + scale - one));
512 res.segment(3, 3) = omega;
533 upsilon_omega_sigma.template head<3>() = Omega.col(3).template head<3>();
534 upsilon_omega_sigma.template tail<4>() =
536 return upsilon_omega_sigma;
541 template <
class Scalar_,
int Options>
554 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
562 template <
class OtherDerived>
564 : rxso3_(other.rxso3()), translation_(other.translation()) {
566 "must be same Scalar type");
571 template <
class OtherDerived,
class D>
573 Eigen::MatrixBase<D>
const& translation)
574 : rxso3_(rxso3), translation_(translation) {
576 "must be same Scalar type");
578 "must be same Scalar type");
585 template <
class D1,
class D2>
587 Eigen::MatrixBase<D2>
const& translation)
588 : rxso3_(quaternion), translation_(translation) {
590 "must be same Scalar type");
592 "must be same Scalar type");
601 : rxso3_(T.template topLeftCorner<3, 3>()),
602 translation_(T.template block<3, 1>(0, 3)) {}
611 return rxso3_.data();
618 return rxso3_.data();
651 template <
class Scalar_,
int Options>
652 class Map<
Sophus::Sim3<Scalar_>, Options>
663 EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
664 using Base::operator*=;
665 using Base::operator*;
669 translation_(coeffs +
Sophus::RxSO3<
Scalar>::num_parameters) {}
693 Map<Sophus::RxSO3<Scalar>, Options>
rxso3_;
700 template <
class Scalar_,
int Options>
701 class Map<
Sophus::Sim3<Scalar_> const, Options>
712 EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
713 using Base::operator*=;
714 using Base::operator*;
718 translation_(coeffs +
Sophus::RxSO3<
Scalar>::num_parameters) {}
734 Map<Sophus::RxSO3<Scalar>
const, Options>
const rxso3_;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW SOPHUS_FUNC Sim3()
Map< Sophus::RxSO3< Scalar > const, Options > RxSO3Type
SOPHUS_FUNC Sim3Base< Derived > & operator*=(Sim3< Scalar > const &other)
SOPHUS_FUNC RxSo3Member const & rxso3() const
SOPHUS_FUNC void setScaledRotationMatrix(Matrix3< Scalar > const &sR)
SOPHUS_FUNC Sim3(Eigen::QuaternionBase< D1 > const &quaternion, Eigen::MatrixBase< D2 > const &translation)
SOPHUS_FUNC Sim3(Sim3Base< OtherDerived > const &other)
typename Base::Adjoint Adjoint
SOPHUS_FUNC RxSO3< Scalar > inverse() const
SOPHUS_FUNC Matrix3< Scalar > rotationMatrix() const
SOPHUS_FUNC TranslationMember & translation()
#define SOPHUS_ENSURE(expr, description,...)
Map< Sophus::Vector3< Scalar > const, Options > const translation_
SOPHUS_FUNC Map< Sophus::RxSO3< Scalar > const, Options > const & rxso3() const
static SOPHUS_FUNC Transformation hat(Tangent const &omega)
static SOPHUS_FUNC Sim3< Scalar > exp(Tangent const &a)
SOPHUS_FUNC Map< Sophus::Vector3< Scalar >, Options > & translation()
SOPHUS_FUNC void setQuaternion(Eigen::Quaternion< Scalar > const &quat)
static SOPHUS_FUNC Transformation generator(int i)
SOPHUS_FUNC Map< Sophus::Vector3< Scalar >, Options > const & translation() const
typename Eigen::internal::traits< Sim3< Scalar_, Options > >::TranslationType TranslationType
SOPHUS_FUNC Sim3(Matrix< Scalar, 4, 4 > const &T)
typename Base::Adjoint Adjoint
Matrix< Scalar, DoF, DoF > Adjoint
SOPHUS_FUNC RxSO3Type const & rxso3() const
T value(details::expression_node< T > *n)
Vector< Scalar, DoF > Tangent
SOPHUS_FUNC void setRotationMatrix(Matrix3< Scalar > &R)
Map< Sophus::RxSO3< Scalar >, Options > rxso3_
SOPHUS_FUNC Sim3< Scalar > inverse() const
SOPHUS_FUNC Scalar scale() const
typename Base::Tangent Tangent
SOPHUS_FUNC TranslationType const & translation() const
typename Base::Tangent Tangent
typename Base::Point Point
static SOPHUS_FUNC Tangent logAndTheta(RxSO3< Scalar > const &other, Scalar *theta)
SOPHUS_FUNC QuaternionType const & quaternion() const
Eigen::Matrix< Scalar, M, N > Matrix
TranslationMember translation_
static SOPHUS_FUNC Scalar epsilon()
SOPHUS_FUNC Sim3(RxSO3Base< OtherDerived > const &rxso3, Eigen::MatrixBase< D > const &translation)
SOPHUS_FUNC Map< Sophus::RxSO3< Scalar >, Options > const & rxso3() const
SOPHUS_FUNC Map(Scalar *coeffs)
typename Base::Transformation Transformation
SOPHUS_FUNC Map< Sophus::Vector3< Scalar > const, Options > const & translation() const
SOPHUS_FUNC TranslationType & translation()
static SOPHUS_FUNC Adjoint d_lieBracketab_by_d_a(Tangent const &b)
SOPHUS_FUNC Sim3< Scalar > operator*(Sim3< Scalar > const &other) const
typename Eigen::internal::traits< Sim3< Scalar_, Options > >::RxSO3Type RxSO3Type
Vector< Scalar, 3, Options > Vector3
typename Base::Adjoint Adjoint
SOPHUS_FUNC Scalar * data()
SOPHUS_FUNC Sim3< NewScalarType > cast() const
typename Base::Point Point
SOPHUS_FUNC Matrix< Scalar, 3, 4 > matrix3x4() const
SOPHUS_FUNC Point operator*(Point const &p) const
static SOPHUS_FUNC Transformation hat(Tangent const &a)
SOPHUS_FUNC void setScale(Scalar const &scale)
SOPHUS_FUNC TranslationMember const & translation() const
Matrix< Scalar, N, N > Transformation
SOPHUS_FUNC Adjoint Adj() const
Eigen::Matrix< Scalar, M, 1, Options > Vector
Matrix< Scalar, 3, 3 > Matrix3
Map< Sophus::Vector3< Scalar > const, Options > TranslationType
Map< Sophus::Vector3< Scalar >, Options > translation_
Vector3< Scalar, Options > TranslationMember
static SOPHUS_FUNC Tangent log(Sim3< Scalar > const &other)
SOPHUS_FUNC RxSO3Type & rxso3()
static SOPHUS_FUNC Tangent vee(Transformation const &Omega)
typename Base::Transformation Transformation
typename RxSO3Type::QuaternionType QuaternionType
SOPHUS_FUNC RxSo3Member & rxso3()
SOPHUS_FUNC Sim3Base< Derived > & operator=(Sim3Base< OtherDerived > const &other)
typename Base::Transformation Transformation
typename Base::Tangent Tangent
typename Eigen::internal::traits< Sim3< Scalar_, Options > >::Scalar Scalar
SOPHUS_FUNC Map(Scalar const *coeffs)
SOPHUS_FUNC Scalar const * data() const
Vector< Scalar, 4 > Vector4
SOPHUS_FUNC Scalar scale() const
typename Base::Point Point
static SOPHUS_FUNC Tangent lieBracket(Tangent const &a, Tangent const &b)
SOPHUS_FUNC Transformation matrix() const
Sophus::Vector3< Scalar, Options > TranslationType
SOPHUS_FUNC Map< Sophus::RxSO3< Scalar >, Options > & rxso3()
static SOPHUS_FUNC RxSO3< Scalar > expAndTheta(Tangent const &a, Scalar *theta)
SOPHUS_FUNC Tangent log() const
Map< Sophus::RxSO3< Scalar > const, Options > const rxso3_