Main MRPT website > C++ reference for MRPT 1.5.3
so3.hpp
Go to the documentation of this file.
1 #ifndef SOPHUS_SO3_HPP
2 #define SOPHUS_SO3_HPP
3 
4 #include "types.hpp"
5 
6 // Include only the selective set of Eigen headers that we need.
7 // This helps when using Sophus with unusual compilers, like nvcc.
8 #include <Eigen/src/Geometry/OrthoMethods.h>
9 #include <Eigen/src/Geometry/Quaternion.h>
10 #include <Eigen/src/Geometry/RotationBase.h>
11 
12 namespace Sophus {
13 template <class Scalar_, int Options = 0>
14 class SO3;
15 using SO3d = SO3<double>;
16 using SO3f = SO3<float>;
17 } // namespace Sophus
18 
19 namespace Eigen {
20 namespace internal {
21 
22 template <class Scalar_, int Options>
23 struct traits<Sophus::SO3<Scalar_, Options>> {
24  using Scalar = Scalar_;
25  using QuaternionType = Eigen::Quaternion<Scalar, Options>;
26 };
27 
28 template <class Scalar_, int Options>
29 struct traits<Map<Sophus::SO3<Scalar_>, Options>>
30  : traits<Sophus::SO3<Scalar_, Options>> {
31  using Scalar = Scalar_;
32  using QuaternionType = Map<Eigen::Quaternion<Scalar>, Options>;
33 };
34 
35 template <class Scalar_, int Options>
36 struct traits<Map<Sophus::SO3<Scalar_> const, Options>>
37  : traits<Sophus::SO3<Scalar_, Options> const> {
38  using Scalar = Scalar_;
39  using QuaternionType = Map<Eigen::Quaternion<Scalar> const, Options>;
40 };
41 } // namespace internal
42 } // namespace Eigen
43 
44 namespace Sophus {
45 
46 // SO3 base type - implements SO3 class but is storage agnostic.
47 //
48 // SO(3) is the group of rotations in 3d. As a matrix group, it is the set of
49 // matrices which are orthogonal such that ``R * R' = I`` (with ``R'`` being the
50 // transpose of ``R``) and have a positive determinant. In particular, the
51 // determinant is 1. Internally, the group is represented as a unit quaternion.
52 // Unit quaternion can be seen as members of the special unitary group SU(2).
53 // SU(2) is a double cover of SO(3). Hence, for every rotation matrix ``R``,
54 // there exists two unit quaternion: ``(r, v)`` and ``(r, -v)``, with ``r`` the
55 // real part and ``v`` being the imaginary 3-vector part of the quaternion.
56 //
57 // SO(3) is a compact, but non-commutative group. First it is compact since the
58 // set of rotation matrices is a closed and bounded set. Second it is
59 // non-commutative since the equation ``R_1 * R_2 = R_2 * R_1`` does not hold in
60 // general. For example rotating an object by some degrees about its ``x``-axis
61 // and then by some degrees about its y axis, does not lead to the same
62 // orienation when rotation first about ``y`` and then about ``x``.
63 //
64 // Class invairant: The 2-norm of ``unit_quaternion`` must be close to 1.
65 // Technically speaking, it must hold that:
66 //
67 // ``|unit_quaternion().squaredNorm() - 1| <= Constants<Scalar>::epsilon()``.
68 template <class Derived>
69 class SO3Base {
70  public:
71  using Scalar = typename Eigen::internal::traits<Derived>::Scalar;
72  using QuaternionType =
73  typename Eigen::internal::traits<Derived>::QuaternionType;
74 
75  // Degrees of freedom of group, number of dimensions in tangent space.
76  static int constexpr DoF = 3;
77  // Number of internal parameters used (quaternion is a 4-tuple).
78  static int constexpr num_parameters = 4;
79  // Group transformations are 3x3 matrices.
80  static int constexpr N = 3;
85 
86  // Adjoint transformation
87  //
88  // This function return the adjoint transformation ``Ad`` of the group
89  // element ``A`` such that for all ``x`` it holds that
90  // ``hat(Ad_A * x) = A * hat(x) A^{-1}``. See hat-operator below.
91  //
92  // For SO(3), it simply returns the rotation matrix corresponding to ``A``.
93  //
94  SOPHUS_FUNC Adjoint Adj() const { return matrix(); }
95 
96  // Returns copy of instance casted to NewScalarType.
97  //
98  template <class NewScalarType>
100  return SO3<NewScalarType>(unit_quaternion().template cast<NewScalarType>());
101  }
102 
103  // This provides unsafe read/write access to internal data. SO(3) is
104  // represented by an Eigen::Quaternion (four parameters). When using direct
105  // write access, the user needs to take care of that the quaternion stays
106  // normalized.
107  //
108  // Note: The first three Scalars represent the imaginary parts, while the
109  // forth Scalar represent the real part.
110  //
112  return unit_quaternion_nonconst().coeffs().data();
113  }
114 
115  // Const version of data() above.
116  //
117  SOPHUS_FUNC Scalar const* data() const {
118  return unit_quaternion().coeffs().data();
119  }
120 
121  // Returns ``*this`` times the ith generator of internal SU(2) representation.
122  //
124  int i) const {
126  Eigen::Quaternion<Scalar> internal_gen_q;
127  internalGenerator(i, &internal_gen_q);
128  res.template head<4>() = (unit_quaternion() * internal_gen_q).coeffs();
129  return res;
130  }
131 
132  // Returns Jacobian of generator of internal SU(2) representation.
133  //
136  for (int i = 0; i < DoF; ++i) {
137  J.col(i) = internalMultiplyByGenerator(i);
138  }
139  return J;
140  }
141 
142  // Returns group inverse.
143  //
145  return SO3<Scalar>(unit_quaternion().conjugate());
146  }
147 
148  // Logarithmic map
149  //
150  // Returns tangent space representation (= rotation vector) of the instance.
151  //
152  SOPHUS_FUNC Tangent log() const { return SO3<Scalar>::log(*this); }
153 
154  // It re-normalizes ``unit_quaternion`` to unit length.
155  //
156  // Note: Because of the class invariant, there is typically no need to call
157  // this function directly.
158  //
160  Scalar length = unit_quaternion_nonconst().norm();
162  "Quaternion (%) should not be close to zero!",
163  unit_quaternion_nonconst().coeffs().transpose());
164  unit_quaternion_nonconst().coeffs() /= length;
165  }
166 
167  // Returns 3x3 matrix representation of the instance.
168  //
169  // For SO(3), the matrix representation is an orthogonal matrix ``R`` with
170  // ``det(R)=1``, thus the so-called "rotation matrix".
171  //
173  return unit_quaternion().toRotationMatrix();
174  }
175 
176  // Assignment operator.
177  //
178  template <class OtherDerived>
180  unit_quaternion_nonconst() = other.unit_quaternion();
181  return *this;
182  }
183 
184  // Group multiplication, which is rotation concatenation.
185  //
187  SO3<Scalar> result(*this);
188  result *= other;
189  return result;
190  }
191 
192  // Group action on 3-points.
193  //
194  // This function rotates a 3 dimensional point ``p`` by the SO3 element
195  // ``bar_R_foo`` (= rotation matrix): ``p_bar = bar_R_foo * p_foo``.
196  //
197  // Since SO3 is internally represented by a unit quaternion ``q``, it is
198  // implemented as ``p_bar = q * p_foo * q^{*}``
199  // with ``q^{*}`` being the quaternion conjugate of ``q``.
200  //
201  // Geometrically, ``p`` is rotated by angle ``|omega|`` around the
202  // axis ``omega/|omega|`` with ``omega := vee(log(bar_R_foo))``.
203  //
204  // For ``vee``-operator, see below.
205  //
206  SOPHUS_FUNC Point operator*(Point const& p) const {
207  return unit_quaternion()._transformVector(p);
208  }
209 
210  // In-place group multiplication.
211  //
213  unit_quaternion_nonconst() *= other.unit_quaternion();
214 
215  Scalar squared_norm = unit_quaternion().squaredNorm();
216 
217  // We can assume that the squared-norm is close to 1 since we deal with a
218  // unit quaternion. Due to numerical precision issues, there might
219  // be a small drift after pose concatenation. Hence, we need to renormalizes
220  // the quaternion here.
221  // Since squared-norm is close to 1, we do not need to calculate the costly
222  // square-root, but can use an approximation around 1 (see
223  // http://stackoverflow.com/a/12934750 for details).
224  if (squared_norm != Scalar(1.0)) {
225  unit_quaternion_nonconst().coeffs() *=
226  Scalar(2.0) / (Scalar(1.0) + squared_norm);
227  }
228  return *this;
229  }
230 
231  // Takes in quaternion, and normalizes it.
232  //
233  // Precondition: The quaternion must not be close to zero.
234  //
235  SOPHUS_FUNC void setQuaternion(Eigen::Quaternion<Scalar> const& quaternion) {
236  unit_quaternion_nonconst() = quaternion;
237  normalize();
238  }
239 
240  // Accessor of unit quaternion.
241  //
243  return static_cast<Derived const*>(this)->unit_quaternion();
244  }
245 
246  ////////////////////////////////////////////////////////////////////////////
247  // public static functions
248  ////////////////////////////////////////////////////////////////////////////
249 
250  // Derivative of Lie bracket with respect to first element.
251  //
252  // This function returns ``D_a [a, b]`` with ``D_a`` being the
253  // differential operator with respect to ``a``, ``[a, b]`` being the lie
254  // bracket of the Lie algebra so3.
255  // See ``lieBracket()`` below.
256  //
258  return -hat(b);
259  }
260 
261  // Group exponential
262  //
263  // This functions takes in an element of tangent space (= rotation vector
264  // ``omega``) and returns the corresponding element of the group SO(3).
265  //
266  // To be more specific, this function computes ``expmat(hat(omega))``
267  // with ``expmat(.)`` being the matrix exponential and ``hat(.)`` being the
268  // hat()-operator of SO(3).
269  //
270  SOPHUS_FUNC static SO3<Scalar> exp(Tangent const& omega) {
271  Scalar theta;
272  return expAndTheta(omega, &theta);
273  }
274 
275  // As above, but also returns ``theta = |omega|`` as out-parameter.
276  //
277  // Precondition: ``theta`` must not be ``nullptr``.
278  //
280  Scalar* theta) {
281  using std::sqrt;
282  using std::abs;
283  using std::sin;
284  using std::cos;
285  Scalar theta_sq = omega.squaredNorm();
286  *theta = sqrt(theta_sq);
287  Scalar half_theta = Scalar(0.5) * (*theta);
288 
289  Scalar imag_factor;
290  Scalar real_factor;
291  if ((*theta) < Constants<Scalar>::epsilon()) {
292  Scalar theta_po4 = theta_sq * theta_sq;
293  imag_factor = Scalar(0.5) - Scalar(1.0 / 48.0) * theta_sq +
294  Scalar(1.0 / 3840.0) * theta_po4;
295  real_factor =
296  Scalar(1) - Scalar(0.5) * theta_sq + Scalar(1.0 / 384.0) * theta_po4;
297  } else {
298  Scalar sin_half_theta = sin(half_theta);
299  imag_factor = sin_half_theta / (*theta);
300  real_factor = cos(half_theta);
301  }
302 
303  Derived q;
304  q.unit_quaternion_nonconst() =
305  QuaternionType(real_factor, imag_factor * omega.x(),
306  imag_factor * omega.y(), imag_factor * omega.z());
307  SOPHUS_ENSURE(abs(q.unit_quaternion().squaredNorm() - Scalar(1)) <
309  "SO3::exp failed! omega: %, real: %, img: %",
310  omega.transpose(), real_factor, imag_factor);
311  return q;
312  }
313 
314  // Returns the ith infinitesimal generators of SO(3).
315  //
316  // The infinitesimal generators of SO(3) are:
317  //
318  // | 0 0 0 |
319  // G_0 = | 0 0 -1 |
320  // | 0 1 0 |
321  //
322  // | 0 0 1 |
323  // G_1 = | 0 0 0 |
324  // | -1 0 0 |
325  //
326  // | 0 -1 0 |
327  // G_2 = | 1 0 0 |
328  // | 0 0 0 |
329  //
330  // Precondition: ``i`` must be 0, 1 or 2.
331  //
333  SOPHUS_ENSURE(i >= 0 && i <= 2, "i should be in range [0,2].");
334  Tangent e;
335  e.setZero();
336  e[i] = Scalar(1);
337  return hat(e);
338  }
339 
340  // Returns the ith generator of internal SU(2) representation.
341  //
342  // Precondition: ``i`` must be 0, 1 or 2.
343  //
345  int i, Eigen::Quaternion<Scalar>* internal_gen_q) {
346  SOPHUS_ENSURE(i >= 0 && i <= 2, "i should be in range [0,2]");
347  SOPHUS_ENSURE(internal_gen_q != NULL,
348  "internal_gen_q must not be the null pointer");
349  // Factor of 0.5 since SU(2) is a double cover of SO(3).
350  internal_gen_q->coeffs().setZero();
351  internal_gen_q->coeffs()[i] = Scalar(0.5);
352  }
353 
354  // hat-operator
355  //
356  // It takes in the 3-vector representation ``omega`` (= rotation vector) and
357  // returns the corresponding matrix representation of Lie algebra element.
358  //
359  // Formally, the ``hat()`` operator of SO(3) is defined as
360  //
361  // ``hat(.): R^3 -> R^{3x3}, hat(omega) = sum_i omega_i * G_i``
362  // (for i=0,1,2)
363  //
364  // with ``G_i`` being the ith infinitesimal generator of SO(3).
365  //
366  // The corresponding inverse is the ``vee``-operator, see below.
367  //
368  SOPHUS_FUNC static Transformation hat(Tangent const& omega) {
369  Transformation Omega;
370  // clang-format off
371  Omega <<
372  Scalar(0), -omega(2), omega(1),
373  omega(2), Scalar(0), -omega(0),
374  -omega(1), omega(0), Scalar(0);
375  // clang-format on
376  return Omega;
377  }
378 
379  // Lie bracket
380  //
381  // It computes the Lie bracket of SO(3). To be more specific, it computes
382  //
383  // ``[omega_1, omega_2]_so3 := vee([hat(omega_1), hat(omega_2)])``
384  //
385  // with ``[A,B] := AB-BA`` being the matrix commutator, ``hat(.) the
386  // hat-operator and ``vee(.)`` the vee-operator of SO3.
387  //
388  // For the Lie algebra so3, the Lie bracket is simply the cross product:
389  //
390  // ``[omega_1, \omega_2]_so3 = omega_1 x \omega_2.``
391  //
392  SOPHUS_FUNC static Tangent lieBracket(Tangent const& omega1,
393  Tangent const& omega2) {
394  return omega1.cross(omega2);
395  }
396 
397  // Logarithmic map
398  //
399  // Computes the logarithm, the inverse of the group exponential which maps
400  // element of the group (rotation matrices) to elements of the tangent space
401  // (rotation-vector).
402  //
403  // To be specific, this function computes ``vee(logmat(.))`` with
404  // ``logmat(.)`` being the matrix logarithm and ``vee(.)`` the vee-operator
405  // of SO(3).
406  //
407  SOPHUS_FUNC static Tangent log(SO3<Scalar> const& other) {
408  Scalar theta;
409  return logAndTheta(other, &theta);
410  }
411 
412  // As above, but also returns ``theta = |omega|`` as out-parameter.
413  //
415  Scalar* theta) {
416  using std::sqrt;
417  using std::atan;
418  using std::abs;
419  Scalar squared_n = other.unit_quaternion().vec().squaredNorm();
420  Scalar n = sqrt(squared_n);
421  Scalar w = other.unit_quaternion().w();
422 
423  Scalar two_atan_nbyw_by_n;
424 
425  // Atan-based log thanks to
426  //
427  // C. Hertzberg et al.:
428  // "Integrating Generic Sensor Fusion Algorithms with Sound State
429  // Representation through Encapsulation of Manifolds"
430  // Information Fusion, 2011
431 
432  if (n < Constants<Scalar>::epsilon()) {
433  // If quaternion is normalized and n=0, then w should be 1;
434  // w=0 should never happen here!
436  "Quaternion (%) should be normalized!",
437  other.unit_quaternion().coeffs().transpose());
438  Scalar squared_w = w * w;
439  two_atan_nbyw_by_n =
440  Scalar(2) / w - Scalar(2) * (squared_n) / (w * squared_w);
441  } else {
442  if (abs(w) < Constants<Scalar>::epsilon()) {
443  if (w > Scalar(0)) {
444  two_atan_nbyw_by_n = Constants<Scalar>::pi() / n;
445  } else {
446  two_atan_nbyw_by_n = -Constants<Scalar>::pi() / n;
447  }
448  } else {
449  two_atan_nbyw_by_n = Scalar(2) * atan(n / w) / n;
450  }
451  }
452 
453  *theta = two_atan_nbyw_by_n * n;
454 
455  return two_atan_nbyw_by_n * other.unit_quaternion().vec();
456  }
457 
458  // vee-operator
459  //
460  // It takes the 3x3-matrix representation ``Omega`` and maps it to the
461  // corresponding vector representation of Lie algebra.
462  //
463  // This is the inverse of the hat-operator, see above.
464  //
465  // Precondition: ``Omega`` must have the following structure:
466  //
467  // | 0 -c b |
468  // | c 0 -a |
469  // | -b a 0 | .
470  //
471  SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
472  using std::abs;
473  return Tangent(Omega(2, 1), Omega(0, 2), Omega(1, 0));
474  }
475 
476  private:
477  // Mutator of unit_quaternion is private to ensure class invariant. That is
478  // the quaternion must stay close to unit length.
479  //
481  return static_cast<Derived*>(this)->unit_quaternion_nonconst();
482  }
483 };
484 
485 // SO3 default type - Constructors and default storage for SO3 Type.
486 template <class Scalar_, int Options>
487 class SO3 : public SO3Base<SO3<Scalar_, Options>> {
489 
490  public:
491  using Scalar = Scalar_;
493  using Point = typename Base::Point;
494  using Tangent = typename Base::Tangent;
495  using Adjoint = typename Base::Adjoint;
496  using QuaternionMember = Eigen::Quaternion<Scalar, Options>;
497 
498  // ``Base`` is friend so unit_quaternion_nonconst can be accessed from
499  // ``Base``.
500  friend class SO3Base<SO3<Scalar, Options>>;
501 
502  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
503 
504  // Default constructor initialize unit quaternion to identity rotation.
505  //
507  : unit_quaternion_(Scalar(1), Scalar(0), Scalar(0), Scalar(0)) {}
508 
509  // Copy constructor
510  //
511  template <class OtherDerived>
513  : unit_quaternion_(other.unit_quaternion()) {}
514 
515  // Constructor from rotation matrix
516  //
517  // Precondition: rotation matrix needs to be orthogonal with determinant of 1.
518  //
519  SOPHUS_FUNC SO3(Transformation const& R) : unit_quaternion_(R) {}
520 
521  // Constructor from quaternion
522  //
523  // Precondition: quaternion must not be close to zero.
524  //
525  template <class D>
526  SOPHUS_FUNC explicit SO3(Eigen::QuaternionBase<D> const& quat)
527  : unit_quaternion_(quat) {
528  static_assert(
529  std::is_same<typename Eigen::QuaternionBase<D>::Scalar, Scalar>::value,
530  "Input must be of same scalar type");
531  Base::normalize();
532  }
533 
534  // Contruct x-axis rotation.
535  //
536  static SOPHUS_FUNC SO3 rotX(Scalar const& x) {
537  return SO3::exp(Sophus::Vector3<Scalar>(x, Scalar(0), Scalar(0)));
538  }
539 
540  // Contruct y-axis rotation.
541  //
542  static SOPHUS_FUNC SO3 rotY(Scalar const& y) {
543  return SO3::exp(Sophus::Vector3<Scalar>(Scalar(0), y, Scalar(0)));
544  }
545 
546  // Contruct z-axis rotation.
547  //
548  static SOPHUS_FUNC SO3 rotZ(Scalar const& z) {
549  return SO3::exp(Sophus::Vector3<Scalar>(Scalar(0), Scalar(0), z));
550  }
551 
552  // Accessor of unit quaternion.
553  //
555  return unit_quaternion_;
556  }
557 
558  protected:
559  // Mutator of unit_quaternion is protected to ensure class invariant.
560  //
562  return unit_quaternion_;
563  }
564 
566 };
567 
568 } // namespace Sophus
569 
570 namespace Eigen {
571 
572 // Specialization of Eigen::Map for ``SO3``.
573 //
574 // Allows us to wrap SO3 objects around POD array (e.g. external c style
575 // quaternion).
576 template <class Scalar_, int Options>
577 class Map<Sophus::SO3<Scalar_>, Options>
578  : public Sophus::SO3Base<Map<Sophus::SO3<Scalar_>, Options>> {
580 
581  public:
582  using Scalar = Scalar_;
584  using Point = typename Base::Point;
585  using Tangent = typename Base::Tangent;
586  using Adjoint = typename Base::Adjoint;
587 
588  // ``Base`` is friend so unit_quaternion_nonconst can be accessed from
589  // ``Base``.
590  friend class Sophus::SO3Base<Map<Sophus::SO3<Scalar_>, Options>>;
591 
592  EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
593  using Base::operator*=;
594  using Base::operator*;
595 
596  SOPHUS_FUNC Map(Scalar* coeffs) : unit_quaternion_(coeffs) {}
597 
598  // Accessor of unit quaternion.
599  //
600  SOPHUS_FUNC Map<Eigen::Quaternion<Scalar>, Options> const& unit_quaternion()
601  const {
602  return unit_quaternion_;
603  }
604 
605  protected:
606  // Mutator of unit_quaternion is protected to ensure class invariant.
607  //
608  SOPHUS_FUNC Map<Eigen::Quaternion<Scalar>, Options>&
610  return unit_quaternion_;
611  }
612 
613  Map<Eigen::Quaternion<Scalar>, Options> unit_quaternion_;
614 };
615 
616 // Specialization of Eigen::Map for ``SO3 const``.
617 //
618 // Allows us to wrap SO3 objects around POD array (e.g. external c style
619 // quaternion).
620 template <class Scalar_, int Options>
621 class Map<Sophus::SO3<Scalar_> const, Options>
622  : public Sophus::SO3Base<Map<Sophus::SO3<Scalar_> const, Options>> {
624 
625  public:
626  using Scalar = Scalar_;
628  using Point = typename Base::Point;
629  using Tangent = typename Base::Tangent;
630  using Adjoint = typename Base::Adjoint;
631 
632  EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
633  using Base::operator*=;
634  using Base::operator*;
635 
636  SOPHUS_FUNC Map(Scalar const* coeffs) : unit_quaternion_(coeffs) {}
637 
638  // Accessor of unit quaternion.
639  //
640  SOPHUS_FUNC Map<Eigen::Quaternion<Scalar> const, Options> const&
641  unit_quaternion() const {
642  return unit_quaternion_;
643  }
644 
645  protected:
646  // Mutator of unit_quaternion is protected to ensure class invariant.
647  //
648  Map<Eigen::Quaternion<Scalar> const, Options> const unit_quaternion_;
649 };
650 }
651 
652 #endif
SOPHUS_FUNC Map< Eigen::Quaternion< Scalar >, Options > const & unit_quaternion() const
Definition: so3.hpp:600
typename Base::Tangent Tangent
Definition: so3.hpp:494
#define SOPHUS_ENSURE(expr, description,...)
Definition: common.hpp:129
static SOPHUS_FUNC Tangent log(SO3< Scalar > const &other)
Definition: so3.hpp:407
Matrix< Scalar, DoF, DoF > Adjoint
Definition: so3.hpp:84
Vector< Scalar, DoF > Tangent
Definition: so3.hpp:83
static SOPHUS_FUNC Transformation hat(Tangent const &omega)
Definition: so3.hpp:368
static SOPHUS_FUNC Adjoint d_lieBracketab_by_d_a(Tangent const &b)
Definition: so3.hpp:257
EIGEN_MAKE_ALIGNED_OPERATOR_NEW SOPHUS_FUNC SO3()
Definition: so3.hpp:506
Matrix< Scalar, N, N > Transformation
Definition: so3.hpp:81
static SOPHUS_FUNC Tangent vee(Transformation const &Omega)
Definition: so3.hpp:471
SOPHUS_FUNC QuaternionMember const & unit_quaternion() const
Definition: so3.hpp:554
QuaternionMember unit_quaternion_
Definition: so3.hpp:565
static SOPHUS_FUNC SO3 rotX(Scalar const &x)
Definition: so3.hpp:536
Map< Eigen::Quaternion< Scalar >, Options > unit_quaternion_
Definition: so3.hpp:613
SOPHUS_FUNC SO3< Scalar > inverse() const
Definition: so3.hpp:144
SOPHUS_FUNC Scalar const * data() const
Definition: so3.hpp:117
SOPHUS_FUNC QuaternionType & unit_quaternion_nonconst()
Definition: so3.hpp:480
T value(details::expression_node< T > *n)
Definition: exprtk.hpp:12104
typename Base::Adjoint Adjoint
Definition: so3.hpp:495
SOPHUS_FUNC SO3< NewScalarType > cast() const
Definition: so3.hpp:99
static SOPHUS_FUNC Scalar pi()
Definition: common.hpp:141
static SOPHUS_FUNC SO3< Scalar > exp(Tangent const &omega)
Definition: so3.hpp:270
static SOPHUS_FUNC Transformation generator(int i)
Definition: so3.hpp:332
SOPHUS_FUNC void setQuaternion(Eigen::Quaternion< Scalar > const &quaternion)
Definition: so3.hpp:235
Map< Eigen::Quaternion< Scalar > const, Options > QuaternionType
Definition: so3.hpp:39
SOPHUS_FUNC SO3(Transformation const &R)
Definition: so3.hpp:519
auto transpose(T const &p) -> decltype(details::Transpose< T >::impl(T()))
Definition: types.hpp:150
SOPHUS_FUNC Map< Eigen::Quaternion< Scalar > const, Options > const & unit_quaternion() const
Definition: so3.hpp:641
Eigen::Matrix< Scalar, M, N > Matrix
Definition: types.hpp:38
SOPHUS_FUNC Vector< Scalar, num_parameters > internalMultiplyByGenerator(int i) const
Definition: so3.hpp:123
static SOPHUS_FUNC Scalar epsilon()
Definition: common.hpp:139
static SOPHUS_FUNC void internalGenerator(int i, Eigen::Quaternion< Scalar > *internal_gen_q)
Definition: so3.hpp:344
SOPHUS_FUNC Tangent log() const
Definition: so3.hpp:152
Vector< Scalar, 3, Options > Vector3
Definition: types.hpp:18
SOPHUS_FUNC SO3Base< Derived > & operator*=(SO3< Scalar > const &other)
Definition: so3.hpp:212
SOPHUS_FUNC Matrix< Scalar, num_parameters, DoF > internalJacobian() const
Definition: so3.hpp:134
SOPHUS_FUNC Adjoint Adj() const
Definition: so3.hpp:94
Eigen::Quaternion< Scalar, Options > QuaternionType
Definition: so3.hpp:25
static SOPHUS_FUNC SO3 rotZ(Scalar const &z)
Definition: so3.hpp:548
void normalize(Scalar valMin, Scalar valMax)
Scales all elements such as the minimum & maximum values are shifted to the given values...
typename Base::Point Point
Definition: so3.hpp:493
static SOPHUS_FUNC Tangent logAndTheta(SO3< Scalar > const &other, Scalar *theta)
Definition: so3.hpp:414
typename Base::Transformation Transformation
Definition: so3.hpp:627
Eigen::Quaternion< Scalar, Options > QuaternionMember
Definition: so3.hpp:496
SOPHUS_FUNC SO3Base< Derived > & operator=(SO3Base< OtherDerived > const &other)
Definition: so3.hpp:179
typename Eigen::internal::traits< SO3< Scalar_, Options > >::QuaternionType QuaternionType
Definition: so3.hpp:73
SOPHUS_FUNC SO3< Scalar > operator*(SO3< Scalar > const &other) const
Definition: so3.hpp:186
typename Base::Transformation Transformation
Definition: so3.hpp:492
SOPHUS_FUNC Transformation matrix() const
Definition: so3.hpp:172
Eigen::Matrix< Scalar, M, 1, Options > Vector
Definition: types.hpp:10
SOPHUS_FUNC QuaternionMember & unit_quaternion_nonconst()
Definition: so3.hpp:561
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 SO3(SO3Base< OtherDerived > const &other)
Definition: so3.hpp:512
SOPHUS_FUNC SO3(Eigen::QuaternionBase< D > const &quat)
Definition: so3.hpp:526
Map< Eigen::Quaternion< Scalar > const, Options > const unit_quaternion_
Definition: so3.hpp:648
static SOPHUS_FUNC Tangent lieBracket(Tangent const &omega1, Tangent const &omega2)
Definition: so3.hpp:392
SOPHUS_FUNC Point operator*(Point const &p) const
Definition: so3.hpp:206
static SOPHUS_FUNC SO3< Scalar > expAndTheta(Tangent const &omega, Scalar *theta)
Definition: so3.hpp:279
static SOPHUS_FUNC SO3 rotY(Scalar const &y)
Definition: so3.hpp:542
SOPHUS_FUNC QuaternionType const & unit_quaternion() const
Definition: so3.hpp:242
SOPHUS_FUNC Scalar * data()
Definition: so3.hpp:111
SOPHUS_FUNC Map< Eigen::Quaternion< Scalar >, Options > & unit_quaternion_nonconst()
Definition: so3.hpp:609
typename Eigen::internal::traits< SO3< Scalar_, Options > >::Scalar Scalar
Definition: so3.hpp:71
#define SOPHUS_FUNC
Definition: common.hpp:32
SOPHUS_FUNC void normalize()
Definition: so3.hpp:159
SOPHUS_FUNC Map(Scalar const *coeffs)
Definition: so3.hpp:636



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