7 template <
class Scalar_,
int Options = 0>
16 template <
class Scalar_,
int Options>
17 struct traits<
Sophus::SE2<Scalar_, Options>> {
23 template <
class Scalar_,
int Options>
24 struct traits<Map<
Sophus::SE2<Scalar_>, Options>>
25 : traits<Sophus::SE2<Scalar_, Options>> {
28 using SO2Type = Map<Sophus::SO2<Scalar>, Options>;
31 template <
class Scalar_,
int Options>
32 struct traits<Map<
Sophus::SE2<Scalar_> const, Options>>
33 : traits<Sophus::SE2<Scalar_, Options> const> {
36 using SO2Type = Map<Sophus::SO2<Scalar>
const, Options>;
53 template <
class Derived>
56 using Scalar =
typename Eigen::internal::traits<Derived>::Scalar;
58 typename Eigen::internal::traits<Derived>::TranslationType;
59 using SO2Type =
typename Eigen::internal::traits<Derived>::SO2Type;
63 static int constexpr DoF = 3;
66 static int constexpr num_parameters = 4;
68 static int constexpr N = 3;
84 res.template topLeftCorner<2, 2>() = R;
85 res(0, 2) = translation()[1];
86 res(1, 2) = -translation()[0];
92 template <
class NewScalarType>
95 translation().template cast<NewScalarType>());
130 homogenious_matrix.template topLeftCorner<2, 3>() = matrix2x3();
131 homogenious_matrix.row(2) =
133 return homogenious_matrix;
140 matrix.template topLeftCorner<2, 2>() = rotationMatrix();
141 matrix.col(2) = translation();
147 template <
class OtherDerived>
171 return so2() * p + translation();
178 so2() *= other.
so2();
185 return so2().matrix();
193 return so2().setComplex(complex);
201 so2().setComplex(
Scalar(0.5) * (R(0, 0) + R(1, 1)),
202 Scalar(0.5) * (R(1, 0) - R(0, 1)));
214 return static_cast<Derived const*
>(
this)->so2();
221 return static_cast<Derived*
>(
this)->translation();
228 return static_cast<Derived const*
>(
this)->translation();
234 typename Eigen::internal::traits<Derived>::SO2Type::Complex
const&
236 return so2().unit_complex();
256 res << zero, theta2, -upsilon2[1], -theta2, zero, upsilon2[0], zero, zero,
276 Scalar sin_theta_by_theta;
277 Scalar one_minus_cos_theta_by_theta;
280 Scalar theta_sq = theta * theta;
281 sin_theta_by_theta =
Scalar(1.) -
Scalar(1. / 6.) * theta_sq;
282 one_minus_cos_theta_by_theta =
283 Scalar(0.5) * theta -
Scalar(1. / 24.) * theta * theta_sq;
286 one_minus_cos_theta_by_theta =
290 sin_theta_by_theta * a[0] - one_minus_cos_theta_by_theta * a[1],
291 one_minus_cos_theta_by_theta * a[0] + sin_theta_by_theta * a[1]);
313 SOPHUS_ENSURE(i >= 0 || i <= 2,
"i should be in range [0,2].");
335 Omega.col(2).template head<2>() = a.template head<2>();
354 return Tangent(-theta1 * upsilon2[1] + theta2 * upsilon1[1],
355 theta1 * upsilon2[0] - theta2 * upsilon1[0],
Scalar(0));
372 upsilon_theta[2] = theta;
374 Scalar halftheta_by_tan_of_halftheta;
379 halftheta_by_tan_of_halftheta =
382 halftheta_by_tan_of_halftheta = -(halftheta * z.y()) / (real_minus_one);
385 V_inv << halftheta_by_tan_of_halftheta, halftheta, -halftheta,
386 halftheta_by_tan_of_halftheta;
387 upsilon_theta.template head<2>() = V_inv * other.
translation();
388 return upsilon_theta;
407 "Omega: \n%", Omega);
409 upsilon_omega.template head<2>() = Omega.col(2).template head<2>();
411 return upsilon_omega;
416 template <
class Scalar_,
int Options>
417 class SE2 :
public SE2Base<SE2<Scalar_, Options>> {
429 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
437 template <
class OtherDerived>
439 : so2_(other.so2()), translation_(other.translation()) {
441 "must be same Scalar type");
446 template <
class OtherDerived,
class D>
448 Eigen::MatrixBase<D>
const& translation)
449 : so2_(so2), translation_(translation) {
451 "must be same Scalar type");
453 "must be same Scalar type");
462 Point const& translation)
463 : so2_(rotation_matrix), translation_(translation) {}
468 : so2_(theta), translation_(translation) {}
474 : so2_(complex), translation_(translation) {}
482 : so2_(T.template topLeftCorner<2, 2>().eval()),
483 translation_(T.template block<2, 1>(0, 2)) {}
487 template <
class T0,
class T1>
557 template <
class Scalar_,
int Options>
569 EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
570 using Base::operator*=;
571 using Base::operator*;
576 translation_(coeffs +
Sophus::SO2<
Scalar>::num_parameters) {}
601 Map<Sophus::SO2<Scalar>, Options>
so2_;
608 template <
class Scalar_,
int Options>
609 class Map<
Sophus::SE2<Scalar_> const, Options>
620 EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
621 using Base::operator*=;
622 using Base::operator*;
626 translation_(coeffs +
Sophus::SO2<
Scalar>::num_parameters) {}
642 Map<Sophus::SO2<Scalar>
const, Options>
const so2_;
SOPHUS_FUNC SE2< NewScalarType > cast() const
SOPHUS_FUNC SE2< Scalar > operator*(SE2< Scalar > const &other) const
typename Base::Transformation Transformation
SOPHUS_FUNC Map< Sophus::SO2< Scalar >, Options > const & so2() const
SOPHUS_FUNC SO2Member const & so2() const
Map< Sophus::SO2< Scalar > const, Options > SO2Type
#define SOPHUS_ENSURE(expr, description,...)
static SOPHUS_FUNC SE2 transX(Scalar const &x)
Map< Sophus::SO2< Scalar > const, Options > const so2_
static SOPHUS_FUNC Transformation hat(Tangent const &a)
SOPHUS_FUNC SE2(Vector2< Scalar > const &complex, Point const &translation)
SOPHUS_FUNC void setComplex(Sophus::Vector2< Scalar > const &complex)
typename Base::Adjoint Adjoint
static SOPHUS_FUNC SO2< Scalar > exp(Tangent const &theta)
static SOPHUS_FUNC SE2 transY(Scalar const &y)
SOPHUS_FUNC TranslationMember & translation()
Map< Sophus::SO2< Scalar >, Options > so2_
SOPHUS_FUNC Map(Scalar *coeffs)
Matrix< Scalar, N, N > Transformation
T value(details::expression_node< T > *n)
SOPHUS_FUNC Adjoint Adj() const
Sophus::Vector2< Scalar, Options > TranslationType
typename Base::Point Point
static SOPHUS_FUNC Tangent log(SE2< Scalar > const &other)
SOPHUS_FUNC SE2Base< Derived > & operator=(SE2Base< OtherDerived > const &other)
typename Base::Adjoint Adjoint
Map< Sophus::Vector2< Scalar >, Options > translation_
SOPHUS_FUNC SE2(Transformation const &T)
SOPHUS_FUNC Tangent log() const
SOPHUS_FUNC Map< Sophus::Vector2< Scalar >, Options > & translation()
typename Base::Tangent Tangent
SOPHUS_FUNC Map< Sophus::SO2< Scalar >, Options > & so2()
SOPHUS_FUNC SE2Base< Derived > & operator*=(SE2< Scalar > const &other)
SOPHUS_FUNC Map(Scalar const *coeffs)
static SOPHUS_FUNC Transformation d_lieBracketab_by_d_a(Tangent const &b)
Vector< Scalar, 2, Options > Vector2
SOPHUS_FUNC Map< Sophus::Vector2< Scalar > const, Options > const & translation() const
SOPHUS_FUNC TranslationType const & translation() const
Map< Sophus::Vector2< Scalar > const, Options > const translation_
SOPHUS_FUNC SE2(typename SO2< Scalar >::Transformation const &rotation_matrix, Point const &translation)
SOPHUS_FUNC Matrix< Scalar, 2, 3 > matrix2x3() const
SOPHUS_FUNC TranslationType & translation()
Eigen::Matrix< Scalar, M, N > Matrix
Vector< Scalar, DoF > Tangent
typename Base::Transformation Transformation
typename Base::Transformation Transformation
typename Base::Point Point
static SOPHUS_FUNC SE2< Scalar > exp(Tangent const &a)
typename Base::Point Point
typename Base::Transformation Transformation
static SOPHUS_FUNC Transformation generator(int i)
SOPHUS_FUNC SO2Type const & so2() const
SOPHUS_FUNC SO2Member & so2()
SOPHUS_FUNC Scalar log() const
SOPHUS_FUNC Matrix< Scalar, 2, 2 > rotationMatrix() const
SOPHUS_FUNC void normalize()
Normalize SO2 element.
SOPHUS_FUNC TranslationMember const & translation() const
SOPHUS_FUNC Transformation matrix() const
SOPHUS_FUNC Point operator*(Point const &p) const
typename Eigen::internal::traits< Map< Sophus::SE2< Scalar_ >, Options > >::TranslationType TranslationType
typename Base::Tangent Tangent
const e_voc T1 e_none const e_none T1 e_none T0
typename Base::Tangent Tangent
static SOPHUS_FUNC Tangent vee(Transformation const &Omega)
TranslationMember translation_
SOPHUS_FUNC SE2< Scalar > inverse() const
SOPHUS_FUNC Eigen::internal::traits< Derived >::SO2Type::Complex const & unit_complex() const
SOPHUS_FUNC SE2(Scalar const &theta, Point const &translation)
SOPHUS_FUNC Scalar const * data() const
Eigen::Matrix< Scalar, M, 1, Options > Vector
Vector2< Scalar, Options > TranslationMember
static SOPHUS_FUNC SE2 trans(T0 const &x, T1 const &y)
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)
static SOPHUS_FUNC SE2 rot(Scalar const &x)
SOPHUS_FUNC Map< Sophus::Vector2< Scalar >, Options > const & translation() const
static SOPHUS_FUNC Tangent vee(Transformation const &Omega)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW SOPHUS_FUNC SE2()
SOPHUS_FUNC void setRotationMatrix(Matrix< Scalar, 2, 2 > const &R)
SOPHUS_FUNC ComplexMember const & unit_complex() const
SOPHUS_FUNC Scalar * data()
typename Eigen::internal::traits< Map< Sophus::SE2< Scalar_ >, Options > >::Scalar Scalar
Map< Sophus::Vector2< Scalar > const, Options > TranslationType
SOPHUS_FUNC SE2(SO2Base< OtherDerived > const &so2, Eigen::MatrixBase< D > const &translation)
SOPHUS_FUNC SO2< Scalar > inverse() const
static SOPHUS_FUNC Tangent lieBracket(Tangent const &a, Tangent const &b)
Matrix< Scalar, DoF, DoF > Adjoint
SOPHUS_FUNC SE2(SE2Base< OtherDerived > const &other)
typename Base::Adjoint Adjoint
SOPHUS_FUNC SO2Type & so2()
SOPHUS_FUNC Map< Sophus::SO2< Scalar > const, Options > const & so2() const
typename Eigen::internal::traits< Map< Sophus::SE2< Scalar_ >, Options > >::SO2Type SO2Type