Main MRPT website > C++ reference for MRPT 1.5.3
se3.hpp
Go to the documentation of this file.
1 #ifndef SOPHUS_SE3_HPP
2 #define SOPHUS_SE3_HPP
3 
4 #include "so3.hpp"
5 
6 namespace Sophus {
7 template <class Scalar_, int Options = 0>
8 class SE3;
9 using SE3d = SE3<double>;
10 using SE3f = SE3<float>;
11 } // namespace Sophus
12 
13 namespace Eigen {
14 namespace internal {
15 
16 template <class Scalar_, int Options>
17 struct traits<Sophus::SE3<Scalar_, Options>> {
18  using Scalar = Scalar_;
21 };
22 
23 template <class Scalar_, int Options>
24 struct traits<Map<Sophus::SE3<Scalar_>, Options>>
25  : traits<Sophus::SE3<Scalar_, Options>> {
26  using Scalar = Scalar_;
27  using TranslationType = Map<Sophus::Vector3<Scalar>, Options>;
28  using SO3Type = Map<Sophus::SO3<Scalar>, Options>;
29 };
30 
31 template <class Scalar_, int Options>
32 struct traits<Map<Sophus::SE3<Scalar_> const, Options>>
33  : traits<Sophus::SE3<Scalar_, Options> const> {
34  using Scalar = Scalar_;
35  using TranslationType = Map<Sophus::Vector3<Scalar> const, Options>;
36  using SO3Type = Map<Sophus::SO3<Scalar> const, Options>;
37 };
38 } // namespace internal
39 } // namespace Eigen
40 
41 namespace Sophus {
42 
43 // SE3 base type - implements SE3 class but is storage agnostic.
44 //
45 // SE(3) is the group of rotations and translation in 3d. It is the semi-direct
46 // product of SO(3) and the 3d Euclidean vector space. The class is represented
47 // using a composition of SO3 for rotation and a one 3-vector for
48 // translation.
49 //
50 // SE(3) is neither compact, nor a commutative group.
51 //
52 // See SO3 for more details of the rotation representation in 3d.
53 //
54 template <class Derived>
55 class SE3Base {
56  public:
57  using Scalar = typename Eigen::internal::traits<Derived>::Scalar;
58  using TranslationType =
59  typename Eigen::internal::traits<Derived>::TranslationType;
60  using SO3Type = typename Eigen::internal::traits<Derived>::SO3Type;
61  using QuaternionType = typename SO3Type::QuaternionType;
62  // Degrees of freedom of manifold, number of dimensions in tangent space
63  // (three for translation, three for rotation).
64  static int constexpr DoF = 6;
65  // Number of internal parameters used (4-tuple for quaternion, three for
66  // translation).
67  static int constexpr num_parameters = 7;
68  // Group transformations are 4x4 matrices.
69  static int constexpr N = 4;
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  Sophus::Matrix3<Scalar> const R = so3().matrix();
82  Adjoint res;
83  res.block(0, 0, 3, 3) = R;
84  res.block(3, 3, 3, 3) = R;
85  res.block(0, 3, 3, 3) = SO3<Scalar>::hat(translation()) * R;
86  res.block(3, 0, 3, 3) = Matrix3<Scalar>::Zero(3, 3);
87  return res;
88  }
89 
90  // Returns copy of instance casted to NewScalarType.
91  //
92  template <class NewScalarType>
94  return SE3<NewScalarType>(so3().template cast<NewScalarType>(),
95  translation().template cast<NewScalarType>());
96  }
97 
98  // Returns ``*this`` times the ith generator of internal representation.
99  //
101  int i) const {
103 
104  Eigen::Quaternion<Scalar> internal_gen_q;
105  Vector<Scalar, 3> internal_gen_t;
106 
107  internalGenerator(i, &internal_gen_q, &internal_gen_t);
108 
109  res.template head<4>() =
110  (so3().unit_quaternion() * internal_gen_q).coeffs();
111  res.template tail<3>() = so3().unit_quaternion() * internal_gen_t;
112  return res;
113  }
114 
115  // Returns Jacobian of generator of internal SU(2) representation.
116  //
119  for (int i = 0; i < DoF; ++i) {
120  J.col(i) = internalMultiplyByGenerator(i);
121  }
122  return J;
123  }
124 
125  // Returns group inverse.
126  //
128  SO3<Scalar> invR = so3().inverse();
129  return SE3<Scalar>(invR, invR * (translation() * Scalar(-1)));
130  }
131 
132  // Logarithmic map
133  //
134  // Returns tangent space representation (= twist) of the instance.
135  //
136  SOPHUS_FUNC Tangent log() const { return log(*this); }
137 
138  // It re-normalizes the SO3 element.
139  //
140  // Note: Because of the class invariant of SO3, there is typically no need to
141  // call this function directly.
142  //
143  SOPHUS_FUNC void normalize() { so3().normalize(); }
144 
145  // Returns 4x4 matrix representation of the instance.
146  //
147  // It has the following form:
148  //
149  // | R t |
150  // | o 1 |
151  //
152  // where ``R`` is a 3x3 rotation matrix, ``t`` a translation 3-vector and
153  // ``o`` a 3-column vector of zeros.
154  //
156  Transformation homogenious_matrix;
157  homogenious_matrix.template topLeftCorner<3, 4>() = matrix3x4();
158  homogenious_matrix.row(3) =
160  return homogenious_matrix;
161  }
162 
163  // Returns the significant first three rows of the matrix above.
164  //
166  Matrix<Scalar, 3, 4> matrix;
167  matrix.template topLeftCorner<3, 3>() = rotationMatrix();
168  matrix.col(3) = translation();
169  return matrix;
170  }
171 
172  // Assignment operator.
173  //
174  template <class OtherDerived>
176  so3() = other.so3();
177  translation() = other.translation();
178  return *this;
179  }
180 
181  // Group multiplication, which is rotation concatenation.
182  //
184  SE3<Scalar> result(*this);
185  result *= other;
186  return result;
187  }
188 
189  // Group action on 3-points.
190  //
191  // This function rotates and translates a three dimensional point ``p`` by the
192  // SE(3) element ``bar_T_foo = (bar_R_foo, t_bar)`` (= rigid body
193  // transformation):
194  //
195  // ``p_bar = bar_R_foo * p_foo + t_bar``.
196  //
197  SOPHUS_FUNC Point operator*(Point const& p) const {
198  return so3() * p + translation();
199  }
200 
201  // In-place group multiplication.
202  //
204  translation() += so3() * (other.translation());
205  so3() *= other.so3();
206  return *this;
207  }
208 
209  // Returns rotation matrix.
210  //
211  SOPHUS_FUNC Matrix3<Scalar> rotationMatrix() const { return so3().matrix(); }
212 
213  // Mutator of SO3 group.
214  //
215  SOPHUS_FUNC SO3Type& so3() { return static_cast<Derived*>(this)->so3(); }
216 
217  // Accessor of SO3 group.
218  //
219  SOPHUS_FUNC SO3Type const& so3() const {
220  return static_cast<const Derived*>(this)->so3();
221  }
222 
223  // Takes in quaternion, and normalizes it.
224  //
225  // Precondition: The quaternion must not be close to zero.
226  //
227  SOPHUS_FUNC void setQuaternion(Eigen::Quaternion<Scalar> const& quat) {
228  so3().setQuaternion(quat);
229  }
230 
231  // Sets ``so3`` using ``rotation_matrix``.
232  //
233  // Precondition: ``R`` must be orthogonal and ``det(R)=1``.
234  //
235  SOPHUS_FUNC void setRotationMatrix(Matrix3<Scalar> const& rotation_matrix) {
236  so3().setQuaternion(Eigen::Quaternion<Scalar>(rotation_matrix));
237  }
238 
239  // Mutator of translation vector.
240  //
242  return static_cast<Derived*>(this)->translation();
243  }
244 
245  // Accessor of translation vector
246  //
248  return static_cast<Derived const*>(this)->translation();
249  }
250 
251  // Accessor of unit quaternion.
252  //
254  return this->so3().unit_quaternion();
255  }
256 
257  ////////////////////////////////////////////////////////////////////////////
258  // public static functions
259  ////////////////////////////////////////////////////////////////////////////
260 
261  // Derivative of Lie bracket with respect to first element.
262  //
263  // This function returns ``D_a [a, b]`` with ``D_a`` being the
264  // differential operator with respect to ``a``, ``[a, b]`` being the lie
265  // bracket of the Lie algebra se(3).
266  // See ``lieBracket()`` below.
267  //
269  Adjoint res;
270  res.setZero();
271 
272  Vector3<Scalar> const upsilon2 = b.template head<3>();
273  Vector3<Scalar> const omega2 = b.template tail<3>();
274 
275  res.template topLeftCorner<3, 3>() = -SO3<Scalar>::hat(omega2);
276  res.template topRightCorner<3, 3>() = -SO3<Scalar>::hat(upsilon2);
277  res.template bottomRightCorner<3, 3>() = -SO3<Scalar>::hat(omega2);
278  return res;
279  }
280 
281  // Group exponential
282  //
283  // This functions takes in an element of tangent space (= twist ``a``) and
284  // returns the corresponding element of the group SE(3).
285  //
286  // The first three components of ``a`` represent the translational part
287  // ``upsilon`` in the tangent space of SE(3), while the last three components
288  // of ``a`` represents the rotation vector ``omega``.
289  // To be more specific, this function computes ``expmat(hat(a))`` with
290  // ``expmat(.)`` being the matrix exponential and ``hat(.)`` the hat-operator
291  // of SE(3), see below.
292  //
293  SOPHUS_FUNC static SE3<Scalar> exp(Tangent const& a) {
294  using std::cos;
295  using std::sin;
296  Vector3<Scalar> const omega = a.template tail<3>();
297 
298  Scalar theta;
299  SO3<Scalar> const so3 = SO3<Scalar>::expAndTheta(omega, &theta);
300  Matrix3<Scalar> const Omega = SO3<Scalar>::hat(omega);
301  Matrix3<Scalar> const Omega_sq = Omega * Omega;
302  Matrix3<Scalar> V;
303 
304  if (theta < Constants<Scalar>::epsilon()) {
305  V = so3.matrix();
306  // Note: That is an accurate expansion!
307  } else {
308  Scalar theta_sq = theta * theta;
310  (Scalar(1) - cos(theta)) / (theta_sq)*Omega +
311  (theta - sin(theta)) / (theta_sq * theta) * Omega_sq);
312  }
313  return SE3<Scalar>(so3, V * a.template head<3>());
314  }
315 
316  // Returns the ith infinitesimal generators of SE(3).
317  //
318  // The infinitesimal generators of SE(3) are:
319  //
320  // | 0 0 0 1 |
321  // G_0 = | 0 0 0 0 |
322  // | 0 0 0 0 |
323  // | 0 0 0 0 |
324  //
325  // | 0 0 0 0 |
326  // G_1 = | 0 0 0 1 |
327  // | 0 0 0 0 |
328  // | 0 0 0 0 |
329  //
330  // | 0 0 0 0 |
331  // G_2 = | 0 0 0 0 |
332  // | 0 0 0 1 |
333  // | 0 0 0 0 |
334  //
335  // | 0 0 0 0 |
336  // G_3 = | 0 0 -1 0 |
337  // | 0 1 0 0 |
338  // | 0 0 0 0 |
339  //
340  // | 0 0 1 0 |
341  // G_4 = | 0 0 0 0 |
342  // | -1 0 0 0 |
343  // | 0 0 0 0 |
344  //
345  // | 0 -1 0 0 |
346  // G_5 = | 1 0 0 0 |
347  // | 0 0 0 0 |
348  // | 0 0 0 0 |
349  //
350  // Precondition: ``i`` must be in [0, 5].
351  //
353  SOPHUS_ENSURE(i >= 0 && i <= 5, "i should be in range [0,5].");
354  Tangent e;
355  e.setZero();
356  e[i] = Scalar(1);
357  return hat(e);
358  }
359 
360  // Returns the ith generator of internal representation.
361  //
362  // Precondition: ``i`` must be in [0, 5].
363  //
365  int i, Eigen::Quaternion<Scalar>* internal_gen_q,
366  Vector3<Scalar>* internal_gen_t) {
367  SOPHUS_ENSURE(i >= 0 && i <= 5, "i should be in range [0,5]");
368  SOPHUS_ENSURE(internal_gen_q != NULL,
369  "internal_gen_q must not be the null pointer");
370  SOPHUS_ENSURE(internal_gen_t != NULL,
371  "internal_gen_t must not be the null pointer");
372 
373  internal_gen_q->coeffs().setZero();
374  internal_gen_t->setZero();
375  if (i < 3) {
376  (*internal_gen_t)[i] = Scalar(1);
377  } else {
378  SO3<Scalar>::internalGenerator(i - 3, internal_gen_q);
379  ;
380  }
381  }
382 
383  // hat-operator
384  //
385  // It takes in the 6-vector representation (= twist) and returns the
386  // corresponding matrix representation of Lie algebra element.
387  //
388  // Formally, the ``hat()`` operator of SE(3) is defined as
389  //
390  // ``hat(.): R^6 -> R^{4x4}, hat(a) = sum_i a_i * G_i`` (for i=0,...,5)
391  //
392  // with ``G_i`` being the ith infinitesimal generator of SE(3).
393  //
395  Transformation Omega;
396  Omega.setZero();
397  Omega.template topLeftCorner<3, 3>() =
398  SO3<Scalar>::hat(a.template tail<3>());
399  Omega.col(3).template head<3>() = a.template head<3>();
400  return Omega;
401  }
402 
403  // Lie bracket
404  //
405  // It computes the Lie bracket of SE(3). To be more specific, it computes
406  //
407  // ``[omega_1, omega_2]_se3 := vee([hat(omega_1), hat(omega_2)])``
408  //
409  // with ``[A,B] := AB-BA`` being the matrix commutator, ``hat(.) the
410  // hat-operator and ``vee(.)`` the vee-operator of SE(3).
411  //
412  SOPHUS_FUNC static Tangent lieBracket(Tangent const& a, Tangent const& b) {
413  Vector3<Scalar> const upsilon1 = a.template head<3>();
414  Vector3<Scalar> const upsilon2 = b.template head<3>();
415  Vector3<Scalar> const omega1 = a.template tail<3>();
416  Vector3<Scalar> const omega2 = b.template tail<3>();
417 
418  Tangent res;
419  res.template head<3>() = omega1.cross(upsilon2) + upsilon1.cross(omega2);
420  res.template tail<3>() = omega1.cross(omega2);
421 
422  return res;
423  }
424 
425  // Logarithmic map
426  //
427  // Computes the logarithm, the inverse of the group exponential which maps
428  // element of the group (rigid body transformations) to elements of the
429  // tangent space (twist).
430  //
431  // To be specific, this function computes ``vee(logmat(.))`` with
432  // ``logmat(.)`` being the matrix logarithm and ``vee(.)`` the vee-operator
433  // of SE(3).
434  //
435  SOPHUS_FUNC static Tangent log(SE3<Scalar> const& se3) {
436  // For the derivation of the logarithm of SE(3), see
437  // J. Gallier, D. Xu, "Computing exponentials of skew symmetric matrices and
438  // logarithms of orthogonal matrices", IJRA 2002.
439  // https://pdfs.semanticscholar.org/cfe3/e4b39de63c8cabd89bf3feff7f5449fc981d.pdf
440  // (Sec. 6., pp. 8)
441  using std::abs;
442  using std::cos;
443  using std::sin;
444  Tangent upsilon_omega;
445  Scalar theta;
446  upsilon_omega.template tail<3>() =
447  SO3<Scalar>::logAndTheta(se3.so3(), &theta);
448  Matrix3<Scalar> const Omega =
449  SO3<Scalar>::hat(upsilon_omega.template tail<3>());
450 
451  if (abs(theta) < Constants<Scalar>::epsilon()) {
453  Scalar(0.5) * Omega +
454  Scalar(1. / 12.) * (Omega * Omega);
455 
456  upsilon_omega.template head<3>() = V_inv * se3.translation();
457  } else {
458  Scalar const half_theta = Scalar(0.5) * theta;
459 
460  Matrix3<Scalar> const V_inv =
461  (Matrix3<Scalar>::Identity() - Scalar(0.5) * Omega +
462  (Scalar(1) -
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();
466  }
467  return upsilon_omega;
468  }
469 
470  // vee-operator
471  //
472  // It takes 4x4-matrix representation ``Omega`` and maps it to the
473  // corresponding 6-vector representation of Lie algebra.
474  //
475  // This is the inverse of the hat-operator, see above.
476  //
477  // Precondition: ``Omega`` must have the following structure:
478  //
479  // | 0 -f e a |
480  // | f 0 -d b |
481  // | -e d 0 c |
482  // | 0 0 0 0 | .
483  //
484  SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
485  Tangent upsilon_omega;
486  upsilon_omega.template head<3>() = Omega.col(3).template head<3>();
487  upsilon_omega.template tail<3>() =
488  SO3<Scalar>::vee(Omega.template topLeftCorner<3, 3>());
489  return upsilon_omega;
490  }
491 };
492 
493 // SE3 default type - Constructors and default storage for SE3 Type.
494 template <class Scalar_, int Options>
495 class SE3 : public SE3Base<SE3<Scalar_, Options>> {
497 
498  public:
499  using Scalar = Scalar_;
501  using Point = typename Base::Point;
502  using Tangent = typename Base::Tangent;
503  using Adjoint = typename Base::Adjoint;
506 
507  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
508 
509  // Default constructor initialize rigid body motion to the identity.
510  //
511  SOPHUS_FUNC SE3() : translation_(Vector3<Scalar>::Zero()) {}
512 
513  // Copy constructor
514  //
515  template <class OtherDerived>
517  : so3_(other.so3()), translation_(other.translation()) {
519  "must be same Scalar type");
520  }
521 
522  // Constructor from SO3 and translation vector
523  //
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");
532  }
533 
534  // Constructor from rotation matrix and translation vector
535  //
536  // Precondition: Rotation matrix needs to be orthogonal with determinant of 1.
537  //
539  SE3(Matrix3<Scalar> const& rotation_matrix, Point const& translation)
540  : so3_(rotation_matrix), translation_(translation) {}
541 
542  // Constructor from quaternion and translation vector.
543  //
544  // Precondition: quaternion must not be close to zero.
545  //
546  SOPHUS_FUNC SE3(Eigen::Quaternion<Scalar> const& quaternion,
547  Point const& translation)
548  : so3_(quaternion), translation_(translation) {}
549 
550  // Constructor from 4x4 matrix
551  //
552  // Precondition: Rotation matrix needs to be orthogonal with determinant of 1.
553  // The last row must be (0, 0, 0, 1).
554  //
555  SOPHUS_FUNC explicit SE3(Matrix4<Scalar> const& T)
556  : so3_(T.template topLeftCorner<3, 3>()),
557  translation_(T.template block<3, 1>(0, 3)) {
558  SOPHUS_ENSURE((T.row(3) - Matrix<Scalar, 1, 4>(0, 0, 0, 1)).squaredNorm() <
560  "Last row is not (0,0,0,1), but (%).", T.row(3));
561  }
562 
563  // Construct a translation only SE3 instance.
564  //
565  template <class T0, class T1, class T2>
566  static SOPHUS_FUNC SE3 trans(T0 const& x, T1 const& y, T2 const& z) {
567  return SE3(SO3<Scalar>(), Vector3<Scalar>(x, y, z));
568  }
569 
570  // Contruct x-axis translation.
571  //
572  static SOPHUS_FUNC SE3 transX(Scalar const& x) {
573  return SE3::trans(x, Scalar(0), Scalar(0));
574  }
575 
576  // Contruct y-axis translation.
577  //
578  static SOPHUS_FUNC SE3 transY(Scalar const& y) {
579  return SE3::trans(Scalar(0), y, Scalar(0));
580  }
581 
582  // Contruct z-axis translation.
583  //
584  static SOPHUS_FUNC SE3 transZ(Scalar const& z) {
585  return SE3::trans(Scalar(0), Scalar(0), z);
586  }
587 
588  // Contruct x-axis rotation.
589  //
590  static SOPHUS_FUNC SE3 rotX(Scalar const& x) {
592  }
593 
594  // Contruct y-axis rotation.
595  //
596  static SOPHUS_FUNC SE3 rotY(Scalar const& y) {
598  }
599 
600  // Contruct z-axis rotation.
601  //
602  static SOPHUS_FUNC SE3 rotZ(Scalar const& z) {
604  }
605 
606  // This provides unsafe read/write access to internal data. SO(3) is
607  // represented by an Eigen::Quaternion (four parameters). When using direct
608  // write access, the user needs to take care of that the quaternion stays
609  // normalized.
610  //
612  // so3_ and translation_ are laid out sequentially with no padding
613  return so3_.data();
614  }
615 
616  // Const version of data() above.
617  //
618  SOPHUS_FUNC Scalar const* data() const {
619  // so3_ and translation_ are laid out sequentially with no padding
620  return so3_.data();
621  }
622 
623  // Accessor of SO3
624  //
625  SOPHUS_FUNC SO3Member& so3() { return so3_; }
626 
627  // Mutator of SO3
628  //
629  SOPHUS_FUNC SO3Member const& so3() const { return so3_; }
630 
631  // Mutator of translation vector
632  //
633  SOPHUS_FUNC TranslationMember& translation() { return translation_; }
634 
635  // Accessor of translation vector
636  //
638  return translation_;
639  }
640 
641  protected:
644 };
645 
646 } // namespace Sophus
647 
648 namespace Eigen {
649 
650 // Specialization of Eigen::Map for ``SE3``.
651 //
652 // Allows us to wrap SE3 objects around POD array.
653 template <class Scalar_, int Options>
654 class Map<Sophus::SE3<Scalar_>, Options>
655  : public Sophus::SE3Base<Map<Sophus::SE3<Scalar_>, Options>> {
657 
658  public:
659  using Scalar = Scalar_;
661  using Point = typename Base::Point;
662  using Tangent = typename Base::Tangent;
663  using Adjoint = typename Base::Adjoint;
664 
665  EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
666  using Base::operator*=;
667  using Base::operator*;
668 
670  : so3_(coeffs),
671  translation_(coeffs + Sophus::SO3<Scalar>::num_parameters) {}
672 
673  // Mutator of SO3
674  //
675  SOPHUS_FUNC Map<Sophus::SO3<Scalar>, Options>& so3() { return so3_; }
676 
677  // Accessor of SO3
678  //
679  SOPHUS_FUNC Map<Sophus::SO3<Scalar>, Options> const& so3() const {
680  return so3_;
681  }
682 
683  // Mutator of translation vector
684  //
685  SOPHUS_FUNC Map<Sophus::Vector3<Scalar>>& translation() {
686  return translation_;
687  }
688 
689  // Accessor of translation vector
690  //
691  SOPHUS_FUNC Map<Sophus::Vector3<Scalar>> const& translation() const {
692  return translation_;
693  }
694 
695  protected:
696  Map<Sophus::SO3<Scalar>, Options> so3_;
697  Map<Sophus::Vector3<Scalar>, Options> translation_;
698 };
699 
700 // Specialization of Eigen::Map for ``SE3 const``.
701 //
702 // Allows us to wrap SE3 objects around POD array.
703 template <class Scalar_, int Options>
704 class Map<Sophus::SE3<Scalar_> const, Options>
705  : public Sophus::SE3Base<Map<Sophus::SE3<Scalar_> const, Options>> {
707 
708  public:
709  using Scalar = Scalar_;
711  using Point = typename Base::Point;
712  using Tangent = typename Base::Tangent;
713  using Adjoint = typename Base::Adjoint;
714 
715  EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
716  using Base::operator*=;
717  using Base::operator*;
718 
719  SOPHUS_FUNC Map(Scalar const* coeffs)
720  : so3_(coeffs),
721  translation_(coeffs + Sophus::SO3<Scalar>::num_parameters) {}
722 
723  // Accessor of SO3
724  //
725  SOPHUS_FUNC Map<Sophus::SO3<Scalar> const, Options> const& so3() const {
726  return so3_;
727  }
728 
729  // Accessor of translation vector
730  //
731  SOPHUS_FUNC Map<Sophus::Vector3<Scalar> const, Options> const& translation()
732  const {
733  return translation_;
734  }
735 
736  protected:
737  Map<Sophus::SO3<Scalar> const, Options> const so3_;
738  Map<Sophus::Vector3<Scalar> const, Options> const translation_;
739 };
740 }
741 
742 #endif
typename Base::Transformation Transformation
Definition: se3.hpp:710
typename Base::Tangent Tangent
Definition: se3.hpp:502
SOPHUS_FUNC Vector< Scalar, num_parameters > internalMultiplyByGenerator(int i) const
Definition: se3.hpp:100
Matrix< Scalar, 4, 4 > Matrix4
Definition: types.hpp:51
static SOPHUS_FUNC Transformation generator(int i)
Definition: se3.hpp:352
SOPHUS_FUNC Map< Sophus::Vector3< Scalar > > const & translation() const
Definition: se3.hpp:691
#define SOPHUS_ENSURE(expr, description,...)
Definition: common.hpp:129
typename Base::Adjoint Adjoint
Definition: se3.hpp:503
SOPHUS_FUNC void setQuaternion(Eigen::Quaternion< Scalar > const &quat)
Definition: se3.hpp:227
static SOPHUS_FUNC Transformation hat(Tangent const &omega)
Definition: so3.hpp:368
SOPHUS_FUNC Matrix3< Scalar > rotationMatrix() const
Definition: se3.hpp:211
SOPHUS_FUNC void normalize()
Definition: se3.hpp:143
SOPHUS_FUNC Transformation matrix() const
Definition: se3.hpp:155
typename Base::Transformation Transformation
Definition: se3.hpp:660
static SOPHUS_FUNC Transformation hat(Tangent const &a)
Definition: se3.hpp:394
Map< Sophus::Vector3< Scalar > const, Options > const translation_
Definition: se3.hpp:738
SOPHUS_FUNC Point operator*(Point const &p) const
Definition: se3.hpp:197
SOPHUS_FUNC SE3(Matrix4< Scalar > const &T)
Definition: se3.hpp:555
SOPHUS_FUNC Matrix< Scalar, num_parameters, DoF > internalJacobian() const
Definition: se3.hpp:117
SOPHUS_FUNC Map(Scalar *coeffs)
Definition: se3.hpp:669
SOPHUS_FUNC Scalar const * data() const
Definition: se3.hpp:618
SOPHUS_FUNC TranslationType const & translation() const
Definition: se3.hpp:247
SOPHUS_FUNC SO3< Scalar > inverse() const
Definition: so3.hpp:144
SOPHUS_FUNC SE3(Eigen::Quaternion< Scalar > const &quaternion, Point const &translation)
Definition: se3.hpp:546
T value(details::expression_node< T > *n)
Definition: exprtk.hpp:12104
SOPHUS_FUNC SO3Type const & so3() const
Definition: se3.hpp:219
const T1 const T2
Definition: exprtk.hpp:13530
typename Base::Transformation Transformation
Definition: se3.hpp:500
SOPHUS_FUNC Map(Scalar const *coeffs)
Definition: se3.hpp:719
SOPHUS_FUNC SO3Type & so3()
Definition: se3.hpp:215
Vector< Scalar, DoF > Tangent
Definition: se3.hpp:72
SOPHUS_FUNC void setRotationMatrix(Matrix3< Scalar > const &rotation_matrix)
Definition: se3.hpp:235
static SOPHUS_FUNC SE3 transX(Scalar const &x)
Definition: se3.hpp:572
SOPHUS_FUNC TranslationMember const & translation() const
Definition: se3.hpp:637
EIGEN_MAKE_ALIGNED_OPERATOR_NEW SOPHUS_FUNC SE3()
Definition: se3.hpp:511
static SOPHUS_FUNC SE3 transZ(Scalar const &z)
Definition: se3.hpp:584
static SOPHUS_FUNC SE3 rotX(Scalar const &x)
Definition: se3.hpp:590
static SOPHUS_FUNC SE3 rotZ(Scalar const &z)
Definition: se3.hpp:602
static SOPHUS_FUNC Tangent vee(Transformation const &Omega)
Definition: se3.hpp:484
SOPHUS_FUNC SE3< Scalar > operator*(SE3< Scalar > const &other) const
Definition: se3.hpp:183
Eigen::Matrix< Scalar, M, N > Matrix
Definition: types.hpp:38
TranslationMember translation_
Definition: se3.hpp:643
static SOPHUS_FUNC Scalar epsilon()
Definition: common.hpp:139
SOPHUS_FUNC Scalar * data()
Definition: se3.hpp:611
Map< Sophus::SO3< Scalar >, Options > so3_
Definition: se3.hpp:696
static SOPHUS_FUNC void internalGenerator(int i, Eigen::Quaternion< Scalar > *internal_gen_q)
Definition: so3.hpp:344
typename SO3Type::QuaternionType QuaternionType
Definition: se3.hpp:61
Matrix< Scalar, DoF, DoF > Adjoint
Definition: se3.hpp:73
Vector< Scalar, 3, Options > Vector3
Definition: types.hpp:18
Map< Sophus::Vector3< Scalar > const, Options > TranslationType
Definition: se3.hpp:35
SOPHUS_FUNC Tangent log() const
Definition: se3.hpp:136
Scalar_ Scalar
Definition: se3.hpp:499
SOPHUS_FUNC Adjoint Adj() const
Definition: se3.hpp:80
SOPHUS_FUNC SE3(SO3Base< OtherDerived > const &so3, Eigen::MatrixBase< D > const &translation)
Definition: se3.hpp:525
SO3Member so3_
Definition: se3.hpp:642
const e_voc T1 e_none const e_none T1 e_none T0
Definition: exprtk.hpp:13518
SOPHUS_FUNC SE3(Matrix3< Scalar > const &rotation_matrix, Point const &translation)
Definition: se3.hpp:539
Matrix< Scalar, N, N > Transformation
Definition: se3.hpp:70
SOPHUS_FUNC Map< Sophus::SO3< Scalar >, Options > & so3()
Definition: se3.hpp:675
SOPHUS_FUNC Map< Sophus::SO3< Scalar >, Options > const & so3() const
Definition: se3.hpp:679
static SOPHUS_FUNC void internalGenerator(int i, Eigen::Quaternion< Scalar > *internal_gen_q, Vector3< Scalar > *internal_gen_t)
Definition: se3.hpp:364
SOPHUS_FUNC SE3Base< Derived > & operator=(SE3Base< OtherDerived > const &other)
Definition: se3.hpp:175
static SOPHUS_FUNC SE3 transY(Scalar const &y)
Definition: se3.hpp:578
SOPHUS_FUNC Map< Sophus::Vector3< Scalar > const, Options > const & translation() const
Definition: se3.hpp:731
SOPHUS_FUNC QuaternionType const & unit_quaternion() const
Definition: se3.hpp:253
SOPHUS_FUNC Transformation matrix() const
Definition: so3.hpp:172
static SOPHUS_FUNC Adjoint d_lieBracketab_by_d_a(Tangent const &b)
Definition: se3.hpp:268
Eigen::Matrix< Scalar, M, 1, Options > Vector
Definition: types.hpp:10
Matrix< Scalar, 3, 3 > Matrix3
Definition: types.hpp:46
Map< Sophus::SO3< Scalar > const, Options > SO3Type
Definition: se3.hpp:36
SOPHUS_FUNC SO3Member & so3()
Definition: se3.hpp:625
SOPHUS_FUNC SO3Member const & so3() const
Definition: se3.hpp:629
static SOPHUS_FUNC SE3 trans(T0 const &x, T1 const &y, T2 const &z)
Definition: se3.hpp:566
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()
Definition: se3.hpp:241
typename Base::Point Point
Definition: se3.hpp:501
static SOPHUS_FUNC Tangent lieBracket(Tangent const &a, Tangent const &b)
Definition: se3.hpp:412
SOPHUS_FUNC SE3(SE3Base< OtherDerived > const &other)
Definition: se3.hpp:516
Map< Sophus::SO3< Scalar > const, Options > const so3_
Definition: se3.hpp:737
typename Eigen::internal::traits< SE3< Scalar_, Options > >::Scalar Scalar
Definition: se3.hpp:57
static SOPHUS_FUNC Tangent log(SE3< Scalar > const &se3)
Definition: se3.hpp:435
SOPHUS_FUNC SE3< NewScalarType > cast() const
Definition: se3.hpp:93
static SOPHUS_FUNC SE3 rotY(Scalar const &y)
Definition: se3.hpp:596
static SOPHUS_FUNC SE3< Scalar > exp(Tangent const &a)
Definition: se3.hpp:293
Map< Sophus::Vector3< Scalar >, Options > translation_
Definition: se3.hpp:697
SOPHUS_FUNC TranslationMember & translation()
Definition: se3.hpp:633
SOPHUS_FUNC Matrix< Scalar, 3, 4 > matrix3x4() const
Definition: se3.hpp:165
static SOPHUS_FUNC SO3< Scalar > expAndTheta(Tangent const &omega, Scalar *theta)
Definition: so3.hpp:279
SOPHUS_FUNC SE3< Scalar > inverse() const
Definition: se3.hpp:127
SOPHUS_FUNC Map< Sophus::SO3< Scalar > const, Options > const & so3() const
Definition: se3.hpp:725
SOPHUS_FUNC Map< Sophus::Vector3< Scalar > > & translation()
Definition: se3.hpp:685
typename Eigen::internal::traits< SE3< Scalar_, Options > >::TranslationType TranslationType
Definition: se3.hpp:59
typename Eigen::internal::traits< SE3< Scalar_, Options > >::SO3Type SO3Type
Definition: se3.hpp:60
Sophus::Vector3< Scalar, Options > TranslationType
Definition: se3.hpp:19
#define SOPHUS_FUNC
Definition: common.hpp:32
SOPHUS_FUNC SE3Base< Derived > & operator*=(SE3< Scalar > const &other)
Definition: se3.hpp:203
Vector3< Scalar, Options > TranslationMember
Definition: se3.hpp:505



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