MRPT  2.0.1
TPose3D.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 "math-precomp.h" // Precompiled headers
11 
12 #include <mrpt/math/CQuaternion.h>
13 #include <mrpt/math/TPoint2D.h>
14 #include <mrpt/math/TPoint3D.h>
15 #include <mrpt/math/TPose2D.h>
16 #include <mrpt/math/TPose3D.h>
17 #include <mrpt/math/homog_matrices.h> // homogeneousMatrixInverse()
18 #include <Eigen/Dense>
19 
20 using namespace mrpt::math;
21 
22 static_assert(std::is_trivially_copyable_v<TPose3D>);
23 
25  : x(p.x), y(p.y), z(0.0), yaw(0.0), pitch(0.0), roll(0.0)
26 {
27 }
29  : x(p.x), y(p.y), z(0.0), yaw(p.phi), pitch(0.0), roll(0.0)
30 {
31 }
33  : x(p.x), y(p.y), z(p.z), yaw(0.0), pitch(0.0), roll(0.0)
34 {
35 }
36 void TPose3D::asString(std::string& s) const
37 {
38  s = mrpt::format(
39  "[%f %f %f %f %f %f]", x, y, z, RAD2DEG(yaw), RAD2DEG(pitch),
40  RAD2DEG(roll));
41 }
45 {
46  // See:
47  // http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
48  const double cy = cos(yaw * 0.5), sy = sin(yaw * 0.5);
49  const double cp = cos(pitch * 0.5), sp = sin(pitch * 0.5);
50  const double cr = cos(roll * 0.5), sr = sin(roll * 0.5);
51 
52  const double ccc = cr * cp * cy;
53  const double ccs = cr * cp * sy;
54  const double css = cr * sp * sy;
55  const double sss = sr * sp * sy;
56  const double scc = sr * cp * cy;
57  const double ssc = sr * sp * cy;
58  const double csc = cr * sp * cy;
59  const double scs = sr * cp * sy;
60 
61  q.w(ccc + sss);
62  q.x(scc - css);
63  q.y(csc + scs);
64  q.z(ccs - ssc);
65 
66  // Compute 4x3 Jacobian: for details, see technical report:
67  // Parameterizations of SE(3) transformations: equivalences, compositions
68  // and uncertainty, J.L. Blanco (2010).
69  // https://www.mrpt.org/6D_poses:equivalences_compositions_and_uncertainty
70  if (out_dq_dr)
71  {
72  alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double nums[4 * 3] = {
73  -0.5 * q[3], 0.5 * (-csc + scs), -0.5 * q[1],
74  -0.5 * q[2], 0.5 * (-ssc - ccs), 0.5 * q[0],
75  0.5 * q[1], 0.5 * (ccc - sss), 0.5 * q[3],
76  0.5 * q[0], 0.5 * (-css - scc), -0.5 * q[2]};
77  out_dq_dr.value().get().loadFromArray(nums);
78  }
79 }
80 void TPose3D::composePoint(const TPoint3D& l, TPoint3D& g) const
81 {
83  this->getRotationMatrix(R);
84  TPoint3D res;
85  res.x = R(0, 0) * l.x + R(0, 1) * l.y + R(0, 2) * l.z + this->x;
86  res.y = R(1, 0) * l.x + R(1, 1) * l.y + R(1, 2) * l.z + this->y;
87  res.z = R(2, 0) * l.x + R(2, 1) * l.y + R(2, 2) * l.z + this->z;
88 
89  g = res;
90 }
92 {
93  TPoint3D g;
94  composePoint(l, g);
95  return g;
96 }
97 
99 {
100  CMatrixDouble44 H;
102  TPoint3D res;
103  res.x = H(0, 0) * g.x + H(0, 1) * g.y + H(0, 2) * g.z + H(0, 3);
104  res.y = H(1, 0) * g.x + H(1, 1) * g.y + H(1, 2) * g.z + H(1, 3);
105  res.z = H(2, 0) * g.x + H(2, 1) * g.y + H(2, 2) * g.z + H(2, 3);
106 
107  l = res;
108 }
110 {
111  TPoint3D l;
112  inverseComposePoint(g, l);
113  return l;
114 }
115 
117 {
118  const double cy = cos(yaw);
119  const double sy = sin(yaw);
120  const double cp = cos(pitch);
121  const double sp = sin(pitch);
122  const double cr = cos(roll);
123  const double sr = sin(roll);
124 
125  alignas(MRPT_MAX_STATIC_ALIGN_BYTES)
126  const double rot_vals[] = {cy * cp,
127  cy * sp * sr - sy * cr,
128  cy * sp * cr + sy * sr,
129  sy * cp,
130  sy * sp * sr + cy * cr,
131  sy * sp * cr - cy * sr,
132  -sp,
133  cp * sr,
134  cp * cr};
135  R.loadFromArray(rot_vals);
136 }
138  const mrpt::math::CMatrixDouble33& R, double& yaw, double& pitch,
139  double& roll)
140 {
142  std::abs(
143  sqrt(square(R(0, 0)) + square(R(1, 0)) + square(R(2, 0))) - 1) <
144  3e-3,
145  "Homogeneous matrix is not orthogonal & normalized!: " +
146  R.inMatlabFormat());
148  std::abs(
149  sqrt(square(R(0, 1)) + square(R(1, 1)) + square(R(2, 1))) - 1) <
150  3e-3,
151  "Homogeneous matrix is not orthogonal & normalized!: " +
152  R.inMatlabFormat());
154  std::abs(
155  sqrt(square(R(0, 2)) + square(R(1, 2)) + square(R(2, 2))) - 1) <
156  3e-3,
157  "Homogeneous matrix is not orthogonal & normalized!: " +
158  R.inMatlabFormat());
159 
160  // Pitch is in the range [-pi/2, pi/2 ], so this calculation is enough:
161  pitch = atan2(-R(2, 0), hypot(R(0, 0), R(1, 0)));
162 
163  // Roll:
164  if ((fabs(R(2, 1)) + fabs(R(2, 2))) <
165  10 * std::numeric_limits<double>::epsilon())
166  {
167  // Gimbal lock between yaw and roll. This one is arbitrarily forced to
168  // be zero.
169  // Check
170  // https://reference.mrpt.org/devel/classmrpt_1_1poses_1_1_c_pose3_d.html.
171  // If cos(pitch)==0, the homogeneous matrix is:
172  // When sin(pitch)==1:
173  // /0 cysr-sycr cycr+sysr x\ /0 sin(r-y) cos(r-y) x\.
174  // |0 sysr+cycr sycr-cysr y| = |0 cos(r-y) -sin(r-y) y|
175  // |-1 0 0 z| |-1 0 0 z|
176  // \0 0 0 1/ \0 0 0 1/
177  //
178  // And when sin(pitch)=-1:
179  // /0 -cysr-sycr -cycr+sysr x\ /0 -sin(r+y) -cos(r+y) x\.
180  // |0 -sysr+cycr -sycr-cysr y| = |0 cos(r+y) -sin(r+y) y|
181  // |1 0 0 z| |1 0 0 z|
182  // \0 0 0 1/ \0 0 0 1/
183  //
184  // Both cases are in a "gimbal lock" status. This happens because pitch
185  // is vertical.
186 
187  roll = 0.0;
188  if (pitch > 0)
189  yaw = atan2(R(1, 2), R(0, 2));
190  else
191  yaw = atan2(-R(1, 2), -R(0, 2));
192  }
193  else
194  {
195  roll = atan2(R(2, 1), R(2, 2));
196  // Yaw:
197  yaw = atan2(R(1, 0), R(0, 0));
198  }
199 }
200 
202 {
204  CMatrixDouble33(HG.blockCopy<3, 3>(0, 0)), yaw, pitch, roll);
205  x = HG(0, 3);
206  y = HG(1, 3);
207  z = HG(2, 3);
208 }
209 void TPose3D::composePose(const TPose3D other, TPose3D& result) const
210 {
211  CMatrixDouble44 me_H, o_H;
212  this->getHomogeneousMatrix(me_H);
213  other.getHomogeneousMatrix(o_H);
214  result.fromHomogeneousMatrix(
215  CMatrixDouble44(me_H.asEigen() * o_H.asEigen()));
216 }
218 {
221  HG.block<3, 3>(0, 0) = R.asEigen();
222  HG(0, 3) = x;
223  HG(1, 3) = y;
224  HG(2, 3) = z;
225  HG(3, 0) = HG(3, 1) = HG(3, 2) = 0.;
226  HG(3, 3) = 1.;
227 }
229 { // Get current HM & inverse in-place:
230  this->getHomogeneousMatrix(HG);
232 }
233 
234 void TPose3D::fromString(const std::string& s)
235 {
236  CMatrixDouble m;
237  if (!m.fromMatlabStringFormat(s))
238  THROW_EXCEPTION("Malformed expression in ::fromString");
239  ASSERTMSG_(
240  m.rows() == 1 && m.cols() == 6, "Wrong size of vector in ::fromString");
241  x = m(0, 0);
242  y = m(0, 1);
243  z = m(0, 2);
244  yaw = DEG2RAD(m(0, 3));
245  pitch = DEG2RAD(m(0, 4));
246  roll = DEG2RAD(m(0, 5));
247 }
248 
250 {
251  CMatrixDouble44 H;
253  TPose3D ret;
254  ret.fromHomogeneousMatrix(H);
255  return ret;
256 }
258 {
259  // b - a = A^{-1} * B
260  CMatrixDouble44 Hainv, Hb;
262  b.getHomogeneousMatrix(Hb);
263  TPose3D ret;
265  return ret;
266 }
std::vector< T1 > operator-(const std::vector< T1 > &v1, const std::vector< T2 > &v2)
Definition: ops_vectors.h:92
CMatrixFixed< Scalar, BLOCK_ROWS, BLOCK_COLS > blockCopy(int start_row=0, int start_col=0) const
const blockCopy(): Returns a copy of the given block
Definition: MatrixBase.h:233
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
double roll
Roll coordinate (rotation angle over X coordinate).
Definition: TPose3D.h:38
T y() const
Return y coordinate of the quaternion.
Definition: CQuaternion.h:107
mrpt::math::CMatrixDouble33 getRotationMatrix() const
Definition: TPose3D.h:169
double x
X,Y,Z, coords.
Definition: TPose3D.h:32
void fromHomogeneousMatrix(const mrpt::math::CMatrixDouble44 &HG)
Definition: TPose3D.cpp:201
void getHomogeneousMatrix(mrpt::math::CMatrixDouble44 &HG) const
Definition: TPose3D.cpp:217
double yaw
Yaw coordinate (rotation angle over Z axis).
Definition: TPose3D.h:34
std::optional< std::reference_wrapper< T > > optional_ref
Shorter name for std::optional<std::reference_wrapper<T>>
Definition: optional_ref.h:20
mrpt::math::CMatrixDouble44 getHomogeneousMatrix() const
Definition: TPose3D.h:176
void loadFromArray(const VECTOR &vals)
Definition: CMatrixFixed.h:171
CMatrixFixed< double, 3, 3 > CMatrixDouble33
Definition: CMatrixFixed.h:367
This base provides a set of functions for maths stuff.
void composePose(const TPose3D other, TPose3D &result) const
Definition: TPose3D.cpp:209
constexpr double DEG2RAD(const double x)
Degrees to radians.
auto block(int start_row, int start_col)
non-const block(): Returns an Eigen::Block reference to the block
constexpr TPose3D()=default
Default fast constructor.
void composePoint(const TPoint3D &l, TPoint3D &g) const
Definition: TPose3D.cpp:80
void getAsQuaternion(mrpt::math::CQuaternion< double > &q, mrpt::optional_ref< mrpt::math::CMatrixFixed< double, 4, 3 >> out_dq_dr=std::nullopt) const
Returns the quaternion associated to the rotation of this object (NOTE: XYZ translation is ignored) ...
Definition: TPose3D.cpp:42
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
bool fromMatlabStringFormat(const std::string &s, mrpt::optional_ref< std::ostream > dump_errors_here=std::nullopt)
Reads a matrix from a string in Matlab-like format, for example: "[1 0 2; 0 4 -1]" The string must st...
size_type rows() const
Number of rows in the matrix.
size_type cols() const
Number of columns in the matrix.
double pitch
Pitch coordinate (rotation angle over Y axis).
Definition: TPose3D.h:36
static void SO3_to_yaw_pitch_roll(const mrpt::math::CMatrixDouble33 &R, double &yaw, double &pitch, double &roll)
Definition: TPose3D.cpp:137
void fromString(const std::string &s)
Set the current object value from a string generated by &#39;asString&#39; (eg: "[0.02 1.04 -0...
Definition: TPose3D.cpp:234
return_t square(const num_t x)
Inline function for the square of a number.
#define ASSERTDEBMSG_(f, __ERROR_MSG)
Definition: exceptions.h:191
T x
X,Y,Z coordinates.
Definition: TPoint3D.h:29
T x() const
Return x coordinate of the quaternion.
Definition: CQuaternion.h:105
void homogeneousMatrixInverse(const MATRIXLIKE1 &M, MATRIXLIKE2 &out_inverse_M)
Efficiently compute the inverse of a 4x4 homogeneous matrix by only transposing the rotation 3x3 part...
const float R
void getInverseHomogeneousMatrix(mrpt::math::CMatrixDouble44 &HG) const
Definition: TPose3D.cpp:228
constexpr double RAD2DEG(const double x)
Radians to degrees.
mrpt::math::CMatrixDouble44 getInverseHomogeneousMatrix() const
Definition: TPose3D.h:183
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:24
Lightweight 2D pose.
Definition: TPose2D.h:22
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
Definition: CMatrixFixed.h:251
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...
Definition: CQuaternion.h:44
void inverseComposePoint(const TPoint3D &g, TPoint3D &l) const
Definition: TPose3D.cpp:98
T z() const
Return z coordinate of the quaternion.
Definition: CQuaternion.h:109
std::string asString() const
Definition: TPose3D.h:136
CMatrixFixed< double, 4, 4 > CMatrixDouble44
Definition: CMatrixFixed.h:368
T w() const
Return w (real part) coordinate of the quaternion.
Definition: CQuaternion.h:103



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