Main MRPT website > C++ reference for MRPT 1.5.3
rxso3.hpp
Go to the documentation of this file.
1 #ifndef SOPHUS_RXSO3_HPP
2 #define SOPHUS_RXSO3_HPP
3 
4 #include "so3.hpp"
5 
6 namespace Sophus {
7 template <class Scalar_, int Options = 0>
8 class RxSO3;
11 } // namespace Sophus
12 
13 namespace Eigen {
14 namespace internal {
15 
16 template <class Scalar_, int Options>
17 struct traits<Sophus::RxSO3<Scalar_, Options>> {
18  using Scalar = Scalar_;
19  using QuaternionType = Eigen::Quaternion<Scalar, Options>;
20 };
21 
22 template <class Scalar_, int Options>
23 struct traits<Map<Sophus::RxSO3<Scalar_>, Options>>
24  : traits<Sophus::RxSO3<Scalar_, Options>> {
25  using Scalar = Scalar_;
26  using QuaternionType = Map<Eigen::Quaternion<Scalar>, Options>;
27 };
28 
29 template <class Scalar_, int Options>
30 struct traits<Map<Sophus::RxSO3<Scalar_> const, Options>>
31  : traits<Sophus::RxSO3<Scalar_, Options> const> {
32  using Scalar = Scalar_;
33  using QuaternionType = Map<Eigen::Quaternion<Scalar> const, Options>;
34 };
35 } // namespace internal
36 } // namespace Eigen
37 
38 namespace Sophus {
39 
40 // RxSO3 base type - implements RxSO3 class but is storage agnostic
41 //
42 // This class implements the group ``R+ x SO(3)``, the direct product of the
43 // group of positive scalar 3x3 matrices (= isomorph to the positive
44 // real numbers) and the three-dimensional special orthogonal group SO(3).
45 // Geometrically, it is the group of rotation and scaling in three dimensions.
46 // As a matrix groups, RxSO3 consists of matrices of the form ``s * R``
47 // where ``R`` is an orthogonal matrix with ``det(R)=1`` and ``s > 0``
48 // being a positive real number.
49 //
50 // Internally, RxSO3 is represented by the group of non-zero quaternions.
51 // In particular, the scale equals the squared(!) norm of the quaternion ``q``,
52 // ``s = |q|^2``. This is a most compact representation since the degrees of
53 // freedom (DoF) of RxSO3 (=4) equals the number of internal parameters (=4).
54 //
55 // This class has the explicit class invariant that the scale ``s`` is not
56 // too close to zero. Strictly speaking, it must hold that:
57 //
58 // ``quaternion().squaredNorm() >= Constants<Scalar>::epsilon()``.
59 //
60 // In order to obey this condition, group multiplication is implemented with
61 // saturation such that a product always has a scale which is equal or greater
62 // this threshold.
63 template <class Derived>
64 class RxSO3Base {
65  public:
66  using Scalar = typename Eigen::internal::traits<Derived>::Scalar;
67  using QuaternionType =
68  typename Eigen::internal::traits<Derived>::QuaternionType;
69 
70  // Degrees of freedom of manifold, number of dimensions in tangent space
71  // (three for rotation and one for scaling).
72  static int constexpr DoF = 4;
73  // Number of internal parameters used (quaternion is a 4-tuple).
74  static int constexpr num_parameters = 4;
75  // Group transformations are 3x3 matrices.
76  static int constexpr N = 3;
81 
82  // Adjoint transformation
83  //
84  // This function return the adjoint transformation ``Ad`` of the group
85  // element ``A`` such that for all ``x`` it holds that
86  // ``hat(Ad_A * x) = A * hat(x) A^{-1}``. See hat-operator below.
87  //
88  // For RxSO(3), it simply returns the rotation matrix corresponding to ``A``.
89  //
91  Adjoint res;
92  res.setIdentity();
93  res.template topLeftCorner<3, 3>() = rotationMatrix();
94  return res;
95  }
96 
97  // Returns copy of instance casted to NewScalarType.
98  //
99  template <class NewScalarType>
101  return RxSO3<NewScalarType>(quaternion().template cast<NewScalarType>());
102  }
103 
104  // This provides unsafe read/write access to internal data. RxSO(3) is
105  // represented by an Eigen::Quaternion (four parameters). When using direct
106  // write access, the user needs to take care of that the quaternion is not
107  // set close to zero.
108  //
109  // Note: The first three Scalars represent the imaginary parts, while the
110  // forth Scalar represent the real part.
111  //
112  SOPHUS_FUNC Scalar* data() { return quaternion_nonconst().coeffs().data(); }
113 
114  // Const version of data() above.
115  //
116  SOPHUS_FUNC Scalar const* data() const {
117  return quaternion().coeffs().data();
118  }
119 
120  // Returns group inverse.
121  //
123  return RxSO3<Scalar>(quaternion().inverse());
124  }
125 
126  // Logarithmic map
127  //
128  // Returns tangent space representation of the instance.
129  //
130  SOPHUS_FUNC Tangent log() const { return RxSO3<Scalar>::log(*this); }
131 
132  /**
133  * \returns 3x3 matrix representation of instance
134  *
135  * For RxSO3, the matrix representation is a scaled orthogonal
136  * matrix \f$ sR \f$ with \f$ det(sR)=s^3 \f$, thus a scaled rotation
137  * matrix \f$ R \f$ with scale s.
138  */
139 
140  // Returns 3x3 matrix representation of the instance.
141  //
142  // For RxSO3, the matrix representation is an scaled orthogonal matrix ``sR``
143  // with ``det(R)=s^3``, thus a scaled rotation matrix ``R`` with scale ``s``.
144  //
146  Transformation sR;
147 
148  Scalar const vx_sq = quaternion().vec().x() * quaternion().vec().x();
149  Scalar const vy_sq = quaternion().vec().y() * quaternion().vec().y();
150  Scalar const vz_sq = quaternion().vec().z() * quaternion().vec().z();
151  Scalar const w_sq = quaternion().w() * quaternion().w();
152  Scalar const two_vx = Scalar(2) * quaternion().vec().x();
153  Scalar const two_vy = Scalar(2) * quaternion().vec().y();
154  Scalar const two_vz = Scalar(2) * quaternion().vec().z();
155  Scalar const two_vx_vy = two_vx * quaternion().vec().y();
156  Scalar const two_vx_vz = two_vx * quaternion().vec().z();
157  Scalar const two_vx_w = two_vx * quaternion().w();
158  Scalar const two_vy_vz = two_vy * quaternion().vec().z();
159  Scalar const two_vy_w = two_vy * quaternion().w();
160  Scalar const two_vz_w = two_vz * quaternion().w();
161 
162  sR(0, 0) = vx_sq - vy_sq - vz_sq + w_sq;
163  sR(1, 0) = two_vx_vy + two_vz_w;
164  sR(2, 0) = two_vx_vz - two_vy_w;
165 
166  sR(0, 1) = two_vx_vy - two_vz_w;
167  sR(1, 1) = -vx_sq + vy_sq - vz_sq + w_sq;
168  sR(2, 1) = two_vx_w + two_vy_vz;
169 
170  sR(0, 2) = two_vx_vz + two_vy_w;
171  sR(1, 2) = -two_vx_w + two_vy_vz;
172  sR(2, 2) = -vx_sq - vy_sq + vz_sq + w_sq;
173  return sR;
174  }
175 
176  // Assignment operator.
177  //
178  template <class OtherDerived>
180  RxSO3Base<OtherDerived> const& other) {
181  quaternion() = other.quaternion();
182  return *this;
183  }
184 
185  // Group multiplication, which is rotation concatenation and scale
186  // multiplication.
187  //
188  // Note: This function performs saturation for products close to zero in order
189  // to ensure the class invariant.
190  //
192  RxSO3<Scalar> result(*this);
193  result *= other;
194  return result;
195  }
196 
197  // Group action on 3-points.
198  //
199  // This function rotates a 3 dimensional point ``p`` by the SO3 element
200  // ``bar_R_foo`` (= rotation matrix) and scales it by the scale factor ``s``:
201  //
202  // ``p_bar = s * (bar_R_foo * p_foo)``.
203  //
204  SOPHUS_FUNC Point operator*(Point const& p) const {
205  // Follows http://eigen.tuxfamily.org/bz/show_bug.cgi?id=459
206  Scalar scale = quaternion().squaredNorm();
207  Point two_vec_cross_p = quaternion().vec().cross(p);
208  two_vec_cross_p += two_vec_cross_p;
209  return scale * p + (quaternion().w() * two_vec_cross_p +
210  quaternion().vec().cross(two_vec_cross_p));
211  }
212 
213  // In-place group multiplication.
214  //
215  // Note: This function performs saturation for products close to zero in order
216  // to ensure the class invariant.
217  //
219  using std::sqrt;
220 
221  quaternion_nonconst() *= other.quaternion();
222  Scalar scale = this->scale();
223  if (scale < Constants<Scalar>::epsilon()) {
224  SOPHUS_ENSURE(scale > 0, "Scale must be greater zero.");
225  // Saturation to ensure class invariant.
226  quaternion_nonconst().normalize();
227  quaternion_nonconst().coeffs() *= sqrt(Constants<Scalar>::epsilon());
228  }
229  return *this;
230  }
231 
232  // Sets non-zero quaternion
233  //
234  // Precondition: ``quat`` must not be close to zero.
235  SOPHUS_FUNC void setQuaternion(Eigen::Quaternion<Scalar> const& quat) {
236  SOPHUS_ENSURE(quat.squaredNorm() > Constants<Scalar>::epsilon() *
238  "Scale factor must be greater-equal epsilon.");
239  static_cast<Derived*>(this)->quaternion_nonconst() = quat;
240  }
241 
242  // Accessor of quaternion.
243  //
245  return static_cast<Derived const*>(this)->quaternion();
246  }
247 
248  // Returns rotation matrix.
249  //
251  QuaternionType norm_quad = quaternion();
252  norm_quad.normalize();
253  return norm_quad.toRotationMatrix();
254  }
255 
256  // Returns scale.
257  //
259  Scalar scale() const { return quaternion().squaredNorm(); }
260 
261  // Setter of quaternion using rotation matrix ``R``, leaves scale as is.
262  //
264  using std::sqrt;
265  Scalar saved_scale = scale();
266  quaternion_nonconst() = R;
267  quaternion_nonconst().coeffs() *= std::sqrt(saved_scale);
268  }
269 
270  // Sets scale and leaves rotation as is.
271  //
272  // Note: This function as a significant computational cost, since it has to
273  // call the square root twice.
274  //
276  void setScale(Scalar const& scale) {
277  using std::sqrt;
278  quaternion_nonconst().normalize();
279  quaternion_nonconst().coeffs() *= sqrt(scale);
280  }
281 
282  // Setter of quaternion using scaled rotation matrix ``sR``.
283  //
284  // Precondition: The 3x3 matrix must be "scaled orthogonal"
285  // and have a positive determinant.
286  //
288  Transformation squared_sR = sR * sR.transpose();
289  Scalar squared_scale =
290  Scalar(1. / 3.) *
291  (squared_sR(0, 0) + squared_sR(1, 1) + squared_sR(2, 2));
292  SOPHUS_ENSURE(squared_scale > Constants<Scalar>::epsilon() *
294  "Scale factor must be greater-equal epsilon.");
295  Scalar scale = sqrt(squared_scale);
296  quaternion_nonconst() = sR / scale;
297  quaternion_nonconst().coeffs() *= sqrt(scale);
298  }
299 
300  // Setter of SO(3) rotations, leaves scale as is.
301  //
302  SOPHUS_FUNC void setSO3(SO3<Scalar> const& so3) {
303  using std::sqrt;
304  Scalar saved_scale = scale();
305  quaternion_nonconst() = so3.unit_quaternion();
306  quaternion_nonconst().coeffs() *= sqrt(saved_scale);
307  }
308 
309  SOPHUS_FUNC SO3<Scalar> so3() const { return SO3<Scalar>(quaternion()); }
310 
311  ////////////////////////////////////////////////////////////////////////////
312  // public static functions
313  ////////////////////////////////////////////////////////////////////////////
314 
315  // Derivative of Lie bracket with respect to first element.
316  //
317  // This function returns ``D_a [a, b]`` with ``D_a`` being the
318  // differential operator with respect to ``a``, ``[a, b]`` being the lie
319  // bracket of the Lie algebra rxso3.
320  // See ``lieBracket()`` below.
321  //
323  Adjoint res;
324  res.setZero();
325  res.template topLeftCorner<3, 3>() =
326  -SO3<Scalar>::hat(b.template head<3>());
327  return res;
328  }
329 
330  // Group exponential
331  //
332  // This functions takes in an element of tangent space (= rotation 3-vector
333  // plus logarithm of scale) and returns the corresponding element of the group
334  // RxSO3.
335  //
336  // To be more specific, thixs function computes ``expmat(hat(omega))``
337  // with ``expmat(.)`` being the matrix exponential and ``hat(.)`` being the
338  // hat()-operator of RSO3.
339  //
341  Scalar theta;
342  return expAndTheta(a, &theta);
343  }
344 
345  // As above, but also returns ``theta = |omega|`` as out-parameter.
346  //
347  // Precondition: ``theta`` must not be ``nullptr``.
348  //
350  Scalar* theta) {
351  SOPHUS_ENSURE(theta != nullptr, "must not be nullptr.");
352  using std::exp;
353  using std::sqrt;
354 
355  Vector3<Scalar> const omega = a.template head<3>();
356  Scalar sigma = a[3];
357  Scalar sqrt_scale = sqrt(exp(sigma));
358  Eigen::Quaternion<Scalar> quat =
359  SO3<Scalar>::expAndTheta(omega, theta).unit_quaternion();
360  quat.coeffs() *= sqrt_scale;
361  return RxSO3<Scalar>(quat);
362  }
363 
364  // Returns the ith infinitesimal generators of ``R+ x SO(3)``.
365  //
366  // The infinitesimal generators of RxSO3 are:
367  //
368  // | 0 0 0 |
369  // G_0 = | 0 0 -1 |
370  // | 0 1 0 |
371  //
372  // | 0 0 1 |
373  // G_1 = | 0 0 0 |
374  // | -1 0 0 |
375  //
376  // | 0 -1 0 |
377  // G_2 = | 1 0 0 |
378  // | 0 0 0 |
379  //
380  // | 1 0 0 |
381  // G_2 = | 0 1 0 |
382  // | 0 0 1 |
383  //
384  // Precondition: ``i`` must be 0, 1, 2 or 3.
385  //
387  SOPHUS_ENSURE(i >= 0 && i <= 3, "i should be in range [0,3].");
388  Tangent e;
389  e.setZero();
390  e[i] = Scalar(1);
391  return hat(e);
392  }
393 
394  // hat-operator
395  //
396  // It takes in the 4-vector representation ``a`` (= rotation vector plus
397  // logarithm of scale) and returns the corresponding matrix representation of
398  // Lie algebra element.
399  //
400  // Formally, the ``hat()`` operator of RxSO3 is defined as
401  //
402  // ``hat(.): R^4 -> R^{3x3}, hat(a) = sum_i a_i * G_i`` (for i=0,1,2,3)
403  //
404  // with ``G_i`` being the ith infinitesial generator of RxSO3.
405  //
406  // The corresponding inverse is the ``vee``-operator, see below.
407  //
409  Transformation A;
410  // clang-format off
411  A <<
412  a(3), -a(2), a(1),
413  a(2), a(3), -a(0),
414  -a(1), a(0), a(3);
415  // clang-format on
416  return A;
417  }
418 
419  // Lie bracket
420  //
421  // It computes the Lie bracket of RxSO(3). To be more specific, it computes
422  //
423  // ``[omega_1, omega_2]_rxso3 := vee([hat(omega_1), hat(omega_2)])``
424  //
425  // with ``[A,B] := AB-BA`` being the matrix commutator, ``hat(.) the
426  // hat-operator and ``vee(.)`` the vee-operator of RxSO3.
427  //
428  SOPHUS_FUNC static Tangent lieBracket(Tangent const& a, Tangent const& b) {
429  Vector3<Scalar> const omega1 = a.template head<3>();
430  Vector3<Scalar> const omega2 = b.template head<3>();
431  Vector4<Scalar> res;
432  res.template head<3>() = omega1.cross(omega2);
433  res[3] = Scalar(0);
434  return res;
435  }
436 
437  // Logarithmic map
438  //
439  // Computes the logarithm, the inverse of the group exponential which maps
440  // element of the group (scaled rotation matrices) to elements of the tangent
441  // space (rotation-vector plus logarithm of scale factor).
442  //
443  // To be specific, this function computes ``vee(logmat(.))`` with
444  // ``logmat(.)`` being the matrix logarithm and ``vee(.)`` the vee-operator
445  // of RxSO3.
446  //
447  SOPHUS_FUNC static Tangent log(RxSO3<Scalar> const& other) {
448  Scalar theta;
449  return logAndTheta(other, &theta);
450  }
451 
452  // As above, but also returns ``theta = |omega|`` as out-parameter.
453  //
455  Scalar* theta) {
456  using std::log;
457 
458  Scalar scale = other.quaternion().squaredNorm();
459  Tangent omega_sigma;
460  omega_sigma[3] = log(scale);
461  omega_sigma.template head<3>() =
463  return omega_sigma;
464  }
465 
466  // vee-operator
467  //
468  // It takes the 3x3-matrix representation ``Omega`` and maps it to the
469  // corresponding vector representation of Lie algebra.
470  //
471  // This is the inverse of the hat-operator, see above.
472  //
473  // Precondition: ``Omega`` must have the following structure:
474  //
475  // | d -c b |
476  // | c d -a |
477  // | -b a d | .
478  //
479  SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
480  using std::abs;
481  return Tangent(Omega(2, 1), Omega(0, 2), Omega(1, 0), Omega(0, 0));
482  }
483 
484  protected:
485  // Mutator of quaternion is private to ensure class invariant.
486  //
488  return static_cast<Derived*>(this)->quaternion_nonconst();
489  }
490 };
491 
492 // RxSO3 default type - Constructors and default storage for RxSO3 Type.
493 template <class Scalar_, int Options>
494 class RxSO3 : public RxSO3Base<RxSO3<Scalar_, Options>> {
496 
497  public:
498  using Scalar = Scalar_;
500  using Point = typename Base::Point;
501  using Tangent = typename Base::Tangent;
502  using Adjoint = typename Base::Adjoint;
503  using QuaternionMember = Eigen::Quaternion<Scalar, Options>;
504 
505  // ``Base`` is friend so quaternion_nonconst can be accessed from ``Base``.
506  friend class RxSO3Base<RxSO3<Scalar_, Options>>;
507 
508  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
509 
510  // Default constructor initialize quaternion to identity rotation and scale.
511  //
513  : quaternion_(Scalar(1), Scalar(0), Scalar(0), Scalar(0)) {}
514 
515  // Copy constructor
516  //
517  template <class OtherDerived>
519  : quaternion_(other.quaternion()) {}
520 
521  // Constructor from scaled rotation matrix
522  //
523  // Precondition: rotation matrix need to be scaled orthogonal with determinant
524  // of s^3.
525  //
526  SOPHUS_FUNC explicit RxSO3(Transformation const& sR) {
527  this->setScaledRotationMatrix(sR);
528  }
529 
530  // Constructor from scale factor and rotation matrix ``R``.
531  //
532  // Precondition: Rotation matrix ``R`` must to be orthogonal with determinant
533  // of 1 and ``scale`` must to be close to zero.
534  //
535  SOPHUS_FUNC RxSO3(Scalar const& scale, Transformation const& R)
536  : quaternion_(R) {
538  "Scale factor must be greater-equal epsilon.");
539  quaternion_.coeffs() *= std::sqrt(scale);
540  }
541 
542  // Constructor from scale factor and SO3
543  //
544  // Precondition: ``scale`` must to be close to zero.
545  //
546  SOPHUS_FUNC RxSO3(Scalar const& scale, SO3<Scalar> const& so3)
547  : quaternion_(so3.unit_quaternion()) {
549  "Scale factor must be greater-equal epsilon.");
550  quaternion_.coeffs() *= std::sqrt(scale);
551  }
552 
553  // Constructor from quaternion
554  //
555  // Precondition: quaternion must not be close to zero.
556  //
557  template <class D>
558  SOPHUS_FUNC explicit RxSO3(Eigen::QuaternionBase<D> const& quat)
559  : quaternion_(quat) {
561  "must be same Scalar type.");
562  SOPHUS_ENSURE(quaternion_.squaredNorm() > Constants<Scalar>::epsilon(),
563  "Scale factor must be greater-equal epsilon.");
564  }
565 
566  // Accessor of quaternion.
567  //
568  SOPHUS_FUNC QuaternionMember const& quaternion() const { return quaternion_; }
569 
570  protected:
572 
574 };
575 
576 } // namespace Sophus
577 
578 namespace Eigen {
579 
580 // Specialization of Eigen::Map for ``RxSO3``.
581 //
582 // Allows us to wrap RxSO3 objects around POD array (e.g. external c style
583 // quaternion).
584 template <class Scalar_, int Options>
585 class Map<Sophus::RxSO3<Scalar_>, Options>
586  : public Sophus::RxSO3Base<Map<Sophus::RxSO3<Scalar_>, Options>> {
588 
589  public:
590  using Scalar = Scalar_;
592  using Point = typename Base::Point;
593  using Tangent = typename Base::Tangent;
594  using Adjoint = typename Base::Adjoint;
595 
596  // ``Base`` is friend so quaternion_nonconst can be accessed from ``Base``.
597  friend class Sophus::RxSO3Base<Map<Sophus::RxSO3<Scalar_>, Options>>;
598 
599  EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
600  using Base::operator*=;
601  using Base::operator*;
602 
603  SOPHUS_FUNC Map(Scalar* coeffs) : quaternion_(coeffs) {}
604 
605  // Accessor of quaternion.
606  //
608  Map<Eigen::Quaternion<Scalar>, Options> const& quaternion() const {
609  return quaternion_;
610  }
611 
612  protected:
613  SOPHUS_FUNC Map<Eigen::Quaternion<Scalar>, Options>& quaternion_nonconst() {
614  return quaternion_;
615  }
616 
617  Map<Eigen::Quaternion<Scalar>, Options> quaternion_;
618 };
619 
620 // Specialization of Eigen::Map for ``RxSO3 const``.
621 //
622 // Allows us to wrap RxSO3 objects around POD array (e.g. external c style
623 // quaternion).
624 template <class Scalar_, int Options>
625 class Map<Sophus::RxSO3<Scalar_> const, Options>
626  : public Sophus::RxSO3Base<Map<Sophus::RxSO3<Scalar_> const, Options>> {
628 
629  public:
630  using Scalar = Scalar_;
632  using Point = typename Base::Point;
633  using Tangent = typename Base::Tangent;
634  using Adjoint = typename Base::Adjoint;
635 
636  EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
637  using Base::operator*=;
638  using Base::operator*;
639 
641  Map(Scalar const* coeffs) : quaternion_(coeffs) {}
642 
643  // Accessor of quaternion.
644  //
646  Map<Eigen::Quaternion<Scalar> const, Options> const& quaternion() const {
647  return quaternion_;
648  }
649 
650  protected:
651  Map<Eigen::Quaternion<Scalar> const, Options> const quaternion_;
652 };
653 }
654 
655 #endif // SOPHUS_RXSO3_HPP
Map< Eigen::Quaternion< Scalar >, Options > quaternion_
Definition: rxso3.hpp:617
SOPHUS_FUNC RxSO3< Scalar > inverse() const
Definition: rxso3.hpp:122
#define SOPHUS_ENSURE(expr, description,...)
Definition: common.hpp:129
static SOPHUS_FUNC RxSO3< Scalar > exp(Tangent const &a)
Definition: rxso3.hpp:340
static SOPHUS_FUNC Transformation hat(Tangent const &a)
Definition: rxso3.hpp:408
SOPHUS_FUNC RxSO3< Scalar > operator*(RxSO3< Scalar > const &other) const
Definition: rxso3.hpp:191
SOPHUS_FUNC Transformation rotationMatrix() const
Definition: rxso3.hpp:250
SOPHUS_FUNC void setScaledRotationMatrix(Transformation const &sR)
Definition: rxso3.hpp:287
SOPHUS_FUNC Point operator*(Point const &p) const
Definition: rxso3.hpp:204
SOPHUS_FUNC Map(Scalar const *coeffs)
Definition: rxso3.hpp:641
SOPHUS_FUNC void setSO3(SO3< Scalar > const &so3)
Definition: rxso3.hpp:302
SOPHUS_FUNC QuaternionMember const & unit_quaternion() const
Definition: so3.hpp:554
typename Eigen::internal::traits< RxSO3< Scalar, Options > >::QuaternionType QuaternionType
Definition: rxso3.hpp:68
static SOPHUS_FUNC Tangent log(RxSO3< Scalar > const &other)
Definition: rxso3.hpp:447
SOPHUS_FUNC Scalar const * data() const
Definition: rxso3.hpp:116
SOPHUS_FUNC QuaternionMember & quaternion_nonconst()
Definition: rxso3.hpp:571
T value(details::expression_node< T > *n)
Definition: exprtk.hpp:12104
SOPHUS_FUNC void setScale(Scalar const &scale)
Definition: rxso3.hpp:276
SOPHUS_FUNC Transformation matrix() const
Definition: rxso3.hpp:145
Matrix< Scalar, DoF, DoF > Adjoint
Definition: rxso3.hpp:80
static SOPHUS_FUNC Tangent lieBracket(Tangent const &a, Tangent const &b)
Definition: rxso3.hpp:428
SOPHUS_FUNC Scalar scale() const
Definition: rxso3.hpp:259
SOPHUS_FUNC RxSO3(Scalar const &scale, Transformation const &R)
Definition: rxso3.hpp:535
typename Base::Transformation Transformation
Definition: rxso3.hpp:591
SOPHUS_FUNC SO3< Scalar > so3() const
Definition: rxso3.hpp:309
typename Base::Adjoint Adjoint
Definition: rxso3.hpp:502
QuaternionMember quaternion_
Definition: rxso3.hpp:573
typename Base::Transformation Transformation
Definition: rxso3.hpp:499
SOPHUS_FUNC RxSO3Base< Derived > & operator*=(RxSO3< Scalar > const &other)
Definition: rxso3.hpp:218
SOPHUS_FUNC Adjoint Adj() const
Definition: rxso3.hpp:90
static SOPHUS_FUNC Tangent logAndTheta(RxSO3< Scalar > const &other, Scalar *theta)
Definition: rxso3.hpp:454
Map< Eigen::Quaternion< Scalar > const, Options > QuaternionType
Definition: rxso3.hpp:33
SOPHUS_FUNC void setQuaternion(Eigen::Quaternion< Scalar > const &quat)
Definition: rxso3.hpp:235
Eigen::Matrix< Scalar, M, N > Matrix
Definition: types.hpp:38
SOPHUS_FUNC Map< Eigen::Quaternion< Scalar > const, Options > const & quaternion() const
Definition: rxso3.hpp:646
static SOPHUS_FUNC Transformation generator(int i)
Definition: rxso3.hpp:386
static SOPHUS_FUNC Scalar epsilon()
Definition: common.hpp:139
Map< Eigen::Quaternion< Scalar > const, Options > const quaternion_
Definition: rxso3.hpp:651
SOPHUS_FUNC RxSO3(RxSO3Base< OtherDerived > const &other)
Definition: rxso3.hpp:518
typename Base::Point Point
Definition: rxso3.hpp:500
Vector< Scalar, 3, Options > Vector3
Definition: types.hpp:18
static SOPHUS_FUNC Tangent vee(Transformation const &Omega)
Definition: rxso3.hpp:479
SOPHUS_FUNC Tangent log() const
Definition: rxso3.hpp:130
SOPHUS_FUNC QuaternionMember const & quaternion() const
Definition: rxso3.hpp:568
typename Base::Tangent Tangent
Definition: rxso3.hpp:501
static SOPHUS_FUNC Adjoint d_lieBracketab_by_d_a(Tangent const &b)
Definition: rxso3.hpp:322
SOPHUS_FUNC void setRotationMatrix(Transformation const &R)
Definition: rxso3.hpp:263
SOPHUS_FUNC RxSO3(Transformation const &sR)
Definition: rxso3.hpp:526
Eigen::Matrix< Scalar, M, 1, Options > Vector
Definition: types.hpp:10
SOPHUS_FUNC RxSO3(Scalar const &scale, SO3< Scalar > const &so3)
Definition: rxso3.hpp:546
typename Base::Transformation Transformation
Definition: rxso3.hpp:631
EIGEN_MAKE_ALIGNED_OPERATOR_NEW SOPHUS_FUNC RxSO3()
Definition: rxso3.hpp:512
SOPHUS_FUNC RxSO3< NewScalarType > cast() const
Definition: rxso3.hpp:100
SOPHUS_FUNC QuaternionType const & quaternion() const
Definition: rxso3.hpp:244
SOPHUS_FUNC QuaternionType & quaternion_nonconst()
Definition: rxso3.hpp:487
Eigen::Quaternion< Scalar, Options > QuaternionType
Definition: rxso3.hpp:19
SOPHUS_FUNC Scalar * data()
Definition: rxso3.hpp:112
Eigen::Quaternion< Scalar, Options > QuaternionMember
Definition: rxso3.hpp:503
typename Eigen::internal::traits< RxSO3< Scalar, Options > >::Scalar Scalar
Definition: rxso3.hpp:66
SOPHUS_FUNC RxSO3Base< Derived > & operator=(RxSO3Base< OtherDerived > const &other)
Definition: rxso3.hpp:179
SOPHUS_FUNC RxSO3(Eigen::QuaternionBase< D > const &quat)
Definition: rxso3.hpp:558
Vector< Scalar, 4 > Vector4
Definition: types.hpp:23
static SOPHUS_FUNC SO3< Scalar > expAndTheta(Tangent const &omega, Scalar *theta)
Definition: so3.hpp:279
SOPHUS_FUNC Map< Eigen::Quaternion< Scalar >, Options > & quaternion_nonconst()
Definition: rxso3.hpp:613
Matrix< Scalar, N, N > Transformation
Definition: rxso3.hpp:77
static SOPHUS_FUNC RxSO3< Scalar > expAndTheta(Tangent const &a, Scalar *theta)
Definition: rxso3.hpp:349
SOPHUS_FUNC Map< Eigen::Quaternion< Scalar >, Options > const & quaternion() const
Definition: rxso3.hpp:608
#define SOPHUS_FUNC
Definition: common.hpp:32



Page generated by Doxygen 1.8.13 for MRPT 1.5.3 at Tue Aug 22 01:03:35 UTC 2017