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



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