Main MRPT website > C++ reference for MRPT 1.5.3
so2.hpp
Go to the documentation of this file.
1 #ifndef SOPHUS_SO2_HPP
2 #define SOPHUS_SO2_HPP
3 
4 #include <complex>
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/LU>
9 
10 #include "types.hpp"
11 
12 namespace Sophus {
13 template <class Scalar_, int Options = 0>
14 class SO2;
15 using SO2d = SO2<double>;
16 using SO2f = SO2<float>;
17 } // namespace Sophus
18 
19 namespace Eigen {
20 namespace internal {
21 
22 template <class Scalar_, int Options>
23 struct traits<Sophus::SO2<Scalar_, Options>> {
24  using Scalar = Scalar_;
26 };
27 
28 template <class Scalar_, int Options>
29 struct traits<Map<Sophus::SO2<Scalar_>, Options>>
30  : traits<Sophus::SO2<Scalar_, Options>> {
31  using Scalar = Scalar_;
32  using ComplexType = Map<Sophus::Vector2<Scalar>, Options>;
33 };
34 
35 template <class Scalar_, int Options>
36 struct traits<Map<Sophus::SO2<Scalar_> const, Options>>
37  : traits<Sophus::SO2<Scalar_, Options> const> {
38  using Scalar = Scalar_;
39  using ComplexType = Map<Sophus::Vector2<Scalar> const, Options>;
40 };
41 } // namespace internal
42 } // namespace Eigen
43 
44 namespace Sophus {
45 
46 // SO2 base type - implements SO2 class but is storage agnostic.
47 //
48 // SO(2) is the group of rotations in 2d. 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. Let ``theta`` be the rotation angle, the rotation matrix
52 // can be written in close form:
53 //
54 // | cos(theta) -sin(theta) |
55 // | sin(theta) cos(theta) |
56 //
57 // As a matter of fact, the first column of those matrices is isomorph to the
58 // set of unit complex numbers U(1). Thus, internally, SO2 is represented as
59 // complex number with length 1.
60 //
61 // SO(2) is a compact and commutative group. First it is compact since the set
62 // of rotation matrices is a closed and bounded set. Second it is commutative
63 // since ``R(alpha) * R(beta) = R(beta) * R(alpha``, simply because ``alpha +
64 // beta = beta + alpha`` with ``alpha`` and ``beta`` being rotation angles
65 // (about the same axis).
66 //
67 // Class invairant: The 2-norm of ``unit_complex`` must be close to 1.
68 // Technically speaking, it must hold that:
69 //
70 // ``|unit_complex().squaredNorm() - 1| <= Constants<Scalar>::epsilon()``.
71 template <class Derived>
72 class SO2Base {
73  public:
74  using Scalar = typename Eigen::internal::traits<Derived>::Scalar;
75  using Complex = typename Eigen::internal::traits<Derived>::ComplexType;
76 
77  // Degrees of freedom of manifold, number of dimensions in tangent space (one
78  // since we only have in-plane rotations).
79  static int constexpr DoF = 1;
80  // Number of internal parameters used (complex numbers are a tuples).
81  static int constexpr num_parameters = 2;
82  // Group transformations are 2x2 matrices.
83  static int constexpr N = 2;
86  using Tangent = Scalar;
87  using Adjoint = Scalar;
88 
89  // Adjoint transformation
90  //
91  // This function return the adjoint transformation ``Ad`` of the group
92  // element ``A`` such that for all ``x`` it holds that
93  // ``hat(Ad_A * x) = A * hat(x) A^{-1}``. See hat-operator below.
94  //
95  // It simply ``1``, since ``SO(2)`` is a commutative group.
96  //
97  SOPHUS_FUNC Adjoint Adj() const { return 1; }
98 
99  // Returns copy of instance casted to NewScalarType.
100  //
101  template <class NewScalarType>
103  return SO2<NewScalarType>(unit_complex().template cast<NewScalarType>());
104  }
105 
106  // This provides unsafe read/write access to internal data. SO(2) is
107  // represented by a unit complex number (two parameters). When using direct
108  // write access, the user needs to take care of that the complex number stays
109  // normalized.
110  //
111  SOPHUS_FUNC Scalar* data() { return unit_complex_nonconst().data(); }
112 
113  // Const version of data() above.
114  //
115  SOPHUS_FUNC Scalar const* data() const { return unit_complex().data(); }
116 
117  // Returns ``*this`` times the ith generator of internal U(1) representation.
118  //
120  return SO2<Scalar>(unit_complex().x(), -unit_complex().y());
121  }
122 
123  // Logarithmic map
124  //
125  // Returns tangent space representation (= rotation angle) of the instance.
126  //
127  SOPHUS_FUNC Scalar log() const { return SO2<Scalar>::log(*this); }
128 
129  // It re-normalizes ``unit_complex`` to unit length.
130  //
131  // Note: Because of the class invariant, there is typically no need to call
132  // this function directly.
133  //
135  Scalar length = std::sqrt(unit_complex().x() * unit_complex().x() +
136  unit_complex().y() * unit_complex().y());
138  "Complex number should not be close to zero!");
139  unit_complex_nonconst().x() /= length;
140  unit_complex_nonconst().y() /= length;
141  }
142 
143  // Returns 2x2 matrix representation of the instance.
144  //
145  // For SO(2), the matrix representation is an orthogonal matrix ``R`` with
146  // ``det(R)=1``, thus the so-called "rotation matrix".
147  //
149  Scalar const& real = unit_complex().x();
150  Scalar const& imag = unit_complex().y();
151  Transformation R;
152  // clang-format off
153  R <<
154  real, -imag,
155  imag, real;
156  // clang-format on
157  return R;
158  }
159 
160  // Assignment operator
161  //
162  template <class OtherDerived>
164  unit_complex_nonconst() = other.unit_complex();
165  return *this;
166  }
167 
168  // Group multiplication, which is rotation concatenation.
169  //
171  SO2<Scalar> result(*this);
172  result *= other;
173  return result;
174  }
175 
176  // Group action on 3-points.
177  //
178  // This function rotates a 3 dimensional point ``p`` by the SO3 element
179  // ``bar_R_foo`` (= rotation matrix): ``p_bar = bar_R_foo * p_foo``.
180  //
181  SOPHUS_FUNC Point operator*(Point const& p) const {
182  Scalar const& real = unit_complex().x();
183  Scalar const& imag = unit_complex().y();
184  return Point(real * p[0] - imag * p[1], imag * p[0] + real * p[1]);
185  }
186 
187  // In-place group multiplication.
188  //
190  Scalar lhs_real = unit_complex().x();
191  Scalar lhs_imag = unit_complex().y();
192  Scalar const& rhs_real = other.unit_complex().x();
193  Scalar const& rhs_imag = other.unit_complex().y();
194  // complex multiplication
195  unit_complex_nonconst().x() = lhs_real * rhs_real - lhs_imag * rhs_imag;
196  unit_complex_nonconst().y() = lhs_real * rhs_imag + lhs_imag * rhs_real;
197 
198  Scalar squared_norm = unit_complex_nonconst().squaredNorm();
199  // We can assume that the squared-norm is close to 1 since we deal with a
200  // unit complex number. Due to numerical precision issues, there might
201  // be a small drift after pose concatenation. Hence, we need to renormalizes
202  // the complex number here.
203  // Since squared-norm is close to 1, we do not need to calculate the costly
204  // square-root, but can use an approximation around 1 (see
205  // http://stackoverflow.com/a/12934750 for details).
206  if (squared_norm != Scalar(1.0)) {
207  unit_complex_nonconst() *= Scalar(2.0) / (Scalar(1.0) + squared_norm);
208  }
209  return *this;
210  }
211 
212  // Takes in complex number / tuple and normalizes it.
213  //
214  // Precondition: The complex number must not be close to zero.
215  //
216  SOPHUS_FUNC void setComplex(Point const& complex) {
217  unit_complex_nonconst() = complex;
218  normalize();
219  }
220 
221  // Accessor of unit quaternion.
222  //
224  Complex const& unit_complex() const {
225  return static_cast<Derived const*>(this)->unit_complex();
226  }
227 
228  ////////////////////////////////////////////////////////////////////////////
229  // public static functions
230  ////////////////////////////////////////////////////////////////////////////
231 
232  // Group exponential
233  //
234  // This functions takes in an element of tangent space (= rotation angle
235  // ``theta``) and returns the corresponding element of the group SO(2).
236  //
237  // To be more specific, this function computes ``expmat(hat(omega))``
238  // with ``expmat(.)`` being the matrix exponential and ``hat(.)`` being the
239  // hat()-operator of SO(2).
240  //
241  SOPHUS_FUNC static SO2<Scalar> exp(Tangent const& theta) {
242  return SO2<Scalar>(std::cos(theta), std::sin(theta));
243  }
244 
245  // Returns the infinitesimal generators of SO3.
246  //
247  // The infinitesimal generators of SO(2) is:
248  //
249  // | 0 1 |
250  // | -1 0 |
251  //
252  SOPHUS_FUNC static Transformation generator() { return hat(1); }
253 
254  // hat-operator
255  //
256  // It takes in the scalar representation ``theta`` (= rotation angle) and
257  // returns the corresponding matrix representation of Lie algebra element.
258  //
259  // Formally, the ``hat()`` operator of SO(2) is defined as
260  //
261  // ``hat(.): R^2 -> R^{2x2}, hat(theta) = theta * G``
262  //
263  // with ``G`` being the infinitesimal generator of SO(2).
264  //
265  // The corresponding inverse is the ``vee``-operator, see below.
266  //
267  SOPHUS_FUNC static Transformation hat(Tangent const& theta) {
268  Transformation Omega;
269  // clang-format off
270  Omega <<
271  Scalar(0), -theta,
272  theta, Scalar(0);
273  // clang-format on
274  return Omega;
275  }
276 
277  // Lie bracket
278  //
279  // It returns the Lie bracket of SO(2). Since SO(2) is a commutative group,
280  // the Lie bracket is simple ``0``.
281  //
282  SOPHUS_FUNC static Tangent lieBracket(Tangent const&, Tangent const&) {
283  return Scalar(0);
284  }
285 
286  // Logarithmic map
287  //
288  // Computes the logarithm, the inverse of the group exponential which maps
289  // element of the group (rotation matrices) to elements of the tangent space
290  // (rotation angles).
291  //
292  // To be specific, this function computes ``vee(logmat(.))`` with
293  // ``logmat(.)`` being the matrix logarithm and ``vee(.)`` the vee-operator
294  // of SO(2).
295  //
296  SOPHUS_FUNC static Tangent log(SO2<Scalar> const& other) {
297  using std::atan2;
298  return atan2(other.unit_complex_.y(), other.unit_complex().x());
299  }
300 
301  // vee-operator
302  //
303  // It takes the 2x2-matrix representation ``Omega`` and maps it to the
304  // corresponding scalar representation of Lie algebra.
305  //
306  // This is the inverse of the hat-operator, see above.
307  //
308  // Precondition: ``Omega`` must have the following structure:
309  //
310  // | 0 -a |
311  // | a 0 |
312  //
313  SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
314  using std::abs;
315  return Omega(1, 0);
316  }
317 
318  private:
319  // Mutator of unit_complex is private to ensure class invariant. That is
320  // the complex number must stay close to unit length.
321  //
324  return static_cast<Derived*>(this)->unit_complex_nonconst();
325  }
326 };
327 
328 // SO2 default type - Constructors and default storage for SO2 Type
329 template <class Scalar_, int Options>
330 class SO2 : public SO2Base<SO2<Scalar_, Options>> {
332 
333  public:
334  using Scalar = Scalar_;
336  using Point = typename Base::Point;
337  using Tangent = typename Base::Tangent;
338  using Adjoint = typename Base::Adjoint;
340 
341  // ``Base`` is friend so unit_complex_nonconst can be accessed from ``Base``.
342  friend class SO2Base<SO2<Scalar, Options>>;
343 
344  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
345 
346  // Default constructor initialize unit complex number to identity rotation.
347  //
348  SOPHUS_FUNC SO2() : unit_complex_(Scalar(1), Scalar(0)) {}
349 
350  // Copy constructor
351  //
352  template <class OtherDerived>
354  : unit_complex_(other.unit_complex()) {}
355 
356  // Constructor from rotation matrix
357  //
358  // Precondition: rotation matrix need to be orthogonal with determinant of 1.
359  //
360  SOPHUS_FUNC explicit SO2(Transformation const& R)
361  : unit_complex_(Scalar(0.5) * (R(0, 0) + R(1, 1)),
362  Scalar(0.5) * (R(1, 0) - R(0, 1))) {
364  std::abs(R.determinant() - Scalar(1)) <= Constants<Scalar>::epsilon(),
365  "det(R) should be (close to) 1.");
366  }
367 
368  // Constructor from pair of real and imaginary number.
369  //
370  // Precondition: The pair must not be close to zero.
371  //
372  SOPHUS_FUNC SO2(Scalar const& real, Scalar const& imag)
373  : unit_complex_(real, imag) {
374  Base::normalize();
375  }
376 
377  // Constructor from 2-vector.
378  //
379  // Precondition: The vector must not be close to zero.
380  //
381  template <class D>
382  SOPHUS_FUNC explicit SO2(Eigen::MatrixBase<D> const& complex)
383  : unit_complex_(complex) {
385  "must be same Scalar type");
386  Base::normalize();
387  }
388 
389  // Constructor from an rotation angle.
390  //
391  SOPHUS_FUNC explicit SO2(Scalar theta) {
392  unit_complex_nonconst() = SO2<Scalar>::exp(theta).unit_complex();
393  }
394 
395  // Accessor of unit complex number
396  //
398  return unit_complex_;
399  }
400 
401  protected:
402  // Mutator of complex number is protected to ensure class invariant.
403  //
404  SOPHUS_FUNC ComplexMember& unit_complex_nonconst() { return unit_complex_; }
405 
407 };
408 
409 } // namespace Sophus
410 
411 namespace Eigen {
412 
413 // Specialization of Eigen::Map for ``SO2``.
414 //
415 // Allows us to wrap SO2 objects around POD array (e.g. external c style
416 // complex number / tuple).
417 template <class Scalar_, int Options>
418 class Map<Sophus::SO2<Scalar_>, Options>
419  : public Sophus::SO2Base<Map<Sophus::SO2<Scalar_>, Options>> {
421 
422  public:
423  using Scalar = Scalar_;
424 
426  using Point = typename Base::Point;
427  using Tangent = typename Base::Tangent;
428  using Adjoint = typename Base::Adjoint;
429 
430  // ``Base`` is friend so unit_complex_nonconst can be accessed from ``Base``.
431  friend class Sophus::SO2Base<Map<Sophus::SO2<Scalar_>, Options>>;
432 
433  EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
434  using Base::operator*=;
435  using Base::operator*;
436 
438  Map(Scalar* coeffs) : unit_complex_(coeffs) {}
439 
440  // Accessor of unit complex number.
441  //
443  Map<Sophus::Vector2<Scalar>, Options> const& unit_complex() const {
444  return unit_complex_;
445  }
446 
447  protected:
448  // Mutator of unit_complex is protected to ensure class invariant.
449  //
451  Map<Sophus::Vector2<Scalar>, Options>& unit_complex_nonconst() {
452  return unit_complex_;
453  }
454 
455  Map<Matrix<Scalar, 2, 1>, Options> unit_complex_;
456 };
457 
458 // Specialization of Eigen::Map for ``SO2 const``.
459 //
460 // Allows us to wrap SO2 objects around POD array (e.g. external c style
461 // complex number / tuple).
462 template <class Scalar_, int Options>
463 class Map<Sophus::SO2<Scalar_> const, Options>
464  : public Sophus::SO2Base<Map<Sophus::SO2<Scalar_> const, Options>> {
466 
467  public:
468  using Scalar = Scalar_;
470  using Point = typename Base::Point;
471  using Tangent = typename Base::Tangent;
472  using Adjoint = typename Base::Adjoint;
473 
474  EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
475  using Base::operator*=;
476  using Base::operator*;
477 
478  SOPHUS_FUNC Map(Scalar const* coeffs) : unit_complex_(coeffs) {}
479 
480  // Accessor of unit complex number.
481  //
482  SOPHUS_FUNC Map<Sophus::Vector2<Scalar> const, Options> const& unit_complex()
483  const {
484  return unit_complex_;
485  }
486 
487  protected:
488  // Mutator of unit_complex is protected to ensure class invariant.
489  //
490  Map<Matrix<Scalar, 2, 1> const, Options> const unit_complex_;
491 };
492 }
493 
494 #endif // SOPHUS_SO2_HPP
SOPHUS_FUNC Complex & unit_complex_nonconst()
Definition: so2.hpp:323
SOPHUS_FUNC Map< Sophus::Vector2< Scalar >, Options > & unit_complex_nonconst()
Definition: so2.hpp:451
#define SOPHUS_ENSURE(expr, description,...)
Definition: common.hpp:129
Sophus::Vector2< Scalar, Options > ComplexType
Definition: so2.hpp:25
SOPHUS_FUNC Point operator*(Point const &p) const
Definition: so2.hpp:181
typename Base::Tangent Tangent
Definition: so2.hpp:337
Map< Matrix< Scalar, 2, 1 > const, Options > const unit_complex_
Definition: so2.hpp:490
static SOPHUS_FUNC SO2< Scalar > exp(Tangent const &theta)
Definition: so2.hpp:241
SOPHUS_FUNC SO2< NewScalarType > cast() const
Definition: so2.hpp:102
SOPHUS_FUNC SO2(Transformation const &R)
Definition: so2.hpp:360
Matrix< Scalar, N, N > Transformation
Definition: so2.hpp:84
SOPHUS_FUNC SO2Base< Derived > & operator=(SO2Base< OtherDerived > const &other)
Definition: so2.hpp:163
T value(details::expression_node< T > *n)
Definition: exprtk.hpp:12104
SOPHUS_FUNC SO2(SO2Base< OtherDerived > const &other)
Definition: so2.hpp:353
typename Base::Adjoint Adjoint
Definition: so2.hpp:338
SOPHUS_FUNC SO2< Scalar > operator*(SO2< Scalar > const &other) const
Definition: so2.hpp:170
SOPHUS_FUNC Adjoint Adj() const
Definition: so2.hpp:97
Vector< Scalar, 2, Options > Vector2
Definition: types.hpp:13
static SOPHUS_FUNC Transformation hat(Tangent const &theta)
Definition: so2.hpp:267
SOPHUS_FUNC Scalar * data()
Definition: so2.hpp:111
Eigen::Matrix< Scalar, M, N > Matrix
Definition: types.hpp:38
EIGEN_MAKE_ALIGNED_OPERATOR_NEW SOPHUS_FUNC SO2()
Definition: so2.hpp:348
Map< Matrix< Scalar, 2, 1 >, Options > unit_complex_
Definition: so2.hpp:455
SOPHUS_FUNC void normalize()
Definition: so2.hpp:134
typename Base::Transformation Transformation
Definition: so2.hpp:335
SOPHUS_FUNC ComplexMember & unit_complex_nonconst()
Definition: so2.hpp:404
typename Eigen::internal::traits< SO2< Scalar, Options > >::Scalar Scalar
Definition: so2.hpp:74
ComplexMember unit_complex_
Definition: so2.hpp:406
SOPHUS_FUNC Scalar const * data() const
Definition: so2.hpp:115
SOPHUS_FUNC SO2Base< Derived > operator*=(SO2< Scalar > const &other)
Definition: so2.hpp:189
SOPHUS_FUNC Scalar log() const
Definition: so2.hpp:127
SOPHUS_FUNC Complex const & unit_complex() const
Definition: so2.hpp:224
SOPHUS_FUNC SO2(Scalar theta)
Definition: so2.hpp:391
static SOPHUS_FUNC Transformation generator()
Definition: so2.hpp:252
void normalize(Scalar valMin, Scalar valMax)
Scales all elements such as the minimum & maximum values are shifted to the given values...
static SOPHUS_FUNC Tangent lieBracket(Tangent const &, Tangent const &)
Definition: so2.hpp:282
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 Transformation matrix() const
Definition: so2.hpp:148
typename Base::Point Point
Definition: so2.hpp:336
static SOPHUS_FUNC Tangent vee(Transformation const &Omega)
Definition: so2.hpp:313
Vector2< Scalar, Options > ComplexMember
Definition: so2.hpp:339
SOPHUS_FUNC SO2(Scalar const &real, Scalar const &imag)
Definition: so2.hpp:372
SOPHUS_FUNC ComplexMember const & unit_complex() const
Definition: so2.hpp:397
SOPHUS_FUNC SO2(Eigen::MatrixBase< D > const &complex)
Definition: so2.hpp:382
SOPHUS_FUNC void setComplex(Point const &complex)
Definition: so2.hpp:216
Map< Sophus::Vector2< Scalar > const, Options > ComplexType
Definition: so2.hpp:39
SOPHUS_FUNC Map< Sophus::Vector2< Scalar > const, Options > const & unit_complex() const
Definition: so2.hpp:482
SOPHUS_FUNC SO2< Scalar > inverse() const
Definition: so2.hpp:119
static SOPHUS_FUNC Tangent log(SO2< Scalar > const &other)
Definition: so2.hpp:296
SOPHUS_FUNC Map< Sophus::Vector2< Scalar >, Options > const & unit_complex() const
Definition: so2.hpp:443
typename Eigen::internal::traits< SO2< Scalar, Options > >::ComplexType Complex
Definition: so2.hpp:75
T atan2(const T v0, const T v1)
Definition: exprtk.hpp:1394
SOPHUS_FUNC Map(Scalar const *coeffs)
Definition: so2.hpp:478
#define SOPHUS_FUNC
Definition: common.hpp:32
typename Base::Transformation Transformation
Definition: so2.hpp:469



Page generated by Doxygen 1.8.13 for MRPT 1.5.3 at Sun Nov 26 00:44:48 UTC 2017