Main MRPT website > C++ reference for MRPT 1.5.3
se2.hpp
Go to the documentation of this file.
1 #ifndef SOPHUS_SE2_HPP
2 #define SOPHUS_SE2_HPP
3 
4 #include "so2.hpp"
5 
6 namespace Sophus {
7 template <class Scalar_, int Options = 0>
8 class SE2;
9 using SE2d = SE2<double>;
10 using SE2f = SE2<float>;
11 } // namespace Sophus
12 
13 namespace Eigen {
14 namespace internal {
15 
16 template <class Scalar_, int Options>
17 struct traits<Sophus::SE2<Scalar_, Options>> {
18  using Scalar = Scalar_;
21 };
22 
23 template <class Scalar_, int Options>
24 struct traits<Map<Sophus::SE2<Scalar_>, Options>>
25  : traits<Sophus::SE2<Scalar_, Options>> {
26  using Scalar = Scalar_;
27  using TranslationType = Map<Sophus::Vector2<Scalar>, Options>;
28  using SO2Type = Map<Sophus::SO2<Scalar>, Options>;
29 };
30 
31 template <class Scalar_, int Options>
32 struct traits<Map<Sophus::SE2<Scalar_> const, Options>>
33  : traits<Sophus::SE2<Scalar_, Options> const> {
34  using Scalar = Scalar_;
35  using TranslationType = Map<Sophus::Vector2<Scalar> const, Options>;
36  using SO2Type = Map<Sophus::SO2<Scalar> const, Options>;
37 };
38 } // namespace internal
39 } // namespace Eigen
40 
41 namespace Sophus {
42 
43 // SE2 base type - implements SE2 class but is storage agnostic.
44 //
45 // SE(2) is the group of rotations and translation in 2d. It is the semi-direct
46 // product of SO(2) and the 2d Euclidean vector space. The class is represented
47 // using a composition of SO2Group for rotation and a 2-vector for translation.
48 //
49 // SE(2) is neither compact, nor a commutative group.
50 //
51 // See SO2Group for more details of the rotation representation in 2d.
52 //
53 template <class Derived>
54 class SE2Base {
55  public:
56  using Scalar = typename Eigen::internal::traits<Derived>::Scalar;
57  using TranslationType =
58  typename Eigen::internal::traits<Derived>::TranslationType;
59  using SO2Type = typename Eigen::internal::traits<Derived>::SO2Type;
60 
61  // Degrees of freedom of manifold, number of dimensions in tangent space
62  // (two for translation, three for rotation).
63  static int constexpr DoF = 3;
64  // Number of internal parameters used (tuple for complex, two for
65  // translation).
66  static int constexpr num_parameters = 4;
67  // Group transformations are 3x3 matrices.
68  static int constexpr N = 3;
73 
74  // Adjoint transformation
75  //
76  // This function return the adjoint transformation ``Ad`` of the group
77  // element ``A`` such that for all ``x`` it holds that
78  // ``hat(Ad_A * x) = A * hat(x) A^{-1}``. See hat-operator below.
79  //
81  Matrix<Scalar, 2, 2> const& R = so2().matrix();
82  Transformation res;
83  res.setIdentity();
84  res.template topLeftCorner<2, 2>() = R;
85  res(0, 2) = translation()[1];
86  res(1, 2) = -translation()[0];
87  return res;
88  }
89 
90  // Returns copy of instance casted to NewScalarType.
91  //
92  template <class NewScalarType>
94  return SE2<NewScalarType>(so2().template cast<NewScalarType>(),
95  translation().template cast<NewScalarType>());
96  }
97 
98  // Returns group inverse.
99  //
101  SO2<Scalar> const invR = so2().inverse();
102  return SE2<Scalar>(invR, invR * (translation() * Scalar(-1)));
103  }
104 
105  // Logarithmic map
106  //
107  // Returns tangent space representation (= twist) of the instance.
108  //
109  SOPHUS_FUNC Tangent log() const { return log(*this); }
110 
111  /**
112  * \brief Normalize SO2 element
113  *
114  * It re-normalizes the SO2 element.
115  */
116  SOPHUS_FUNC void normalize() { so2().normalize(); }
117 
118  // Returns 3x3 matrix representation of the instance.
119  //
120  // It has the following form:
121  //
122  // | R t |
123  // | o 1 |
124  //
125  // where ``R`` is a 2x2 rotation matrix, ``t`` a translation 2-vector and
126  // ``o`` a 2-column vector of zeros.
127  //
129  Transformation homogenious_matrix;
130  homogenious_matrix.template topLeftCorner<2, 3>() = matrix2x3();
131  homogenious_matrix.row(2) =
133  return homogenious_matrix;
134  }
135 
136  // Returns the significant first two rows of the matrix above.
137  //
139  Matrix<Scalar, 2, 3> matrix;
140  matrix.template topLeftCorner<2, 2>() = rotationMatrix();
141  matrix.col(2) = translation();
142  return matrix;
143  }
144 
145  // Assignment operator.
146  //
147  template <class OtherDerived>
149  so2() = other.so2();
150  translation() = other.translation();
151  return *this;
152  }
153 
154  // Group multiplication, which is rotation concatenation.
155  //
157  SE2<Scalar> result(*this);
158  result *= other;
159  return result;
160  }
161 
162  // Group action on 2-points.
163  //
164  // This function rotates and translates a two dimensional point ``p`` by the
165  // SE(2) element ``bar_T_foo = (bar_R_foo, t_bar)`` (= rigid body
166  // transformation):
167  //
168  // ``p_bar = bar_R_foo * p_foo + t_bar``.
169  //
170  SOPHUS_FUNC Point operator*(Point const& p) const {
171  return so2() * p + translation();
172  }
173 
174  // In-place group multiplication.
175  //
177  translation() += so2() * (other.translation());
178  so2() *= other.so2();
179  return *this;
180  }
181 
182  // Returns rotation matrix.
183  //
185  return so2().matrix();
186  }
187 
188  // Takes in complex number, and normalizes it.
189  //
190  // Precondition: The complex number must not be close to zero.
191  //
193  return so2().setComplex(complex);
194  }
195 
196  // Sets ``so3`` using ``rotation_matrix``.
197  //
198  // Precondition: ``R`` must be orthogonal and ``det(R)=1``.
199  //
201  so2().setComplex(Scalar(0.5) * (R(0, 0) + R(1, 1)),
202  Scalar(0.5) * (R(1, 0) - R(0, 1)));
203  }
204 
205  // Mutator of SO3 group.
206  //
208  SO2Type& so2() { return static_cast<Derived*>(this)->so2(); }
209 
210  // Accessor of SO3 group.
211  //
213  SO2Type const& so2() const {
214  return static_cast<Derived const*>(this)->so2();
215  }
216 
217  // Mutator of translation vector.
218  //
221  return static_cast<Derived*>(this)->translation();
222  }
223 
224  // Accessor of translation vector
225  //
227  TranslationType const& translation() const {
228  return static_cast<Derived const*>(this)->translation();
229  }
230 
231  // Accessor of unit complex number.
232  //
234  typename Eigen::internal::traits<Derived>::SO2Type::Complex const&
235  unit_complex() const {
236  return so2().unit_complex();
237  }
238 
239  ////////////////////////////////////////////////////////////////////////////
240  // public static functions
241  ////////////////////////////////////////////////////////////////////////////
242 
243  // Derivative of Lie bracket with respect to first element.
244  //
245  // This function returns ``D_a [a, b]`` with ``D_a`` being the
246  // differential operator with respect to ``a``, ``[a, b]`` being the lie
247  // bracket of the Lie algebra se3.
248  // See ``lieBracket()`` below.
249  //
251  static Scalar const zero = Scalar(0);
252  Vector2<Scalar> upsilon2 = b.template head<2>();
253  Scalar theta2 = b[2];
254 
255  Transformation res;
256  res << zero, theta2, -upsilon2[1], -theta2, zero, upsilon2[0], zero, zero,
257  zero;
258  return res;
259  }
260 
261  // Group exponential
262  //
263  // This functions takes in an element of tangent space (= twist ``a``) and
264  // returns the corresponding element of the group SE(2).
265  //
266  // The first two components of ``a`` represent the translational part
267  // ``upsilon`` in the tangent space of SE(2), while the last three components
268  // of ``a`` represents the rotation vector ``omega``.
269  // To be more specific, this function computes ``expmat(hat(a))`` with
270  // ``expmat(.)`` being the matrix exponential and ``hat(.)`` the hat-operator
271  // of SE(2), see below.
272  //
273  SOPHUS_FUNC static SE2<Scalar> exp(Tangent const& a) {
274  Scalar theta = a[2];
275  SO2<Scalar> so2 = SO2<Scalar>::exp(theta);
276  Scalar sin_theta_by_theta;
277  Scalar one_minus_cos_theta_by_theta;
278 
279  if (std::abs(theta) < Constants<Scalar>::epsilon()) {
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;
284  } else {
285  sin_theta_by_theta = so2.unit_complex().y() / theta;
286  one_minus_cos_theta_by_theta =
287  (Scalar(1.) - so2.unit_complex().x()) / theta;
288  }
289  Vector2<Scalar> trans(
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]);
292  return SE2<Scalar>(so2, trans);
293  }
294 
295  // Returns the ith infinitesimal generators of SE(2).
296  //
297  // The infinitesimal generators of SE(2) are:
298  //
299  // | 0 0 1 |
300  // G_0 = | 0 0 0 |
301  // | 0 0 0 |
302  //
303  // | 0 0 0 |
304  // G_1 = | 0 0 1 |
305  // | 0 0 0 |
306  //
307  // | 0 -1 0 |
308  // G_2 = | 1 0 0 |
309  // | 0 0 0 |
310  // Precondition: ``i`` must be in 0, 1 or 2.
311  //
313  SOPHUS_ENSURE(i >= 0 || i <= 2, "i should be in range [0,2].");
314  Tangent e;
315  e.setZero();
316  e[i] = Scalar(1);
317  return hat(e);
318  }
319 
320  // hat-operator
321  //
322  // It takes in the 3-vector representation (= twist) and returns the
323  // corresponding matrix representation of Lie algebra element.
324  //
325  // Formally, the ``hat()`` operator of SE(3) is defined as
326  //
327  // ``hat(.): R^3 -> R^{3x33}, hat(a) = sum_i a_i * G_i`` (for i=0,1,2)
328  //
329  // with ``G_i`` being the ith infinitesimal generator of SE(2).
330  //
332  Transformation Omega;
333  Omega.setZero();
334  Omega.template topLeftCorner<2, 2>() = SO2<Scalar>::hat(a[2]);
335  Omega.col(2).template head<2>() = a.template head<2>();
336  return Omega;
337  }
338 
339  // Lie bracket
340  //
341  // It computes the Lie bracket of SE(2). To be more specific, it computes
342  //
343  // ``[omega_1, omega_2]_se2 := vee([hat(omega_1), hat(omega_2)])``
344  //
345  // with ``[A,B] := AB-BA`` being the matrix commutator, ``hat(.) the
346  // hat-operator and ``vee(.)`` the vee-operator of SE(2).
347  //
348  SOPHUS_FUNC static Tangent lieBracket(Tangent const& a, Tangent const& b) {
349  Vector2<Scalar> upsilon1 = a.template head<2>();
350  Vector2<Scalar> upsilon2 = b.template head<2>();
351  Scalar theta1 = a[2];
352  Scalar theta2 = b[2];
353 
354  return Tangent(-theta1 * upsilon2[1] + theta2 * upsilon1[1],
355  theta1 * upsilon2[0] - theta2 * upsilon1[0], Scalar(0));
356  }
357 
358  // Logarithmic map
359  //
360  // Computes the logarithm, the inverse of the group exponential which maps
361  // element of the group (rigid body transformations) to elements of the
362  // tangent space (twist).
363  //
364  // To be specific, this function computes ``vee(logmat(.))`` with
365  // ``logmat(.)`` being the matrix logarithm and ``vee(.)`` the vee-operator
366  // of SE(2).
367  //
368  SOPHUS_FUNC static Tangent log(SE2<Scalar> const& other) {
369  Tangent upsilon_theta;
370  SO2<Scalar> const& so2 = other.so2();
371  Scalar theta = SO2<Scalar>::log(so2);
372  upsilon_theta[2] = theta;
373  Scalar halftheta = Scalar(0.5) * theta;
374  Scalar halftheta_by_tan_of_halftheta;
375 
376  Vector2<Scalar> const& z = so2.unit_complex();
377  Scalar real_minus_one = z.x() - Scalar(1.);
378  if (std::abs(real_minus_one) < Constants<Scalar>::epsilon()) {
379  halftheta_by_tan_of_halftheta =
380  Scalar(1.) - Scalar(1. / 12) * theta * theta;
381  } else {
382  halftheta_by_tan_of_halftheta = -(halftheta * z.y()) / (real_minus_one);
383  }
384  Matrix<Scalar, 2, 2> V_inv;
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;
389  }
390 
391  // vee-operator
392  //
393  // It takes the 3x3-matrix representation ``Omega`` and maps it to the
394  // corresponding 3-vector representation of Lie algebra.
395  //
396  // This is the inverse of the hat-operator, see above.
397  //
398  // Precondition: ``Omega`` must have the following structure:
399  //
400  // | 0 -d a |
401  // | d 0 b |
402  // | 0 0 0 | .
403  //
404  SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
406  Omega.row(2).template lpNorm<1>() < Constants<Scalar>::epsilon(),
407  "Omega: \n%", Omega);
408  Tangent upsilon_omega;
409  upsilon_omega.template head<2>() = Omega.col(2).template head<2>();
410  upsilon_omega[2] = SO2<Scalar>::vee(Omega.template topLeftCorner<2, 2>());
411  return upsilon_omega;
412  }
413 };
414 
415 // SE2 default type - Constructors and default storage for SE3 Type.
416 template <class Scalar_, int Options>
417 class SE2 : public SE2Base<SE2<Scalar_, Options>> {
419 
420  public:
421  using Scalar = Scalar_;
423  using Point = typename Base::Point;
424  using Tangent = typename Base::Tangent;
425  using Adjoint = typename Base::Adjoint;
428 
429  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
430 
431  // Default constructor initialize rigid body motion to the identity.
432  //
433  SOPHUS_FUNC SE2() : translation_(Vector2<Scalar>::Zero()) {}
434 
435  // Copy constructor
436  //
437  template <class OtherDerived>
439  : so2_(other.so2()), translation_(other.translation()) {
441  "must be same Scalar type");
442  }
443 
444  // Constructor from SO3 and translation vector
445  //
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");
454  }
455 
456  // Constructor from rotation matrix and translation vector
457  //
458  // Precondition: Rotation matrix needs to be orthogonal with determinant of 1.
459  //
461  SE2(typename SO2<Scalar>::Transformation const& rotation_matrix,
462  Point const& translation)
463  : so2_(rotation_matrix), translation_(translation) {}
464 
465  // Constructor from rotation angle and translation vector.
466  //
467  SOPHUS_FUNC SE2(Scalar const& theta, Point const& translation)
468  : so2_(theta), translation_(translation) {}
469 
470  // Constructor from complex number and translation vector
471  //
472  // Precondition: ``complex` must not be close to zero.
473  SOPHUS_FUNC SE2(Vector2<Scalar> const& complex, Point const& translation)
474  : so2_(complex), translation_(translation) {}
475 
476  // Constructor from 3x3 matrix
477  //
478  // Precondition: Rotation matrix needs to be orthogonal with determinant of 1.
479  // The last row must be (0, 0, 1).
480  //
481  SOPHUS_FUNC explicit SE2(Transformation const& T)
482  : so2_(T.template topLeftCorner<2, 2>().eval()),
483  translation_(T.template block<2, 1>(0, 2)) {}
484 
485  // Construct a translation only SE3 instance.
486  //
487  template <class T0, class T1>
488  static SOPHUS_FUNC SE2 trans(T0 const& x, T1 const& y) {
489  return SE2(SO2<Scalar>(), Vector2<Scalar>(x, y));
490  }
491 
492  // Contruct x-axis translation.
493  //
494  static SOPHUS_FUNC SE2 transX(Scalar const& x) {
495  return SE2::trans(x, Scalar(0));
496  }
497 
498  // Contruct y-axis translation.
499  //
500  static SOPHUS_FUNC SE2 transY(Scalar const& y) {
501  return SE2::trans(Scalar(0), y);
502  }
503 
504  // Contruct pure rotation.
505  //
506  static SOPHUS_FUNC SE2 rot(Scalar const& x) {
508  }
509 
510  // This provides unsafe read/write access to internal data. SO(2) is
511  // represented by a complex number (two parameters). When using direct write
512  // access, the user needs to take care of that the complex number stays
513  // normalized.
514  //
516  // so2_ and translation_ are layed out sequentially with no padding
517  return so2_.data();
518  }
519 
520  // Const version of data() above.
521  //
522  SOPHUS_FUNC Scalar const* data() const {
523  // so2_ and translation_ are layed out sequentially with no padding
524  return so2_.data();
525  }
526 
527  // Accessor of SO3
528  //
529  SOPHUS_FUNC SO2Member& so2() { return so2_; }
530 
531  // Mutator of SO3
532  //
533  SOPHUS_FUNC SO2Member const& so2() const { return so2_; }
534 
535  // Mutator of translation vector
536  //
537  SOPHUS_FUNC TranslationMember& translation() { return translation_; }
538 
539  // Accessor of translation vector
540  //
542  return translation_;
543  }
544 
545  protected:
548 };
549 
550 } // end namespace
551 
552 namespace Eigen {
553 
554 // Specialization of Eigen::Map for ``SE2``.
555 //
556 // Allows us to wrap SE2 objects around POD array.
557 template <class Scalar_, int Options>
558 class Map<Sophus::SE2<Scalar_>, Options>
559  : public Sophus::SE2Base<Map<Sophus::SE2<Scalar_>, Options>> {
561 
562  public:
563  using Scalar = Scalar_;
565  using Point = typename Base::Point;
566  using Tangent = typename Base::Tangent;
567  using Adjoint = typename Base::Adjoint;
568 
569  EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
570  using Base::operator*=;
571  using Base::operator*;
572 
574  Map(Scalar* coeffs)
575  : so2_(coeffs),
576  translation_(coeffs + Sophus::SO2<Scalar>::num_parameters) {}
577 
578  // Mutator of SO3
579  //
580  SOPHUS_FUNC Map<Sophus::SO2<Scalar>, Options>& so2() { return so2_; }
581 
582  // Accessor of SO3
583  //
584  SOPHUS_FUNC Map<Sophus::SO2<Scalar>, Options> const& so2() const {
585  return so2_;
586  }
587 
588  // Mutator of translation vector
589  //
590  SOPHUS_FUNC Map<Sophus::Vector2<Scalar>, Options>& translation() {
591  return translation_;
592  }
593 
594  // Accessor of translation vector
595  //
596  SOPHUS_FUNC Map<Sophus::Vector2<Scalar>, Options> const& translation() const {
597  return translation_;
598  }
599 
600  protected:
601  Map<Sophus::SO2<Scalar>, Options> so2_;
602  Map<Sophus::Vector2<Scalar>, Options> translation_;
603 };
604 
605 // Specialization of Eigen::Map for ``SE2 const``.
606 //
607 // Allows us to wrap SE2 objects around POD array.
608 template <class Scalar_, int Options>
609 class Map<Sophus::SE2<Scalar_> const, Options>
610  : public Sophus::SE2Base<Map<Sophus::SE2<Scalar_> const, Options>> {
612 
613  public:
614  using Scalar = Scalar_;
616  using Point = typename Base::Point;
617  using Tangent = typename Base::Tangent;
618  using Adjoint = typename Base::Adjoint;
619 
620  EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
621  using Base::operator*=;
622  using Base::operator*;
623 
624  SOPHUS_FUNC Map(Scalar const* coeffs)
625  : so2_(coeffs),
626  translation_(coeffs + Sophus::SO2<Scalar>::num_parameters) {}
627 
628  // Accessor of SO3
629  //
630  SOPHUS_FUNC Map<Sophus::SO2<Scalar> const, Options> const& so2() const {
631  return so2_;
632  }
633 
634  // Accessor of translation vector
635  //
636  SOPHUS_FUNC Map<Sophus::Vector2<Scalar> const, Options> const& translation()
637  const {
638  return translation_;
639  }
640 
641  protected:
642  Map<Sophus::SO2<Scalar> const, Options> const so2_;
643  Map<Sophus::Vector2<Scalar> const, Options> const translation_;
644 };
645 }
646 
647 #endif
SOPHUS_FUNC SE2< NewScalarType > cast() const
Definition: se2.hpp:93
SOPHUS_FUNC SE2< Scalar > operator*(SE2< Scalar > const &other) const
Definition: se2.hpp:156
typename Base::Transformation Transformation
Definition: se2.hpp:422
SOPHUS_FUNC Map< Sophus::SO2< Scalar >, Options > const & so2() const
Definition: se2.hpp:584
SOPHUS_FUNC SO2Member const & so2() const
Definition: se2.hpp:533
Map< Sophus::SO2< Scalar > const, Options > SO2Type
Definition: se2.hpp:36
#define SOPHUS_ENSURE(expr, description,...)
Definition: common.hpp:129
static SOPHUS_FUNC SE2 transX(Scalar const &x)
Definition: se2.hpp:494
Map< Sophus::SO2< Scalar > const, Options > const so2_
Definition: se2.hpp:642
static SOPHUS_FUNC Transformation hat(Tangent const &a)
Definition: se2.hpp:331
SOPHUS_FUNC SE2(Vector2< Scalar > const &complex, Point const &translation)
Definition: se2.hpp:473
SOPHUS_FUNC void setComplex(Sophus::Vector2< Scalar > const &complex)
Definition: se2.hpp:192
static SOPHUS_FUNC SO2< Scalar > exp(Tangent const &theta)
Definition: so2.hpp:241
static SOPHUS_FUNC SE2 transY(Scalar const &y)
Definition: se2.hpp:500
SOPHUS_FUNC TranslationMember & translation()
Definition: se2.hpp:537
Map< Sophus::SO2< Scalar >, Options > so2_
Definition: se2.hpp:601
SOPHUS_FUNC Map(Scalar *coeffs)
Definition: se2.hpp:574
T value(details::expression_node< T > *n)
Definition: exprtk.hpp:12104
SOPHUS_FUNC Adjoint Adj() const
Definition: se2.hpp:80
Sophus::Vector2< Scalar, Options > TranslationType
Definition: se2.hpp:19
static SOPHUS_FUNC Tangent log(SE2< Scalar > const &other)
Definition: se2.hpp:368
SOPHUS_FUNC SE2Base< Derived > & operator=(SE2Base< OtherDerived > const &other)
Definition: se2.hpp:148
Map< Sophus::Vector2< Scalar >, Options > translation_
Definition: se2.hpp:602
SOPHUS_FUNC SE2(Transformation const &T)
Definition: se2.hpp:481
SOPHUS_FUNC Tangent log() const
Definition: se2.hpp:109
SOPHUS_FUNC Map< Sophus::Vector2< Scalar >, Options > & translation()
Definition: se2.hpp:590
SOPHUS_FUNC Map< Sophus::SO2< Scalar >, Options > & so2()
Definition: se2.hpp:580
SOPHUS_FUNC SE2Base< Derived > & operator*=(SE2< Scalar > const &other)
Definition: se2.hpp:176
SOPHUS_FUNC Map(Scalar const *coeffs)
Definition: se2.hpp:624
static SOPHUS_FUNC Transformation d_lieBracketab_by_d_a(Tangent const &b)
Definition: se2.hpp:250
Vector< Scalar, 2, Options > Vector2
Definition: types.hpp:13
SOPHUS_FUNC Map< Sophus::Vector2< Scalar > const, Options > const & translation() const
Definition: se2.hpp:636
SOPHUS_FUNC TranslationType const & translation() const
Definition: se2.hpp:227
SO2Member so2_
Definition: se2.hpp:546
Map< Sophus::Vector2< Scalar > const, Options > const translation_
Definition: se2.hpp:643
SOPHUS_FUNC SE2(typename SO2< Scalar >::Transformation const &rotation_matrix, Point const &translation)
Definition: se2.hpp:461
SOPHUS_FUNC Matrix< Scalar, 2, 3 > matrix2x3() const
Definition: se2.hpp:138
SOPHUS_FUNC TranslationType & translation()
Definition: se2.hpp:220
Eigen::Matrix< Scalar, M, N > Matrix
Definition: types.hpp:38
typename Base::Transformation Transformation
Definition: se2.hpp:615
typename Base::Transformation Transformation
Definition: so2.hpp:335
static SOPHUS_FUNC SE2< Scalar > exp(Tangent const &a)
Definition: se2.hpp:273
typename Base::Point Point
Definition: se2.hpp:423
typename Base::Transformation Transformation
Definition: se2.hpp:564
static SOPHUS_FUNC Transformation generator(int i)
Definition: se2.hpp:312
SOPHUS_FUNC SO2Type const & so2() const
Definition: se2.hpp:213
SOPHUS_FUNC SO2Member & so2()
Definition: se2.hpp:529
SOPHUS_FUNC Scalar log() const
Definition: so2.hpp:127
SOPHUS_FUNC Matrix< Scalar, 2, 2 > rotationMatrix() const
Definition: se2.hpp:184
SOPHUS_FUNC void normalize()
Normalize SO2 element.
Definition: se2.hpp:116
SOPHUS_FUNC TranslationMember const & translation() const
Definition: se2.hpp:541
SOPHUS_FUNC Transformation matrix() const
Definition: se2.hpp:128
SOPHUS_FUNC Point operator*(Point const &p) const
Definition: se2.hpp:170
typename Eigen::internal::traits< Map< Sophus::SE2< Scalar_ >, Options > >::TranslationType TranslationType
Definition: se2.hpp:58
const e_voc T1 e_none const e_none T1 e_none T0
Definition: exprtk.hpp:13518
typename Base::Tangent Tangent
Definition: se2.hpp:424
static SOPHUS_FUNC Tangent vee(Transformation const &Omega)
Definition: se2.hpp:404
TranslationMember translation_
Definition: se2.hpp:547
SOPHUS_FUNC SE2< Scalar > inverse() const
Definition: se2.hpp:100
SOPHUS_FUNC Eigen::internal::traits< Derived >::SO2Type::Complex const & unit_complex() const
Definition: se2.hpp:235
SOPHUS_FUNC SE2(Scalar const &theta, Point const &translation)
Definition: se2.hpp:467
SOPHUS_FUNC Scalar const * data() const
Definition: se2.hpp:522
Eigen::Matrix< Scalar, M, 1, Options > Vector
Definition: types.hpp:10
Vector2< Scalar, Options > TranslationMember
Definition: se2.hpp:427
static SOPHUS_FUNC SE2 trans(T0 const &x, T1 const &y)
Definition: se2.hpp:488
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)
Definition: se2.hpp:506
SOPHUS_FUNC Map< Sophus::Vector2< Scalar >, Options > const & translation() const
Definition: se2.hpp:596
static SOPHUS_FUNC Tangent vee(Transformation const &Omega)
Definition: so2.hpp:313
EIGEN_MAKE_ALIGNED_OPERATOR_NEW SOPHUS_FUNC SE2()
Definition: se2.hpp:433
SOPHUS_FUNC void setRotationMatrix(Matrix< Scalar, 2, 2 > const &R)
Definition: se2.hpp:200
SOPHUS_FUNC ComplexMember const & unit_complex() const
Definition: so2.hpp:397
SOPHUS_FUNC Scalar * data()
Definition: se2.hpp:515
typename Eigen::internal::traits< Map< Sophus::SE2< Scalar_ >, Options > >::Scalar Scalar
Definition: se2.hpp:56
Map< Sophus::Vector2< Scalar > const, Options > TranslationType
Definition: se2.hpp:35
SOPHUS_FUNC SE2(SO2Base< OtherDerived > const &so2, Eigen::MatrixBase< D > const &translation)
Definition: se2.hpp:447
Scalar_ Scalar
Definition: se2.hpp:421
SOPHUS_FUNC SO2< Scalar > inverse() const
Definition: so2.hpp:119
static SOPHUS_FUNC Tangent lieBracket(Tangent const &a, Tangent const &b)
Definition: se2.hpp:348
SOPHUS_FUNC SE2(SE2Base< OtherDerived > const &other)
Definition: se2.hpp:438
typename Base::Adjoint Adjoint
Definition: se2.hpp:425
SOPHUS_FUNC SO2Type & so2()
Definition: se2.hpp:208
#define SOPHUS_FUNC
Definition: common.hpp:32
SOPHUS_FUNC Map< Sophus::SO2< Scalar > const, Options > const & so2() const
Definition: se2.hpp:630
typename Eigen::internal::traits< Map< Sophus::SE2< Scalar_ >, Options > >::SO2Type SO2Type
Definition: se2.hpp:59



Page generated by Doxygen 1.8.13 for MRPT 1.5.3 at Tue Oct 31 07:27:35 UTC 2017