MRPT  2.0.1
SO.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "poses-precomp.h" // Precompiled headers
11 
12 #include <mrpt/config.h> // for HAVE_SINCOS
13 #include <mrpt/math/CQuaternion.h>
14 #include <mrpt/math/wrap2pi.h>
15 #include <mrpt/poses/CPose3D.h>
16 #include <mrpt/poses/Lie/SO.h>
17 #include <cmath>
18 
19 using namespace mrpt;
20 using namespace mrpt::math;
21 using namespace mrpt::poses;
22 using namespace mrpt::poses::Lie;
23 
24 // See .h for documentation
25 
26 // ====== SO(3) ===========
28 {
30 
33  SO<3>::type R;
34  q.rotationMatrixNoResize(R);
35  return R;
36  MRPT_END
37 }
38 
40 {
41  /*
42  0 0 0
43  0 0 1
44  0 -1 0
45  0 0 -1
46  0 0 0
47  1 0 0
48  0 1 0
49  -1 0 0
50  0 0 0
51  */
52  tang2mat_jacob J = tang2mat_jacob::Zero();
53  J(1, 2) = J(5, 0) = J(6, 1) = +1.0;
54  J(2, 1) = J(3, 2) = J(7, 0) = -1.0;
55  return J;
56 }
57 
59 {
60  // Based on original code from Sophus:
61  // Copyright: 2011-2017 Hauke Strasdat
62  // 2012-2017 Steven Lovegrove
63  // License: Expat
64 
65  // From: Sophus::SO3<>::log()
66 
67  using std::abs;
68  using std::atan;
69  using std::sqrt;
70 
73 
74  const auto squared_n = q.x() * q.x() + q.y() * q.y() + q.z() * q.z();
75  const auto n = sqrt(squared_n);
76  const auto w = q.r();
77 
78  double two_atan_nbyw_by_n;
79 
80  // Atan-based log thanks to
81  //
82  // C. Hertzberg et al.:
83  // "Integrating Generic Sensor Fusion Algorithms with Sound State
84  // Representation through Encapsulation of Manifolds"
85  // Information Fusion, 2011
86 
87  if (n < 1e-7)
88  {
89  // If quaternion is normalized and n=0, then w should be 1;
90  // w=0 should never happen here!
91  ASSERTMSG_(std::abs(w) >= 1e-7, "Quaternion should be normalized!");
92  two_atan_nbyw_by_n = 2.0 / w - 2.0 * (squared_n) / (w * w * w);
93  }
94  else
95  {
96  if (std::abs(w) < 1e-7)
97  {
98  if (w > 0)
99  two_atan_nbyw_by_n = M_PI / n;
100  else
101  two_atan_nbyw_by_n = -M_PI / n;
102  }
103  else
104  {
105  two_atan_nbyw_by_n = 2.0 * atan(n / w) / n;
106  }
107  }
108 
109  tangent_vector ret;
110  ret[0] = two_atan_nbyw_by_n * q.x();
111  ret[1] = two_atan_nbyw_by_n * q.y();
112  ret[2] = two_atan_nbyw_by_n * q.z();
113 
114  return ret;
115 }
116 
118  const double yaw, const double pitch, const double roll)
119 {
120 #ifdef HAVE_SINCOS
121  double cy, sy;
122  ::sincos(yaw, &sy, &cy);
123  double cp, sp;
124  ::sincos(pitch, &sp, &cp);
125  double cr, sr;
126  ::sincos(roll, &sr, &cr);
127 #else
128  const double cy = cos(yaw);
129  const double sy = sin(yaw);
130  const double cp = cos(pitch);
131  const double sp = sin(pitch);
132  const double cr = cos(roll);
133  const double sr = sin(roll);
134 #endif
135 
136  // clang-format off
137  alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double rot_vals[] = {
138  cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr,
139  sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr,
140  -sp, cp * sr, cp * cr
141  };
142  // clang-format on
144  R.loadFromArray(rot_vals);
145  return R;
146 }
147 
148 template <typename VEC3, typename MAT3x3, typename MAT3x9>
149 inline void M3x9(const VEC3& a, const MAT3x3& B, MAT3x9& RES)
150 {
151  // clang-format off
152  alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double vals[] = {
153  a[0], -B(0, 2), B(0, 1), B(0, 2), a[0], -B(0, 0), -B(0, 1), B(0, 0), a[0],
154  a[1], -B(1, 2), B(1, 1), B(1, 2), a[1], -B(1, 0), -B(1, 1), B(1, 0), a[1],
155  a[2], -B(2, 2), B(2, 1), B(2, 2), a[2], -B(2, 0), -B(2, 1), B(2, 0), a[2]
156  };
157  // clang-format on
158  RES.loadFromArray(vals);
159 }
160 
162 {
163  using namespace mrpt::math;
164 
165  const double d = 0.5 * (R(0, 0) + R(1, 1) + R(2, 2) - 1);
168  if (d > 0.99999)
169  {
170  a[0] = a[1] = a[2] = 0;
171  B.setDiagonal(3, -0.5);
172  }
173  else
174  {
175  const double theta = acos(d);
176  const double d2 = square(d);
177  const double sq = std::sqrt(1 - d2);
178  a = SO<3>::vee_RmRt(R);
179  a *= (d * theta - sq) / (4 * (sq * sq * sq));
180  B.setDiagonal(3, -theta / (2 * sq));
181  }
183  M3x9(a, B, M);
184  return M;
185 }
186 
188 {
190  v[0] = R(2, 1) - R(1, 2);
191  v[1] = R(0, 2) - R(2, 0);
192  v[2] = R(1, 0) - R(0, 1);
193  return v;
194 }
195 
196 // ====== SO(2) ===========
198 {
199  return mrpt::math::wrapToPi(x[0]);
200 }
201 
203 {
204  tang2mat_jacob J;
205  J(0, 0) = 1.0;
206  return J;
207 }
208 
210 {
211  tangent_vector v;
212  v[0] = mrpt::math::wrapToPi(R);
213  return v;
214 }
215 
217 {
218  mat2tang_jacob J;
219  J(0, 0) = 1.0;
220  return J;
221 }
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
#define MRPT_START
Definition: exceptions.h:241
T y() const
Return y coordinate of the quaternion.
Definition: CQuaternion.h:107
void getAsQuaternion(mrpt::math::CQuaternionDouble &q, mrpt::optional_ref< mrpt::math::CMatrixDouble43 > out_dq_dr=std::nullopt) const
Returns the quaternion associated to the rotation of this object (NOTE: XYZ translation is ignored) ...
Definition: CPose3D.cpp:501
This base provides a set of functions for maths stuff.
T r() const
Return r (real part) coordinate of the quaternion.
Definition: CQuaternion.h:101
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:50
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
return_t square(const num_t x)
Inline function for the square of a number.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
T x() const
Return x coordinate of the quaternion.
Definition: CQuaternion.h:105
const float R
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
#define MRPT_END
Definition: exceptions.h:245
void M3x9(const VEC3 &a, const MAT3x3 &B, MAT3x9 &RES)
Definition: SO.cpp:149
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...
Definition: CQuaternion.h:44
Traits for SO(n), rotations in R^n space.
Definition: SO.h:21
T z() const
Return z coordinate of the quaternion.
Definition: CQuaternion.h:109
void setDiagonal(const std::size_t N, const T value)
Resize to NxN, set all entries to zero, except the main diagonal which is set to value ...
Definition: MatrixBase.h:34
void fromRodriguesVector(const ARRAYLIKE3 &v)
Set this quaternion to the rotation described by a 3D (Rodrigues) rotation vector : If ...
Definition: CQuaternion.h:141



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020