7 template <
class Scalar_,
int Options = 0>
16 template <
class Scalar_,
int Options>
17 struct traits<
Sophus::SE3<Scalar_, Options>> {
23 template <
class Scalar_,
int Options>
24 struct traits<Map<
Sophus::SE3<Scalar_>, Options>>
25 : traits<Sophus::SE3<Scalar_, Options>> {
28 using SO3Type = Map<Sophus::SO3<Scalar>, Options>;
31 template <
class Scalar_,
int Options>
32 struct traits<Map<
Sophus::SE3<Scalar_> const, Options>>
33 : traits<Sophus::SE3<Scalar_, Options> const> {
36 using SO3Type = Map<Sophus::SO3<Scalar>
const, Options>;
54 template <
class Derived>
57 using Scalar =
typename Eigen::internal::traits<Derived>::Scalar;
59 typename Eigen::internal::traits<Derived>::TranslationType;
60 using SO3Type =
typename Eigen::internal::traits<Derived>::SO3Type;
64 static int constexpr DoF = 6;
67 static int constexpr num_parameters = 7;
69 static int constexpr N = 4;
83 res.block(0, 0, 3, 3) = R;
84 res.block(3, 3, 3, 3) = R;
92 template <
class NewScalarType>
95 translation().template cast<NewScalarType>());
104 Eigen::Quaternion<Scalar> internal_gen_q;
107 internalGenerator(i, &internal_gen_q, &internal_gen_t);
109 res.template head<4>() =
110 (so3().unit_quaternion() * internal_gen_q).coeffs();
111 res.template tail<3>() = so3().unit_quaternion() * internal_gen_t;
119 for (
int i = 0; i < DoF; ++i) {
120 J.col(i) = internalMultiplyByGenerator(i);
157 homogenious_matrix.template topLeftCorner<3, 4>() = matrix3x4();
158 homogenious_matrix.row(3) =
160 return homogenious_matrix;
167 matrix.template topLeftCorner<3, 3>() = rotationMatrix();
168 matrix.col(3) = translation();
174 template <
class OtherDerived>
198 return so3() * p + translation();
205 so3() *= other.
so3();
220 return static_cast<const Derived*
>(
this)->so3();
228 so3().setQuaternion(quat);
236 so3().setQuaternion(Eigen::Quaternion<Scalar>(rotation_matrix));
242 return static_cast<Derived*
>(
this)->translation();
248 return static_cast<Derived const*
>(
this)->translation();
254 return this->so3().unit_quaternion();
308 Scalar theta_sq = theta * theta;
310 (
Scalar(1) - cos(theta)) / (theta_sq)*Omega +
311 (theta - sin(theta)) / (theta_sq * theta) * Omega_sq);
353 SOPHUS_ENSURE(i >= 0 && i <= 5,
"i should be in range [0,5].");
365 int i, Eigen::Quaternion<Scalar>* internal_gen_q,
367 SOPHUS_ENSURE(i >= 0 && i <= 5,
"i should be in range [0,5]");
369 "internal_gen_q must not be the null pointer");
371 "internal_gen_t must not be the null pointer");
373 internal_gen_q->coeffs().setZero();
374 internal_gen_t->setZero();
376 (*internal_gen_t)[i] =
Scalar(1);
397 Omega.template topLeftCorner<3, 3>() =
399 Omega.col(3).template head<3>() = a.template head<3>();
419 res.template head<3>() = omega1.cross(upsilon2) + upsilon1.cross(omega2);
420 res.template tail<3>() = omega1.cross(omega2);
446 upsilon_omega.template tail<3>() =
454 Scalar(1. / 12.) * (Omega * Omega);
456 upsilon_omega.template head<3>() = V_inv * se3.
translation();
463 theta * cos(half_theta) / (
Scalar(2) * sin(half_theta))) /
464 (theta * theta) * (Omega * Omega));
465 upsilon_omega.template head<3>() = V_inv * se3.
translation();
467 return upsilon_omega;
486 upsilon_omega.template head<3>() = Omega.col(3).template head<3>();
487 upsilon_omega.template tail<3>() =
489 return upsilon_omega;
494 template <
class Scalar_,
int Options>
495 class SE3 :
public SE3Base<SE3<Scalar_, Options>> {
507 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
515 template <
class OtherDerived>
517 : so3_(other.so3()), translation_(other.translation()) {
519 "must be same Scalar type");
524 template <
class OtherDerived,
class D>
526 Eigen::MatrixBase<D>
const& translation)
527 : so3_(so3), translation_(translation) {
529 "must be same Scalar type");
531 "must be same Scalar type");
540 : so3_(rotation_matrix), translation_(translation) {}
547 Point const& translation)
548 : so3_(quaternion), translation_(translation) {}
556 : so3_(T.template topLeftCorner<3, 3>()),
557 translation_(T.template block<3, 1>(0, 3)) {
560 "Last row is not (0,0,0,1), but (%).", T.row(3));
565 template <
class T0,
class T1,
class T2>
653 template <
class Scalar_,
int Options>
665 EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
666 using Base::operator*=;
667 using Base::operator*;
671 translation_(coeffs +
Sophus::SO3<
Scalar>::num_parameters) {}
696 Map<Sophus::SO3<Scalar>, Options>
so3_;
703 template <
class Scalar_,
int Options>
704 class Map<
Sophus::SE3<Scalar_> const, Options>
715 EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
716 using Base::operator*=;
717 using Base::operator*;
721 translation_(coeffs +
Sophus::SO3<
Scalar>::num_parameters) {}
737 Map<Sophus::SO3<Scalar>
const, Options>
const so3_;
typename Base::Point Point
typename Base::Transformation Transformation
typename Base::Tangent Tangent
SOPHUS_FUNC Vector< Scalar, num_parameters > internalMultiplyByGenerator(int i) const
Matrix< Scalar, 4, 4 > Matrix4
static SOPHUS_FUNC Transformation generator(int i)
SOPHUS_FUNC Map< Sophus::Vector3< Scalar > > const & translation() const
#define SOPHUS_ENSURE(expr, description,...)
typename Base::Adjoint Adjoint
SOPHUS_FUNC void setQuaternion(Eigen::Quaternion< Scalar > const &quat)
static SOPHUS_FUNC Transformation hat(Tangent const &omega)
SOPHUS_FUNC Matrix3< Scalar > rotationMatrix() const
SOPHUS_FUNC void normalize()
SOPHUS_FUNC Transformation matrix() const
typename Base::Transformation Transformation
static SOPHUS_FUNC Transformation hat(Tangent const &a)
Map< Sophus::Vector3< Scalar > const, Options > const translation_
SOPHUS_FUNC Point operator*(Point const &p) const
SOPHUS_FUNC SE3(Matrix4< Scalar > const &T)
SOPHUS_FUNC Matrix< Scalar, num_parameters, DoF > internalJacobian() const
SOPHUS_FUNC Map(Scalar *coeffs)
typename Base::Point Point
SOPHUS_FUNC Scalar const * data() const
SOPHUS_FUNC TranslationType const & translation() const
SOPHUS_FUNC SO3< Scalar > inverse() const
SOPHUS_FUNC SE3(Eigen::Quaternion< Scalar > const &quaternion, Point const &translation)
T value(details::expression_node< T > *n)
SOPHUS_FUNC SO3Type const & so3() const
typename Base::Transformation Transformation
SOPHUS_FUNC Map(Scalar const *coeffs)
SOPHUS_FUNC SO3Type & so3()
Vector< Scalar, DoF > Tangent
SOPHUS_FUNC void setRotationMatrix(Matrix3< Scalar > const &rotation_matrix)
static SOPHUS_FUNC SE3 transX(Scalar const &x)
SOPHUS_FUNC TranslationMember const & translation() const
EIGEN_MAKE_ALIGNED_OPERATOR_NEW SOPHUS_FUNC SE3()
static SOPHUS_FUNC SE3 transZ(Scalar const &z)
static SOPHUS_FUNC SE3 rotX(Scalar const &x)
static SOPHUS_FUNC SE3 rotZ(Scalar const &z)
static SOPHUS_FUNC Tangent vee(Transformation const &Omega)
SOPHUS_FUNC SE3< Scalar > operator*(SE3< Scalar > const &other) const
Eigen::Matrix< Scalar, M, N > Matrix
TranslationMember translation_
static SOPHUS_FUNC Scalar epsilon()
SOPHUS_FUNC Scalar * data()
Map< Sophus::SO3< Scalar >, Options > so3_
static SOPHUS_FUNC void internalGenerator(int i, Eigen::Quaternion< Scalar > *internal_gen_q)
typename SO3Type::QuaternionType QuaternionType
Matrix< Scalar, DoF, DoF > Adjoint
Vector< Scalar, 3, Options > Vector3
Map< Sophus::Vector3< Scalar > const, Options > TranslationType
SOPHUS_FUNC Tangent log() const
SOPHUS_FUNC Adjoint Adj() const
SOPHUS_FUNC SE3(SO3Base< OtherDerived > const &so3, Eigen::MatrixBase< D > const &translation)
const e_voc T1 e_none const e_none T1 e_none T0
SOPHUS_FUNC SE3(Matrix3< Scalar > const &rotation_matrix, Point const &translation)
Matrix< Scalar, N, N > Transformation
SOPHUS_FUNC Map< Sophus::SO3< Scalar >, Options > & so3()
SOPHUS_FUNC Map< Sophus::SO3< Scalar >, Options > const & so3() const
static SOPHUS_FUNC void internalGenerator(int i, Eigen::Quaternion< Scalar > *internal_gen_q, Vector3< Scalar > *internal_gen_t)
SOPHUS_FUNC SE3Base< Derived > & operator=(SE3Base< OtherDerived > const &other)
static SOPHUS_FUNC SE3 transY(Scalar const &y)
typename Base::Tangent Tangent
SOPHUS_FUNC Map< Sophus::Vector3< Scalar > const, Options > const & translation() const
SOPHUS_FUNC QuaternionType const & unit_quaternion() const
SOPHUS_FUNC Transformation matrix() const
static SOPHUS_FUNC Adjoint d_lieBracketab_by_d_a(Tangent const &b)
Eigen::Matrix< Scalar, M, 1, Options > Vector
Matrix< Scalar, 3, 3 > Matrix3
Map< Sophus::SO3< Scalar > const, Options > SO3Type
SOPHUS_FUNC SO3Member & so3()
SOPHUS_FUNC SO3Member const & so3() const
static SOPHUS_FUNC SE3 trans(T0 const &x, T1 const &y, T2 const &z)
x y t t *t x y t t t x y t t t x *y t *t t x *y t *t t x y t t t x y t t t x(y+z)
SOPHUS_FUNC TranslationType & translation()
typename Base::Point Point
static SOPHUS_FUNC Tangent lieBracket(Tangent const &a, Tangent const &b)
SOPHUS_FUNC SE3(SE3Base< OtherDerived > const &other)
Map< Sophus::SO3< Scalar > const, Options > const so3_
typename Eigen::internal::traits< SE3< Scalar_, Options > >::Scalar Scalar
static SOPHUS_FUNC Tangent log(SE3< Scalar > const &se3)
SOPHUS_FUNC SE3< NewScalarType > cast() const
static SOPHUS_FUNC SE3 rotY(Scalar const &y)
static SOPHUS_FUNC SE3< Scalar > exp(Tangent const &a)
typename Base::Adjoint Adjoint
Map< Sophus::Vector3< Scalar >, Options > translation_
SOPHUS_FUNC TranslationMember & translation()
typename Base::Tangent Tangent
SOPHUS_FUNC Matrix< Scalar, 3, 4 > matrix3x4() const
static SOPHUS_FUNC SO3< Scalar > expAndTheta(Tangent const &omega, Scalar *theta)
SOPHUS_FUNC SE3< Scalar > inverse() const
SOPHUS_FUNC Map< Sophus::SO3< Scalar > const, Options > const & so3() const
SOPHUS_FUNC Map< Sophus::Vector3< Scalar > > & translation()
typename Base::Adjoint Adjoint
typename Eigen::internal::traits< SE3< Scalar_, Options > >::TranslationType TranslationType
typename Eigen::internal::traits< SE3< Scalar_, Options > >::SO3Type SO3Type
Sophus::Vector3< Scalar, Options > TranslationType
SOPHUS_FUNC SE3Base< Derived > & operator*=(SE3< Scalar > const &other)
Vector3< Scalar, Options > TranslationMember