MRPT  1.9.9
TPoseOrPoint.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-2019, 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/core/exceptions.h>
13 #include <mrpt/math/CMatrixFixed.h>
14 #include <mrpt/math/CQuaternion.h>
16 #include <mrpt/math/TLine2D.h>
17 #include <mrpt/math/TLine3D.h>
18 #include <mrpt/math/TObject2D.h>
19 #include <mrpt/math/TObject3D.h>
20 #include <mrpt/math/TPlane.h>
21 #include <mrpt/math/TPoint2D.h>
22 #include <mrpt/math/TPoint3D.h>
23 #include <mrpt/math/TPolygon2D.h>
24 #include <mrpt/math/TPolygon3D.h>
25 #include <mrpt/math/TPose2D.h>
26 #include <mrpt/math/TPose3D.h>
27 #include <mrpt/math/TPose3DQuat.h>
28 #include <mrpt/math/TSegment2D.h>
29 #include <mrpt/math/TSegment3D.h>
30 #include <mrpt/math/TTwist2D.h>
31 #include <mrpt/math/TTwist3D.h>
32 #include <mrpt/math/geometry.h> // distance()
34 #include <mrpt/math/ops_containers.h> // squareNorm()
35 #include <mrpt/serialization/CArchive.h> // impl of << operator
37 #include <Eigen/Dense>
38 
39 using namespace std; // For min/max, etc...
40 using namespace mrpt::serialization; // CArchive, << operator for STL
41 
42 using mrpt::DEG2RAD;
43 using mrpt::RAD2DEG;
44 
45 namespace mrpt::math
46 {
47 static_assert(std::is_trivial_v<TPoint2D_data>);
48 static_assert(std::is_trivially_copyable_v<TPoint2D>);
49 static_assert(std::is_trivial_v<TPoint3D_data>);
50 static_assert(std::is_trivially_copyable_v<TPoint3D>);
51 static_assert(std::is_trivially_copyable_v<TPose2D>);
52 static_assert(std::is_trivially_copyable_v<TPose3D>);
53 static_assert(std::is_trivially_copyable_v<TPlane3D>);
54 static_assert(std::is_trivially_copyable_v<TLine2D>);
55 static_assert(std::is_trivially_copyable_v<TLine3D>);
56 static_assert(std::is_trivially_copyable_v<TTwist2D>);
57 static_assert(std::is_trivially_copyable_v<TTwist3D>);
58 
59 TPoint2D::TPoint2D(const TPose2D& p) : TPoint2D_data{p.x, p.y} {}
62 
63 bool TPoint2D::operator<(const TPoint2D& p) const
64 {
65  if (x < p.x)
66  return true;
67  else if (x > p.x)
68  return false;
69  else
70  return y < p.y;
71 }
73 {
74  CMatrixDouble m;
75  if (!m.fromMatlabStringFormat(s))
76  THROW_EXCEPTION("Malformed expression in ::fromString");
77  ASSERTMSG_(
78  m.rows() == 1 && m.cols() == 2, "Wrong size of vector in ::fromString");
79  x = m(0, 0);
80  y = m(0, 1);
81 }
82 
83 TPose2D::TPose2D(const TPoint2D& p) : x(p.x), y(p.y), phi(0.0) {}
84 TPose2D::TPose2D(const TPoint3D& p) : x(p.x), y(p.y), phi(0.0) {}
85 TPose2D::TPose2D(const TPose3D& p) : x(p.x), y(p.y), phi(p.yaw) {}
87 {
88  s = mrpt::format("[%f %f %f]", x, y, RAD2DEG(phi));
89 }
91 {
92  CMatrixDouble m;
93  if (!m.fromMatlabStringFormat(s))
94  THROW_EXCEPTION("Malformed expression in ::fromString");
95  ASSERTMSG_(
96  m.rows() == 1 && m.cols() == 3, "Wrong size of vector in ::fromString");
97  x = m(0, 0);
98  y = m(0, 1);
99  phi = DEG2RAD(m(0, 2));
100 }
102  const mrpt::math::TPose2D& b) const
103 {
104  const double A_cosphi = cos(this->phi), A_sinphi = sin(this->phi);
105  // Use temporary variables for the cases (A==this) or (B==this)
106  const double new_x = this->x + b.x * A_cosphi - b.y * A_sinphi;
107  const double new_y = this->y + b.x * A_sinphi + b.y * A_cosphi;
108  const double new_phi = mrpt::math::wrapToPi(this->phi + b.phi);
109 
110  return mrpt::math::TPose2D(new_x, new_y, new_phi);
111 }
112 
114  const mrpt::math::TPose2D& b) const
115 {
116  const double B_cosphi = cos(b.phi), B_sinphi = sin(b.phi);
117 
118  const double new_x =
119  (this->x - b.x) * B_cosphi + (this->y - b.y) * B_sinphi;
120  const double new_y =
121  -(this->x - b.x) * B_sinphi + (this->y - b.y) * B_cosphi;
122  const double new_phi = mrpt::math::wrapToPi(this->phi - b.phi);
123 
124  return mrpt::math::TPose2D(new_x, new_y, new_phi);
125 }
127 {
128  const double ccos = ::cos(phi), csin = ::sin(phi);
129  return {x + l.x * ccos - l.y * csin, y + l.x * csin + l.y * ccos};
130 }
132 {
133  return this->composePoint(b);
134 }
135 
137 {
138  const double Ax = g.x - x, Ay = g.y - y, ccos = ::cos(phi),
139  csin = ::sin(phi);
140  return {Ax * ccos + Ay * csin, -Ax * csin + Ay * ccos};
141 }
142 
143 // ----
145 {
146  s = mrpt::format("[%f %f %f]", vx, vy, RAD2DEG(omega));
147 }
149 {
150  CMatrixDouble m;
151  if (!m.fromMatlabStringFormat(s))
152  THROW_EXCEPTION("Malformed expression in ::fromString");
153  ASSERTMSG_(
154  m.rows() == 1 && m.cols() == 3, "Wrong size of vector in ::fromString");
155  vx = m(0, 0);
156  vy = m(0, 1);
157  omega = DEG2RAD(m(0, 2));
158 }
159 // Transform the (vx,vy) components for a counterclockwise rotation of `ang`
160 // radians
161 void TTwist2D::rotate(const double ang)
162 {
163  const double nvx = vx * cos(ang) - vy * sin(ang);
164  const double nvy = vx * sin(ang) + vy * cos(ang);
165  vx = nvx;
166  vy = nvy;
167 }
168 bool TTwist2D::operator==(const TTwist2D& o) const
169 {
170  return vx == o.vx && vy == o.vy && omega == o.omega;
171 }
172 bool TTwist2D::operator!=(const TTwist2D& o) const { return !(*this == o); }
174 {
175  return mrpt::math::TPose2D(vx * dt, vy * dt, omega * dt);
176 }
177 
178 // ----
180 {
181  s = mrpt::format(
182  "[%f %f %f %f %f %f]", vx, vy, vz, RAD2DEG(wx), RAD2DEG(wy),
183  RAD2DEG(wz));
184 }
186 {
187  CMatrixDouble m;
188  if (!m.fromMatlabStringFormat(s))
189  THROW_EXCEPTION("Malformed expression in ::fromString");
190  ASSERTMSG_(
191  m.rows() == 1 && m.cols() == 6, "Wrong size of vector in ::fromString");
192  for (int i = 0; i < 3; i++) (*this)[i] = m(0, i);
193  for (int i = 0; i < 3; i++) (*this)[3 + i] = DEG2RAD(m(0, 3 + i));
194 }
195 // Transform all 6 components for a change of reference frame from "A" to
196 // another frame "B" whose rotation with respect to "A" is given by `rot`. The
197 // translational part of the pose is ignored
198 void TTwist3D::rotate(const TPose3D& rot)
199 {
200  const TTwist3D t = *this;
202  rot.getRotationMatrix(R);
203  vx = R(0, 0) * t.vx + R(0, 1) * t.vy + R(0, 2) * t.vz;
204  vy = R(1, 0) * t.vx + R(1, 1) * t.vy + R(1, 2) * t.vz;
205  vz = R(2, 0) * t.vx + R(2, 1) * t.vy + R(2, 2) * t.vz;
206 
207  wx = R(0, 0) * t.wx + R(0, 1) * t.wy + R(0, 2) * t.wz;
208  wy = R(1, 0) * t.wx + R(1, 1) * t.wy + R(1, 2) * t.wz;
209  wz = R(2, 0) * t.wx + R(2, 1) * t.wy + R(2, 2) * t.wz;
210 }
211 bool TTwist3D::operator==(const TTwist3D& o) const
212 {
213  return vx == o.vx && vy == o.vy && vz == o.vz && wx == o.wx && wy == o.wy &&
214  wz == o.wz;
215 }
216 bool TTwist3D::operator!=(const TTwist3D& o) const { return !(*this == o); }
220 
221 bool TPoint3D::operator<(const TPoint3D& p) const
222 {
223  if (x < p.x)
224  return true;
225  else if (x > p.x)
226  return false;
227  else if (y < p.y)
228  return true;
229  else if (y > p.y)
230  return false;
231  else
232  return z < p.z;
233 }
235 {
236  CMatrixDouble m;
237  if (!m.fromMatlabStringFormat(s))
238  THROW_EXCEPTION("Malformed expression in ::fromString");
239  ASSERTMSG_(
240  m.rows() == 1 && m.cols() == 3, "Wrong size of vector in ::fromString");
241  x = m(0, 0);
242  y = m(0, 1);
243  z = m(0, 2);
244 }
245 
247  : x(p.x), y(p.y), z(0.0), yaw(0.0), pitch(0.0), roll(0.0)
248 {
249 }
251  : x(p.x), y(p.y), z(0.0), yaw(p.phi), pitch(0.0), roll(0.0)
252 {
253 }
255  : x(p.x), y(p.y), z(p.z), yaw(0.0), pitch(0.0), roll(0.0)
256 {
257 }
259 {
260  s = mrpt::format(
261  "[%f %f %f %f %f %f]", x, y, z, RAD2DEG(yaw), RAD2DEG(pitch),
262  RAD2DEG(roll));
263 }
267 {
268  // See:
269  // http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
270  const double cy = cos(yaw * 0.5), sy = sin(yaw * 0.5);
271  const double cp = cos(pitch * 0.5), sp = sin(pitch * 0.5);
272  const double cr = cos(roll * 0.5), sr = sin(roll * 0.5);
273 
274  const double ccc = cr * cp * cy;
275  const double ccs = cr * cp * sy;
276  const double css = cr * sp * sy;
277  const double sss = sr * sp * sy;
278  const double scc = sr * cp * cy;
279  const double ssc = sr * sp * cy;
280  const double csc = cr * sp * cy;
281  const double scs = sr * cp * sy;
282 
283  q[0] = ccc + sss;
284  q[1] = scc - css;
285  q[2] = csc + scs;
286  q[3] = ccs - ssc;
287 
288  // Compute 4x3 Jacobian: for details, see technical report:
289  // Parameterizations of SE(3) transformations: equivalences, compositions
290  // and uncertainty, J.L. Blanco (2010).
291  // https://www.mrpt.org/6D_poses:equivalences_compositions_and_uncertainty
292  if (out_dq_dr)
293  {
294  alignas(MRPT_MAX_STATIC_ALIGN_BYTES) const double nums[4 * 3] = {
295  -0.5 * q[3], 0.5 * (-csc + scs), -0.5 * q[1],
296  -0.5 * q[2], 0.5 * (-ssc - ccs), 0.5 * q[0],
297  0.5 * q[1], 0.5 * (ccc - sss), 0.5 * q[3],
298  0.5 * q[0], 0.5 * (-css - scc), -0.5 * q[2]};
299  out_dq_dr->loadFromArray(nums);
300  }
301 }
302 void TPose3D::composePoint(const TPoint3D& l, TPoint3D& g) const
303 {
305  this->getRotationMatrix(R);
306  TPoint3D res;
307  res.x = R(0, 0) * l.x + R(0, 1) * l.y + R(0, 2) * l.z + this->x;
308  res.y = R(1, 0) * l.x + R(1, 1) * l.y + R(1, 2) * l.z + this->y;
309  res.z = R(2, 0) * l.x + R(2, 1) * l.y + R(2, 2) * l.z + this->z;
310 
311  g = res;
312 }
314 {
315  TPoint3D g;
316  composePoint(l, g);
317  return g;
318 }
319 
321 {
322  CMatrixDouble44 H;
324  TPoint3D res;
325  res.x = H(0, 0) * g.x + H(0, 1) * g.y + H(0, 2) * g.z + H(0, 3);
326  res.y = H(1, 0) * g.x + H(1, 1) * g.y + H(1, 2) * g.z + H(1, 3);
327  res.z = H(2, 0) * g.x + H(2, 1) * g.y + H(2, 2) * g.z + H(2, 3);
328 
329  l = res;
330 }
332 {
333  TPoint3D l;
335  return l;
336 }
337 
339 {
340  const double cy = cos(yaw);
341  const double sy = sin(yaw);
342  const double cp = cos(pitch);
343  const double sp = sin(pitch);
344  const double cr = cos(roll);
345  const double sr = sin(roll);
346 
347  alignas(MRPT_MAX_STATIC_ALIGN_BYTES)
348  const double rot_vals[] = {cy * cp,
349  cy * sp * sr - sy * cr,
350  cy * sp * cr + sy * sr,
351  sy * cp,
352  sy * sp * sr + cy * cr,
353  sy * sp * cr - cy * sr,
354  -sp,
355  cp * sr,
356  cp * cr};
357  R.loadFromArray(rot_vals);
358 }
360  const mrpt::math::CMatrixDouble33& R, double& yaw, double& pitch,
361  double& roll)
362 {
364  std::abs(
365  sqrt(square(R(0, 0)) + square(R(1, 0)) + square(R(2, 0))) - 1) <
366  3e-3,
367  "Homogeneous matrix is not orthogonal & normalized!: " +
368  R.inMatlabFormat());
370  std::abs(
371  sqrt(square(R(0, 1)) + square(R(1, 1)) + square(R(2, 1))) - 1) <
372  3e-3,
373  "Homogeneous matrix is not orthogonal & normalized!: " +
374  R.inMatlabFormat());
376  std::abs(
377  sqrt(square(R(0, 2)) + square(R(1, 2)) + square(R(2, 2))) - 1) <
378  3e-3,
379  "Homogeneous matrix is not orthogonal & normalized!: " +
380  R.inMatlabFormat());
381 
382  // Pitch is in the range [-pi/2, pi/2 ], so this calculation is enough:
383  pitch = atan2(-R(2, 0), hypot(R(0, 0), R(1, 0)));
384 
385  // Roll:
386  if ((fabs(R(2, 1)) + fabs(R(2, 2))) <
387  10 * std::numeric_limits<double>::epsilon())
388  {
389  // Gimbal lock between yaw and roll. This one is arbitrarily forced to
390  // be zero.
391  // Check
392  // https://reference.mrpt.org/devel/classmrpt_1_1poses_1_1_c_pose3_d.html.
393  // If cos(pitch)==0, the homogeneous matrix is:
394  // When sin(pitch)==1:
395  // /0 cysr-sycr cycr+sysr x\ /0 sin(r-y) cos(r-y) x\.
396  // |0 sysr+cycr sycr-cysr y| = |0 cos(r-y) -sin(r-y) y|
397  // |-1 0 0 z| |-1 0 0 z|
398  // \0 0 0 1/ \0 0 0 1/
399  //
400  // And when sin(pitch)=-1:
401  // /0 -cysr-sycr -cycr+sysr x\ /0 -sin(r+y) -cos(r+y) x\.
402  // |0 -sysr+cycr -sycr-cysr y| = |0 cos(r+y) -sin(r+y) y|
403  // |1 0 0 z| |1 0 0 z|
404  // \0 0 0 1/ \0 0 0 1/
405  //
406  // Both cases are in a "gimbal lock" status. This happens because pitch
407  // is vertical.
408 
409  roll = 0.0;
410  if (pitch > 0)
411  yaw = atan2(R(1, 2), R(0, 2));
412  else
413  yaw = atan2(-R(1, 2), -R(0, 2));
414  }
415  else
416  {
417  roll = atan2(R(2, 1), R(2, 2));
418  // Yaw:
419  yaw = atan2(R(1, 0), R(0, 0));
420  }
421 }
422 
424 {
426  CMatrixDouble33(HG.blockCopy<3, 3>(0, 0)), yaw, pitch, roll);
427  x = HG(0, 3);
428  y = HG(1, 3);
429  z = HG(2, 3);
430 }
431 void TPose3D::composePose(const TPose3D other, TPose3D& result) const
432 {
433  CMatrixDouble44 me_H, o_H;
434  this->getHomogeneousMatrix(me_H);
435  other.getHomogeneousMatrix(o_H);
436  result.fromHomogeneousMatrix(
437  CMatrixDouble44(me_H.asEigen() * o_H.asEigen()));
438 }
440 {
443  HG.block<3, 3>(0, 0) = R.asEigen();
444  HG(0, 3) = x;
445  HG(1, 3) = y;
446  HG(2, 3) = z;
447  HG(3, 0) = HG(3, 1) = HG(3, 2) = 0.;
448  HG(3, 3) = 1.;
449 }
451 { // Get current HM & inverse in-place:
452  this->getHomogeneousMatrix(HG);
454 }
455 
457 {
458  CMatrixDouble m;
459  if (!m.fromMatlabStringFormat(s))
460  THROW_EXCEPTION("Malformed expression in ::fromString");
461  ASSERTMSG_(
462  m.rows() == 1 && m.cols() == 6, "Wrong size of vector in ::fromString");
463  x = m(0, 0);
464  y = m(0, 1);
465  z = m(0, 2);
466  yaw = DEG2RAD(m(0, 3));
467  pitch = DEG2RAD(m(0, 4));
468  roll = DEG2RAD(m(0, 5));
469 }
470 
472 {
473  CMatrixDouble m;
474  if (!m.fromMatlabStringFormat(s))
475  THROW_EXCEPTION("Malformed expression in ::fromString");
476  ASSERTMSG_(
477  m.rows() == 1 && m.cols() == 7, "Wrong size of vector in ::fromString");
478  for (int i = 0; i < m.cols(); i++) (*this)[i] = m(0, i);
479 }
481 {
482  CMatrixDouble44 H;
483  p.getInverseHomogeneousMatrix(H);
484  TPose3D ret;
485  ret.fromHomogeneousMatrix(H);
486  return ret;
487 }
489 {
490  // b - a = A^{-1} * B
491  CMatrixDouble44 Hainv, Hb;
492  a.getInverseHomogeneousMatrix(Hainv);
493  b.getHomogeneousMatrix(Hb);
494  TPose3D ret;
496  return ret;
497 }
498 
499 // Text streaming:
500 std::ostream& operator<<(std::ostream& o, const TPoint2D& p)
501 {
502  return (o << p.asString());
503 }
504 std::ostream& operator<<(std::ostream& o, const TPoint3D& p)
505 {
506  return (o << p.asString());
507 }
508 std::ostream& operator<<(std::ostream& o, const TPose2D& p)
509 {
510  return (o << p.asString());
511 }
512 std::ostream& operator<<(std::ostream& o, const TPose3D& p)
513 {
514  return (o << p.asString());
515 }
516 std::ostream& operator<<(std::ostream& o, const TPose3DQuat& p)
517 {
518  return (o << p.asString());
519 }
520 
522 {
523  return in >> s.point1 >> s.point2;
524 }
526 {
527  return out << s.point1 << s.point2;
528 }
530 {
531  return in >> l.coefs[0] >> l.coefs[1] >> l.coefs[2];
532 }
534 {
535  return out << l.coefs[0] << l.coefs[1] << l.coefs[2];
536 }
537 
539 {
540  return in >> s.point1 >> s.point2;
541 }
543 {
544  return out << s.point1 << s.point2;
545 }
547 {
548  return in >> l.pBase >> l.director[0] >> l.director[1] >> l.director[2];
549 }
551 {
552  return out << l.pBase << l.director[0] << l.director[1] << l.director[2];
553 }
555 {
556  return in >> p.coefs[0] >> p.coefs[1] >> p.coefs[2] >> p.coefs[3];
557 }
559 {
560  return out << p.coefs[0] << p.coefs[1] << p.coefs[2] << p.coefs[3];
561 }
562 
563 double TSegment2D::length() const { return math::distance(point1, point2); }
564 double TSegment2D::distance(const TPoint2D& point) const
565 {
566  return std::abs(signedDistance(point));
567 }
568 double TSegment2D::signedDistance(const TPoint2D& point) const
569 {
570  // It is reckoned whether the perpendicular line to the TSegment2D which
571  // passes through point crosses or not the referred segment,
572  // or what is the same, whether point makes an obtuse triangle with the
573  // segment or not (being the longest segment one between the point and
574  // either end of TSegment2D).
575  const double d1 = math::distance(point, point1);
576  if (point1 == point2) return d1;
577 
578  const double d2 = math::distance(point, point2);
579  const double d3 = length();
580  const double ds1 = square(d1);
581  const double ds2 = square(d2);
582  const double ds3 = square(d3);
583  if (ds1 > (ds2 + ds3) || ds2 > (ds1 + ds3))
584  // Fix sign:
585  return min(d1, d2) *
586  (TLine2D(*this).signedDistance(point) < 0 ? -1 : 1);
587  else
588  return TLine2D(*this).signedDistance(point);
589 }
590 bool TSegment2D::contains(const TPoint2D& point) const
591 {
592  return abs(math::distance(point1, point) + math::distance(point2, point) -
594 }
596 {
597  s = TSegment3D(*this);
598 }
600 {
601  point1 = TPoint2D(s.point1);
602  point2 = TPoint2D(s.point2);
603  if (point1 == point2)
604  throw std::logic_error("Segment is normal to projection plane");
605 }
606 
608 {
609  if (point1 < s.point1)
610  return true;
611  else if (s.point1 < point1)
612  return false;
613  else
614  return point2 < s.point2;
615 }
616 
618 {
619  s = TSegment2D(*this);
620 }
621 
622 double TSegment3D::length() const { return math::distance(point1, point2); }
623 double TSegment3D::distance(const TPoint3D& point) const
624 {
625  return min(
626  min(math::distance(point, point1), math::distance(point, point2)),
627  TLine3D(*this).distance(point));
628 }
629 double TSegment3D::distance(const TSegment3D& segment) const
630 {
631  Eigen::Vector3d u, v, w;
632  TPoint3D diff_vect = point2 - point1;
633  diff_vect.asVector(u);
634  diff_vect = segment.point2 - segment.point1;
635  diff_vect.asVector(v);
636  diff_vect = point1 - segment.point1;
637  diff_vect.asVector(w);
638  double a = u.dot(u); // always >= 0
639  double b = u.dot(v);
640  double c = v.dot(v); // always >= 0
641  double d = u.dot(w);
642  double e = v.dot(w);
643  double D = a * c - b * b; // always >= 0
644  double sc, sN, sD = D; // sc = sN / sD, default sD = D >= 0
645  double tc, tN, tD = D; // tc = tN / tD, default tD = D >= 0
646 
647  // compute the line parameters of the two closest points
648  if (D < 0.00000001)
649  { // the lines are almost parallel
650  sN = 0.0; // force using point P0 on segment S1
651  sD = 1.0; // to prevent possible division by 0.0 later
652  tN = e;
653  tD = c;
654  }
655  else
656  { // get the closest points on the infinite lines
657  sN = (b * e - c * d);
658  tN = (a * e - b * d);
659  if (sN < 0.0)
660  { // sc < 0 => the s=0 edge is visible
661  sN = 0.0;
662  tN = e;
663  tD = c;
664  }
665  else if (sN > sD)
666  { // sc > 1 => the s=1 edge is visible
667  sN = sD;
668  tN = e + b;
669  tD = c;
670  }
671  }
672 
673  if (tN < 0.0)
674  { // tc < 0 => the t=0 edge is visible
675  tN = 0.0;
676  // recompute sc for this edge
677  if (-d < 0.0)
678  sN = 0.0;
679  else if (-d > a)
680  sN = sD;
681  else
682  {
683  sN = -d;
684  sD = a;
685  }
686  }
687  else if (tN > tD)
688  { // tc > 1 => the t=1 edge is visible
689  tN = tD;
690  // recompute sc for this edge
691  if ((-d + b) < 0.0)
692  sN = 0;
693  else if ((-d + b) > a)
694  sN = sD;
695  else
696  {
697  sN = (-d + b);
698  sD = a;
699  }
700  }
701  // finally do the division to get sc and tc
702  sc = (fabs(sN) < 0.00000001 ? 0.0 : sN / sD);
703  tc = (fabs(tN) < 0.00000001 ? 0.0 : tN / tD);
704 
705  // get the difference of the two closest points
706  const auto dP = (w + (sc * u) - (tc * v)).eval();
707  return dP.norm(); // return the closest distance
708 }
709 bool TSegment3D::contains(const TPoint3D& point) const
710 {
711  // Not very intuitive, but very fast, method.
712  return abs(math::distance(point1, point) + math::distance(point2, point) -
714 }
715 
717 {
718  if (point1 < s.point1)
719  return true;
720  else if (s.point1 < point1)
721  return false;
722  else
723  return point2 < s.point2;
724 }
725 
726 double TLine2D::evaluatePoint(const TPoint2D& point) const
727 {
728  return coefs[0] * point.x + coefs[1] * point.y + coefs[2];
729 }
730 bool TLine2D::contains(const TPoint2D& point) const
731 {
732  return abs(distance(point)) < getEpsilon();
733 }
734 double TLine2D::distance(const TPoint2D& point) const
735 {
736  return abs(evaluatePoint(point)) /
737  sqrt(coefs[0] * coefs[0] + coefs[1] * coefs[1]);
738 }
739 double TLine2D::signedDistance(const TPoint2D& point) const
740 {
741  return evaluatePoint(point) /
742  sqrt(coefs[0] * coefs[0] + coefs[1] * coefs[1]);
743 }
744 void TLine2D::getNormalVector(double (&vector)[2]) const
745 {
746  vector[0] = coefs[0];
747  vector[1] = coefs[1];
748 }
750 {
751  double s = sqrt(coefs[0] * coefs[0] + coefs[1] * coefs[1]);
752  for (double& coef : coefs) coef /= s;
753 }
754 void TLine2D::getDirectorVector(double (&vector)[2]) const
755 {
756  vector[0] = -coefs[1];
757  vector[1] = coefs[0];
758 }
759 void TLine2D::generate3DObject(TLine3D& l) const { l = TLine3D(*this); }
760 void TLine2D::getAsPose2D(TPose2D& outPose) const
761 {
762  // Line's director vector is (-coefs[1],coefs[0]).
763  // If line is horizontal, force x=0. Else, force y=0. In both cases, we'll
764  // find a suitable point.
765  outPose.phi = atan2(coefs[0], -coefs[1]);
766  if (abs(coefs[0]) < getEpsilon())
767  {
768  outPose.x = 0;
769  outPose.y = -coefs[2] / coefs[1];
770  }
771  else
772  {
773  outPose.x = -coefs[2] / coefs[0];
774  outPose.y = 0;
775  }
776 }
778  const TPoint2D& origin, TPose2D& outPose) const
779 {
780  if (!contains(origin))
781  throw std::logic_error("Base point is not contained in the line");
782  outPose = origin;
783  // Line's director vector is (-coefs[1],coefs[0]).
784  outPose.phi = atan2(coefs[0], -coefs[1]);
785 }
786 TLine2D::TLine2D(const TPoint2D& p1, const TPoint2D& p2)
787 {
788  if (p1 == p2) throw logic_error("Both points are the same");
789  coefs[0] = p2.y - p1.y;
790  coefs[1] = p1.x - p2.x;
791  coefs[2] = p2.x * p1.y - p2.y * p1.x;
792 }
794 {
795  coefs[0] = s.point2.y - s.point1.y;
796  coefs[1] = s.point1.x - s.point2.x;
797  coefs[2] = s.point2.x * s.point1.y - s.point2.y * s.point1.x;
798  // unitarize(); //¿?
799 }
801 {
802  // Line's projection to Z plane may be a point.
803  if (hypot(l.director[0], l.director[1]) < getEpsilon())
804  throw std::logic_error("Line is normal to projection plane");
805  coefs[0] = -l.director[1];
806  coefs[1] = l.director[0];
807  coefs[2] = l.pBase.x * l.director[1] - l.pBase.y * l.director[0];
808 }
809 
810 void TLine3D::generate2DObject(TLine2D& l) const { l = TLine2D(*this); }
811 
812 bool TLine3D::contains(const TPoint3D& point) const
813 {
814  double dx = point.x - pBase.x;
815  double dy = point.y - pBase.y;
816  double dz = point.z - pBase.z;
817  if (abs(dx) < getEpsilon() && abs(dy) < getEpsilon() &&
818  abs(dz) < getEpsilon())
819  return true;
820  // dx dy dz
821  // if -----------=-----------=-----------, point is inside the line.
822  // director[0] director[1] director[2]
823  return (abs(dx * director[1] - dy * director[0]) < getEpsilon()) &&
824  (abs(dx * director[2] - dz * director[0]) < getEpsilon()) &&
825  (abs(dy * director[2] - dz * director[1]) < getEpsilon());
826 }
827 double TLine3D::distance(const TPoint3D& point) const
828 {
829  // Let d be line's base point minus the argument. Then,
830  // d·director/(|d|·|director|) equals both vector's cosine.
831  // So, d·director/|director| equals d's projection over director. Then,
832  // distance is sqrt(|d|²-(d·director/|director|)²).
833  double d[3] = {point.x - pBase.x, point.y - pBase.y, point.z - pBase.z};
834  double dv = 0, d2 = 0, v2 = 0;
835  for (size_t i = 0; i < 3; i++)
836  {
837  dv += d[i] * director[i];
838  d2 += d[i] * d[i];
839  v2 += director[i] * director[i];
840  }
841  return sqrt(d2 - (dv * dv) / v2);
842 }
844 {
845  double s = sqrt(squareNorm<3, double>(director));
846  for (double& i : director) i /= s;
847 }
848 TLine3D::TLine3D(const TPoint3D& p1, const TPoint3D& p2)
849 {
850  if (abs(math::distance(p1, p2)) < getEpsilon())
851  throw logic_error("Both points are the same");
852  pBase = p1;
853  director[0] = p2.x - p1.x;
854  director[1] = p2.y - p1.y;
855  director[2] = p2.z - p1.z;
856 }
858 {
859  pBase = s.point1;
860  director[0] = s.point2.x - s.point1.x;
861  director[1] = s.point2.y - s.point1.y;
862  director[2] = s.point2.z - s.point1.z;
863 }
865 {
866  director[0] = -l.coefs[1];
867  director[1] = l.coefs[0];
868  director[2] = 0;
869  // We assume that either l.coefs[0] or l.coefs[1] is not null. Respectively,
870  // either y or x can be used as free cordinate.
871  if (abs(l.coefs[0]) >= getEpsilon())
872  {
873  pBase.x = -l.coefs[2] / l.coefs[0];
874  pBase.y = 0;
875  }
876  else
877  {
878  pBase.x = 0;
879  pBase.y = -l.coefs[1] / l.coefs[0];
880  }
881  pBase.z = 0;
882 }
883 
884 double TPlane::evaluatePoint(const TPoint3D& point) const
885 {
886  return dotProduct<3, double>(coefs, point) + coefs[3];
887 }
888 bool TPlane::contains(const TPoint3D& point) const
889 {
890  return distance(point) < getEpsilon();
891 }
892 bool TPlane::contains(const TLine3D& line) const
893 {
894  if (!contains(line.pBase)) return false; // Base point must be contained
895  return abs(getAngle(*this, line)) <
896  getEpsilon(); // Plane's normal must be normal to director vector
897 }
898 double TPlane::distance(const TPoint3D& point) const
899 {
900  return abs(evaluatePoint(point)) / sqrt(squareNorm<3, double>(coefs));
901 }
902 double TPlane::distance(const TLine3D& line) const
903 {
904  if (abs(getAngle(*this, line)) >= getEpsilon())
905  return 0; // Plane crosses with line
906  else
907  return distance(line.pBase);
908 }
909 void TPlane::getNormalVector(double (&vector)[3]) const
910 {
911  for (int i = 0; i < 3; i++) vector[i] = coefs[i];
912 }
914 {
915  TVector3D v;
916  for (int i = 0; i < 3; i++) v[i] = coefs[i];
917  return v;
918 }
919 
920 void TPlane::getUnitaryNormalVector(double (&vec)[3]) const
921 {
922  const double s = sqrt(squareNorm<3, double>(coefs));
924  const double k = 1.0 / s;
925  for (int i = 0; i < 3; i++) vec[i] = coefs[i] * k;
926 }
927 
929 {
930  double s = sqrt(squareNorm<3, double>(coefs));
931  for (double& coef : coefs) coef /= s;
932 }
933 
934 // Returns a 6D pose such as its XY plane coincides with the plane
936 {
937  double normal[3];
938  getUnitaryNormalVector(normal);
939  CMatrixDouble44 AXIS;
940  generateAxisBaseFromDirectionAndAxis(normal, 2, AXIS);
941  for (size_t i = 0; i < 3; i++)
942  if (abs(coefs[i]) >= getEpsilon())
943  {
944  AXIS(i, 3) = -coefs[3] / coefs[i];
945  break;
946  }
947  outPose.fromHomogeneousMatrix(AXIS);
948 }
950  const TPoint3D& center, TPose3D& pose) const
951 {
952  if (!contains(center))
953  throw std::logic_error("Base point is not in the plane.");
954  double normal[3];
955  getUnitaryNormalVector(normal);
956  CMatrixDouble44 AXIS;
957  generateAxisBaseFromDirectionAndAxis(normal, 2, AXIS);
958  for (size_t i = 0; i < 3; i++) AXIS(i, 3) = center[i];
959  pose.fromHomogeneousMatrix(AXIS);
960 }
961 TPlane::TPlane(const TPoint3D& p1, const TPoint3D& p2, const TPoint3D& p3)
962 {
963  double dx1 = p2.x - p1.x;
964  double dy1 = p2.y - p1.y;
965  double dz1 = p2.z - p1.z;
966  double dx2 = p3.x - p1.x;
967  double dy2 = p3.y - p1.y;
968  double dz2 = p3.z - p1.z;
969  coefs[0] = dy1 * dz2 - dy2 * dz1;
970  coefs[1] = dz1 * dx2 - dz2 * dx1;
971  coefs[2] = dx1 * dy2 - dx2 * dy1;
972  if (abs(coefs[0]) < getEpsilon() && abs(coefs[1]) < getEpsilon() &&
973  abs(coefs[2]) < getEpsilon())
974  throw logic_error("Points are linearly dependent");
975  coefs[3] = -coefs[0] * p1.x - coefs[1] * p1.y - coefs[2] * p1.z;
976 }
977 TPlane::TPlane(const TPoint3D& p1, const TLine3D& r2)
978 {
979  double dx1 = p1.x - r2.pBase.x;
980  double dy1 = p1.y - r2.pBase.y;
981  double dz1 = p1.z - r2.pBase.z;
982  coefs[0] = dy1 * r2.director[2] - dz1 * r2.director[1];
983  coefs[1] = dz1 * r2.director[0] - dx1 * r2.director[2];
984  coefs[2] = dx1 * r2.director[1] - dy1 * r2.director[0];
985  if (abs(coefs[0]) < getEpsilon() && abs(coefs[1]) < getEpsilon() &&
986  abs(coefs[2]) < getEpsilon())
987  throw logic_error("Point is contained in the line");
988  coefs[3] = -coefs[0] * p1.x - coefs[1] * p1.y - coefs[2] * p1.z;
989 }
990 TPlane::TPlane(const TPoint3D& p1, const TVector3D& normal)
991 {
992  const double normal_norm = normal.norm();
993  ASSERT_ABOVE_(normal_norm, getEpsilon());
994 
995  // Ensure we have a unit vector:
996  const auto n = normal * (1. / normal_norm);
997  coefs[0] = n.x;
998  coefs[1] = n.y;
999  coefs[2] = n.z;
1000  coefs[3] = -coefs[0] * p1.x - coefs[1] * p1.y - coefs[2] * p1.z;
1001 }
1002 TPlane::TPlane(const TLine3D& r1, const TLine3D& r2)
1003 {
1005  coefs[3] =
1006  -coefs[0] * r1.pBase.x - coefs[1] * r1.pBase.y - coefs[2] * r1.pBase.z;
1007  if (abs(coefs[0]) < getEpsilon() && abs(coefs[1]) < getEpsilon() &&
1008  abs(coefs[2]) < getEpsilon())
1009  {
1010  // Lines are parallel
1011  if (r1.contains(r2.pBase)) throw std::logic_error("Lines are the same");
1012  // Use a line's director vector and both pBase's difference to create
1013  // the plane.
1014  double d[3];
1015  for (size_t i = 0; i < 3; i++) d[i] = r1.pBase[i] - r2.pBase[i];
1016  crossProduct3D(r1.director, d, coefs);
1017  coefs[3] = -coefs[0] * r1.pBase.x - coefs[1] * r1.pBase.y -
1018  coefs[2] * r1.pBase.z;
1019  }
1020  else if (abs(evaluatePoint(r2.pBase)) >= getEpsilon())
1021  throw logic_error("Lines do not intersect");
1022 }
1023 
1024 template <class T>
1025 inline void removeUnusedVertices(T& poly)
1026 {
1027  size_t N = poly.size();
1028  if (N < 3) return;
1029  std::vector<size_t> unused;
1030  if (abs(mrpt::math::distance(poly[N - 1], poly[0]) +
1031  mrpt::math::distance(poly[0], poly[1]) -
1032  mrpt::math::distance(poly[N - 1], poly[1])) <
1034  unused.push_back(0);
1035  for (size_t i = 1; i < N - 1; i++)
1036  if (abs(mrpt::math::distance(poly[i - 1], poly[i]) +
1037  mrpt::math::distance(poly[i], poly[i + 1]) -
1038  mrpt::math::distance(poly[i - 1], poly[i + 1])) <
1040  unused.push_back(i);
1041  if (abs(mrpt::math::distance(poly[N - 2], poly[N - 1]) +
1042  mrpt::math::distance(poly[N - 1], poly[0]) -
1043  mrpt::math::distance(poly[N - 2], poly[0])) <
1045  unused.push_back(N - 1);
1046  unused.push_back(N);
1047  size_t diff = 1;
1048  for (size_t i = 0; i < unused.size() - 1; i++)
1049  {
1050  size_t last = unused[i + 1];
1051  for (size_t j = unused[i] + 1 - diff; j < last - diff; j++)
1052  poly[j] = poly[j + diff];
1053  }
1054  poly.resize(N + 1 - unused.size());
1055 }
1056 template <class T>
1057 inline void removeRepVertices(T& poly)
1058 {
1059  size_t N = poly.size();
1060  if (N < 3) return;
1061  std::vector<size_t> rep;
1062  for (size_t i = 0; i < N - 1; i++)
1063  if (mrpt::math::distance(poly[i], poly[i + 1]) < getEpsilon())
1064  rep.push_back(i);
1065  if (mrpt::math::distance(poly[N - 1], poly[0]) < getEpsilon())
1066  rep.push_back(N - 1);
1067  rep.push_back(N);
1068  size_t diff = 1;
1069  for (size_t i = 0; i < rep.size() - 1; i++)
1070  {
1071  size_t last = rep[i + 1];
1072  for (size_t j = rep[i] + 1 - diff; j < last - diff; j++)
1073  poly[j] = poly[j + diff];
1074  }
1075  poly.resize(N + 1 - rep.size());
1076 }
1077 
1078 double TPolygon2D::distance(const TPoint2D& point) const
1079 {
1080  if (contains(point)) return 0;
1081  std::vector<TSegment2D> sgs;
1082  getAsSegmentList(sgs);
1083 
1084  if (sgs.empty())
1085  THROW_EXCEPTION("Cannot compute distance to an empty polygon.");
1086 
1087  double distance = std::numeric_limits<double>::max();
1088 
1089  for (auto it = sgs.begin(); it != sgs.end(); ++it)
1090  {
1091  double d = (*it).distance(point);
1092  if (d < distance) distance = d;
1093  }
1094  return distance;
1095 }
1096 
1098  TPoint2D& min_coords, TPoint2D& max_coords) const
1099 {
1100  ASSERTMSG_(!this->empty(), "getBoundingBox() called on an empty polygon!");
1101  min_coords.x = min_coords.y = std::numeric_limits<double>::max();
1102  max_coords.x = max_coords.y = -std::numeric_limits<double>::max();
1103  for (size_t i = 0; i < size(); i++)
1104  {
1105  mrpt::keep_min(min_coords.x, (*this)[i].x);
1106  mrpt::keep_min(min_coords.y, (*this)[i].y);
1107  mrpt::keep_max(max_coords.x, (*this)[i].x);
1108  mrpt::keep_max(max_coords.y, (*this)[i].y);
1109  }
1110 }
1111 
1112 // isLeft(): tests if a point is Left|On|Right of an infinite line.
1113 // Input: three points P0, P1, and P2
1114 // Return: >0 for P2 left of the line through P0 and P1
1115 // =0 for P2 on the line
1116 // <0 for P2 right of the line
1117 // See: Algorithm 1 "Area of Triangles and Polygons"
1118 inline double isLeft(
1119  const mrpt::math::TPoint2D& P0, const mrpt::math::TPoint2D& P1,
1120  const mrpt::math::TPoint2D& P2)
1121 {
1122  return ((P1.x - P0.x) * (P2.y - P0.y) - (P2.x - P0.x) * (P1.y - P0.y));
1123 }
1124 
1125 bool TPolygon2D::contains(const TPoint2D& P) const
1126 {
1127  int wn = 0; // the winding number counter
1128 
1129  // loop through all edges of the polygon
1130  const size_t n = this->size();
1131  for (size_t i = 0; i < n; i++) // edge from V[i] to V[i+1]
1132  {
1133  if ((*this)[i].y <= P.y)
1134  {
1135  // start y <= P.y
1136  if ((*this)[(i + 1) % n].y > P.y) // an upward crossing
1137  if (isLeft((*this)[i], (*this)[(i + 1) % n], P) >
1138  0) // P left of edge
1139  ++wn; // have a valid up intersect
1140  }
1141  else
1142  {
1143  // start y > P.y (no test needed)
1144  if ((*this)[(i + 1) % n].y <= P.y) // a downward crossing
1145  if (isLeft((*this)[i], (*this)[(i + 1) % n], P) <
1146  0) // P right of edge
1147  --wn; // have a valid down intersect
1148  }
1149  }
1150 
1151  return wn != 0;
1152 }
1153 void TPolygon2D::getAsSegmentList(vector<TSegment2D>& v) const
1154 {
1155  size_t N = size();
1156  v.resize(N);
1157  for (size_t i = 0; i < N - 1; i++)
1158  v[i] = TSegment2D(operator[](i), operator[](i + 1));
1159  v[N - 1] = TSegment2D(operator[](N - 1), operator[](0));
1160 }
1161 
1163 {
1164  p = TPolygon3D(*this);
1165 }
1166 // Auxiliar functor class to compute polygon's center
1167 template <class T, int N>
1169 {
1170  public:
1172  FAddPoint(T& o) : object(o)
1173  {
1174  for (size_t i = 0; i < N; i++) object[i] = 0.0;
1175  }
1176  void operator()(const T& o)
1177  {
1178  for (size_t i = 0; i < N; i++) object[i] += o[i];
1179  }
1180 };
1182 {
1183  for_each(begin(), end(), FAddPoint<TPoint2D, 2>(p));
1184  size_t N = size();
1185  p.x /= N;
1186  p.y /= N;
1187 }
1189 {
1190  size_t N = size();
1191  if (N <= 3) return true;
1192  vector<TSegment2D> sgms;
1193  getAsSegmentList(sgms);
1194  for (size_t i = 0; i < N; i++)
1195  {
1196  char s = 0;
1197  auto l = TLine2D(sgms[i]);
1198  for (size_t j = 0; j < N; j++)
1199  {
1200  double d = l.evaluatePoint(operator[](j));
1201  if (abs(d) < getEpsilon())
1202  continue;
1203  else if (!s)
1204  s = (d > 0) ? 1 : -1;
1205  else if (s != ((d > 0) ? 1 : -1))
1206  return false;
1207  }
1208  }
1209  return true;
1210 }
1213 {
1215  removeUnusedVertices(*this);
1216 }
1218  std::vector<double>& x, std::vector<double>& y) const
1219 {
1220  size_t N = size();
1221  x.resize(N + 1);
1222  y.resize(N + 1);
1223  for (size_t i = 0; i < N; i++)
1224  {
1225  x[i] = operator[](i).x;
1226  y[i] = operator[](i).y;
1227  }
1228  x[N] = operator[](0).x;
1229  y[N] = operator[](0).y;
1230 }
1232 {
1233  size_t N = p.size();
1234  resize(N);
1235  for (size_t i = 0; i < N; i++) operator[](i) = TPoint2D(p[i]);
1236 }
1238  size_t numEdges, double radius, TPolygon2D& poly)
1239 {
1240  if (numEdges < 3 || abs(radius) < getEpsilon())
1241  throw std::logic_error(
1242  "Invalid arguments for regular polygon creations");
1243  poly.resize(numEdges);
1244  for (size_t i = 0; i < numEdges; i++)
1245  {
1246  double angle = i * M_PI * 2 / numEdges;
1247  poly[i] = TPoint2D(radius * cos(angle), radius * sin(angle));
1248  }
1249 }
1251  size_t numEdges, double radius, TPolygon2D& poly, const TPose2D& pose)
1252 {
1253  createRegularPolygon(numEdges, radius, poly);
1254  for (size_t i = 0; i < numEdges; i++) poly[i] = pose.composePoint(poly[i]);
1255 }
1256 
1258 {
1259  p = TPolygon2D(*this);
1260 }
1261 
1262 double TPolygon3D::distance(const TPoint3D& point) const
1263 {
1264  TPlane pl;
1265  if (!getPlane(pl))
1266  throw std::logic_error("Polygon does not conform a plane");
1267  TPoint3D newPoint;
1268  TPolygon3D newPoly;
1269  TPose3D pose;
1270  pl.getAsPose3DForcingOrigin(operator[](0), pose);
1271  project3D(point, pose, newPoint);
1272  project3D(*this, pose, newPoly);
1273  double distance2D = TPolygon2D(newPoly).distance(TPoint2D(newPoint));
1274  return sqrt(newPoint.z * newPoint.z + distance2D * distance2D);
1275 }
1276 bool TPolygon3D::contains(const TPoint3D& point) const
1277 {
1278  TPoint3D pMin, pMax;
1279  getPrismBounds(*this, pMin, pMax);
1280  if (point.x + getEpsilon() < pMin.x || point.y + getEpsilon() < pMin.y ||
1281  point.z + getEpsilon() < pMin.z || point.x > pMax.x + getEpsilon() ||
1282  point.y > pMax.y + getEpsilon() || point.z > pMax.z + getEpsilon())
1283  return false;
1284  TPlane plane;
1285  if (!getPlane(plane))
1286  throw std::logic_error("Polygon does not conform a plane");
1287  TPolygon3D projectedPoly;
1288  TPoint3D projectedPoint;
1289  TPose3D pose;
1290  // plane.getAsPose3DForcingOrigin(operator[](0),pose);
1291  plane.getAsPose3D(pose);
1292  CMatrixDouble44 P_inv;
1293  pose.getInverseHomogeneousMatrix(P_inv);
1294  pose.fromHomogeneousMatrix(P_inv);
1295  project3D(point, pose, projectedPoint);
1296  if (abs(projectedPoint.z) >= getEpsilon())
1297  return false; // Point is not inside the polygon's plane.
1298  project3D(*this, pose, projectedPoly);
1299  return TPolygon2D(projectedPoly).contains(TPoint2D(projectedPoint));
1300 }
1301 void TPolygon3D::getAsSegmentList(vector<TSegment3D>& v) const
1302 {
1303  size_t N = size();
1304  v.resize(N);
1305  for (size_t i = 0; i < N - 1; i++)
1306  v[i] = TSegment3D(operator[](i), operator[](i + 1));
1307  v[N - 1] = TSegment3D(operator[](N - 1), operator[](0));
1308 }
1309 bool TPolygon3D::getPlane(TPlane& p) const { return conformAPlane(*this, p); }
1311 {
1312  getRegressionPlane(*this, p);
1313 }
1315 {
1316  for_each(begin(), end(), FAddPoint<TPoint3D, 3>(p));
1317  size_t N = size();
1318  p.x /= N;
1319  p.y /= N;
1320  p.z /= N;
1321 }
1322 bool TPolygon3D::isSkew() const { return !mrpt::math::conformAPlane(*this); }
1325 {
1327  removeUnusedVertices(*this);
1328 }
1330 {
1331  size_t N = p.size();
1332  resize(N);
1333  for (size_t i = 0; i < N; i++) operator[](i) = p[i];
1334 }
1336  size_t numEdges, double radius, TPolygon3D& poly)
1337 {
1338  if (numEdges < 3 || abs(radius) < getEpsilon())
1339  throw std::logic_error(
1340  "Invalid arguments for regular polygon creations");
1341  poly.resize(numEdges);
1342  for (size_t i = 0; i < numEdges; i++)
1343  {
1344  double angle = i * 2 * M_PI / numEdges;
1345  poly[i] = TPoint3D(radius * cos(angle), radius * sin(angle), 0);
1346  }
1347 }
1349  size_t numEdges, double radius, TPolygon3D& poly, const TPose3D& pose)
1350 {
1351  createRegularPolygon(numEdges, radius, poly);
1352  for (size_t i = 0; i < numEdges; i++) pose.composePoint(poly[i], poly[i]);
1353 }
1354 
1356 {
1357  switch (type)
1358  {
1359  case GEOMETRIC_TYPE_POINT:
1360  obj = TPoint3D(data.point);
1361  break;
1363  obj = TSegment3D(data.segment);
1364  break;
1365  case GEOMETRIC_TYPE_LINE:
1366  obj = TLine3D(data.line);
1367  break;
1369  obj = TPolygon3D(*(data.polygon));
1370  break;
1371  default:
1372  obj = TObject3D();
1373  break;
1374  }
1375 }
1377  const std::vector<TObject2D>& objs, std::vector<TPoint2D>& pnts)
1378 {
1379  for (const auto& obj : objs)
1380  if (obj.isPoint()) pnts.push_back(obj.data.point);
1381 }
1383  const std::vector<TObject2D>& objs, std::vector<TSegment2D>& sgms)
1384 {
1385  for (const auto& obj : objs)
1386  if (obj.isSegment()) sgms.push_back(obj.data.segment);
1387 }
1389  const std::vector<TObject2D>& objs, std::vector<TLine2D>& lins)
1390 {
1391  for (const auto& obj : objs)
1392  if (obj.isLine()) lins.push_back(obj.data.line);
1393 }
1395  const std::vector<TObject2D>& objs, std::vector<TPolygon2D>& polys)
1396 {
1397  for (const auto& obj : objs)
1398  if (obj.isPolygon()) polys.push_back(*(obj.data.polygon));
1399 }
1401  const std::vector<TObject2D>& objs, std::vector<TPoint2D>& pnts,
1402  std::vector<TObject2D>& remainder)
1403 {
1404  for (const auto& obj : objs)
1405  if (obj.isPoint())
1406  pnts.push_back(obj.data.point);
1407  else
1408  remainder.push_back(obj);
1409 }
1411  const std::vector<TObject2D>& objs, std::vector<TSegment2D>& sgms,
1412  std::vector<TObject2D>& remainder)
1413 {
1414  for (const auto& obj : objs)
1415  if (obj.isSegment())
1416  sgms.push_back(obj.data.segment);
1417  else
1418  remainder.push_back(obj);
1419 }
1421  const std::vector<TObject2D>& objs, std::vector<TLine2D>& lins,
1422  std::vector<TObject2D>& remainder)
1423 {
1424  for (const auto& obj : objs)
1425  if (obj.isLine())
1426  lins.push_back(obj.data.line);
1427  else
1428  remainder.push_back(obj);
1429 }
1431  const std::vector<TObject2D>& objs, std::vector<TPolygon2D>& polys,
1432  vector<TObject2D>& remainder)
1433 {
1434  for (const auto& obj : objs)
1435  if (obj.isPolygon())
1436  polys.push_back(*(obj.data.polygon));
1437  else
1438  remainder.push_back(obj);
1439 }
1440 
1442 {
1443  switch (type)
1444  {
1445  case GEOMETRIC_TYPE_POINT:
1446  obj = TPoint2D(data.point);
1447  break;
1449  obj = TSegment2D(data.segment);
1450  break;
1451  case GEOMETRIC_TYPE_LINE:
1452  obj = TLine2D(data.line);
1453  break;
1455  obj = TPolygon2D(*(data.polygon));
1456  break;
1457  case GEOMETRIC_TYPE_PLANE:
1458  throw std::logic_error("Too many dimensions");
1459  default:
1460  obj = TObject2D();
1461  break;
1462  }
1463 }
1464 
1466  const std::vector<TObject3D>& objs, std::vector<TPoint3D>& pnts)
1467 {
1468  for (const auto& obj : objs)
1469  if (obj.isPoint()) pnts.push_back(obj.data.point);
1470 }
1472  const std::vector<TObject3D>& objs, std::vector<TSegment3D>& sgms)
1473 {
1474  for (const auto& obj : objs)
1475  if (obj.isSegment()) sgms.push_back(obj.data.segment);
1476 }
1478  const std::vector<TObject3D>& objs, std::vector<TLine3D>& lins)
1479 {
1480  for (const auto& obj : objs)
1481  if (obj.isLine()) lins.push_back(obj.data.line);
1482 }
1484  const std::vector<TObject3D>& objs, std::vector<TPlane>& plns)
1485 {
1486  for (const auto& obj : objs)
1487  if (obj.isPlane()) plns.push_back(obj.data.plane);
1488 }
1490  const std::vector<TObject3D>& objs, std::vector<TPolygon3D>& polys)
1491 {
1492  for (const auto& obj : objs)
1493  if (obj.isPolygon()) polys.push_back(*(obj.data.polygon));
1494 }
1496  const std::vector<TObject3D>& objs, std::vector<TPoint3D>& pnts,
1497  std::vector<TObject3D>& remainder)
1498 {
1499  for (const auto& obj : objs)
1500  if (obj.isPoint())
1501  pnts.push_back(obj.data.point);
1502  else
1503  remainder.push_back(obj);
1504 }
1506  const std::vector<TObject3D>& objs, std::vector<TSegment3D>& sgms,
1507  std::vector<TObject3D>& remainder)
1508 {
1509  for (const auto& obj : objs)
1510  if (obj.isSegment())
1511  sgms.push_back(obj.data.segment);
1512  else
1513  remainder.push_back(obj);
1514 }
1516  const std::vector<TObject3D>& objs, std::vector<TLine3D>& lins,
1517  std::vector<TObject3D>& remainder)
1518 {
1519  for (const auto& obj : objs)
1520  if (obj.isLine())
1521  lins.push_back(obj.data.line);
1522  else
1523  remainder.push_back(obj);
1524 }
1526  const std::vector<TObject3D>& objs, std::vector<TPlane>& plns,
1527  std::vector<TObject3D>& remainder)
1528 {
1529  for (const auto& obj : objs)
1530  if (obj.isPlane())
1531  plns.push_back(obj.data.plane);
1532  else
1533  remainder.push_back(obj);
1534 }
1536  const std::vector<TObject3D>& objs, std::vector<TPolygon3D>& polys,
1537  vector<TObject3D>& remainder)
1538 {
1539  for (const auto& obj : objs)
1540  if (obj.isPolygon())
1541  polys.push_back(*(obj.data.polygon));
1542  else
1543  remainder.push_back(obj);
1544 }
1545 
1547 {
1548  for (size_t i = 0; i < o.size(); i++) in >> o[i];
1549  return in;
1550 }
1552 {
1553  for (size_t i = 0; i < o.size(); i++) out << o[i];
1554  return out;
1555 }
1556 
1558 {
1559  for (size_t i = 0; i < o.size(); i++) in >> o[i];
1560  return in;
1561 }
1563 {
1564  for (size_t i = 0; i < o.size(); i++) out << o[i];
1565  return out;
1566 }
1567 
1569 {
1570  uint16_t type;
1571  in >> type;
1572  switch (static_cast<unsigned char>(type))
1573  {
1574  case GEOMETRIC_TYPE_POINT:
1575  {
1576  TPoint2D p;
1577  in >> p;
1578  o = p;
1579  }
1580  break;
1582  {
1583  TSegment2D s;
1584  in >> s;
1585  o = s;
1586  }
1587  break;
1588  case GEOMETRIC_TYPE_LINE:
1589  {
1590  TLine2D l;
1591  in >> l;
1592  o = l;
1593  }
1594  break;
1596  {
1597  TPolygon2D p;
1598  in >> p;
1599  o = p;
1600  }
1601  break;
1603  {
1604  o = TObject2D();
1605  }
1606  break;
1607  default:
1608  throw std::logic_error(
1609  "Unknown TObject2D type found while reading stream");
1610  }
1611  return in;
1612 }
1614 {
1615  out << static_cast<uint16_t>(o.getType());
1616  switch (o.getType())
1617  {
1618  case GEOMETRIC_TYPE_POINT:
1619  {
1620  TPoint2D p;
1621  o.getPoint(p);
1622  return out << p;
1623  };
1625  {
1626  TSegment2D s;
1627  o.getSegment(s);
1628  return out << s;
1629  };
1630  case GEOMETRIC_TYPE_LINE:
1631  {
1632  TLine2D l;
1633  o.getLine(l);
1634  return out << l;
1635  };
1637  {
1638  TPolygon2D p;
1639  o.getPolygon(p);
1640  return out << p;
1641  };
1642  }
1643  return out;
1644 }
1645 
1647 {
1648  uint16_t type;
1649  in >> type;
1650  switch (static_cast<unsigned char>(type))
1651  {
1652  case GEOMETRIC_TYPE_POINT:
1653  {
1654  TPoint3D p;
1655  in >> p;
1656  o = p;
1657  }
1658  break;
1660  {
1661  TSegment3D s;
1662  in >> s;
1663  o = s;
1664  }
1665  break;
1666  case GEOMETRIC_TYPE_LINE:
1667  {
1668  TLine3D l;
1669  in >> l;
1670  o = l;
1671  }
1672  break;
1673  case GEOMETRIC_TYPE_PLANE:
1674  {
1675  TPlane p;
1676  in >> p;
1677  o = p;
1678  }
1679  break;
1681  {
1682  TPolygon3D p;
1683  in >> p;
1684  o = p;
1685  }
1686  break;
1688  {
1689  o = TObject3D();
1690  }
1691  break;
1692  default:
1693  throw std::logic_error(
1694  "Unknown TObject3D type found while reading stream");
1695  }
1696  return in;
1697 }
1699 {
1700  out << static_cast<uint16_t>(o.getType());
1701  switch (o.getType())
1702  {
1703  case GEOMETRIC_TYPE_POINT:
1704  {
1705  TPoint3D p;
1706  o.getPoint(p);
1707  return out << p;
1708  };
1710  {
1711  TSegment3D s;
1712  o.getSegment(s);
1713  return out << s;
1714  };
1715  case GEOMETRIC_TYPE_LINE:
1716  {
1717  TLine3D l;
1718  o.getLine(l);
1719  return out << l;
1720  };
1721  case GEOMETRIC_TYPE_PLANE:
1722  {
1723  TPlane p;
1724  o.getPlane(p);
1725  return out << p;
1726  };
1728  {
1729  TPolygon3D p;
1730  o.getPolygon(p);
1731  return out << p;
1732  };
1733  }
1734  return out;
1735 }
1736 } // namespace mrpt::math
void project3D(const TPoint3D &point, const mrpt::math::TPose3D &newXYpose, TPoint3D &newPoint)
Uses the given pose 3D to project a point into a new base.
Definition: geometry.h:329
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
static void getSegments(const std::vector< TObject2D > &objs, std::vector< TSegment2D > &sgms)
Static method to retrieve all the segments in a vector of TObject2D.
bool getPoint(TPoint2D &p) const
Gets the content as a point, returning false if the type is inadequate.
Definition: TObject2D.h:109
void generateAxisBaseFromDirectionAndAxis(const double(&vec)[3], uint8_t coord, CMatrixDouble44 &matrix)
Creates a homogeneus matrix (4x4) such that the coordinate given (0 for x, 1 for y, 2 for z) corresponds to the provided vector.
Definition: geometry.cpp:2080
static void createRegularPolygon(size_t numEdges, double radius, TPolygon3D &poly)
Static method to create a regular polygon, given its size and radius.
static void createRegularPolygon(size_t numEdges, double radius, TPolygon2D &poly)
Static method to create a regular polygon, given its size and radius.
void inverseComposePoint(const TPoint3D &g, TPoint3D &l) const
double x
X,Y coordinates.
Definition: TPoint2D.h:23
void fromString(const std::string &s)
Set the current object value from a string generated by &#39;asString&#39; (eg: "[0.02 1.04]" ) ...
bool getPolygon(TPolygon2D &p) const
Gets the content as a polygon, returning false if the type is inadequate.
Definition: TObject2D.h:150
void rotate(const double ang)
Transform the (vx,vy) components for a counterclockwise rotation of ang radians.
GLdouble GLdouble t
Definition: glext.h:3695
GLdouble GLdouble z
Definition: glext.h:3879
double x
X,Y,Z coordinates.
Definition: TPoint3D.h:83
static void SO3_to_yaw_pitch_roll(const mrpt::math::CMatrixDouble33 &R, double &yaw, double &pitch, double &roll)
double RAD2DEG(const double x)
Radians to degrees.
Trivially copiable underlying data for TPoint3D.
Definition: TPoint3D.h:80
unsigned char getType() const
Gets content type.
Definition: TObject2D.h:105
bool operator==(const TTwist3D &o) const
double x
X,Y coordinates.
Definition: TPose2D.h:30
void getRotationMatrix(mrpt::math::CMatrixDouble33 &R) const
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
double distance(const TPoint3D &point) const
Distance between the line and a point.
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
static constexpr unsigned char GEOMETRIC_TYPE_POLYGON
Object type identifier for TPolygon2D or TPolygon3D.
Definition: TPoseOrPoint.h:116
void getPlotData(std::vector< double > &x, std::vector< double > &y) const
Gets plot data, ready to use on a 2D plot.
size_t size(const MATRIXLIKE &m, const int dim)
bool isSkew() const
Check whether the polygon is skew.
bool getSegment(TSegment3D &s) const
Gets the content as a segment, returning false if the type is not adequate.
Definition: TObject3D.h:134
bool contains(const TPoint2D &point) const
Check whether a point is inside a segment.
double roll
Roll coordinate (rotation angle over X coordinate).
Definition: TPose3D.h:37
static void getPolygons(const std::vector< TObject2D > &objs, std::vector< TPolygon2D > &polys)
Static method to retrieve all the polygons in a vector of TObject2D.
double DEG2RAD(const double x)
Degrees to radians.
mrpt::math::TPoint2D composePoint(const TPoint2D l) const
TPolygon3D()
Default constructor.
Definition: TPolygon3D.h:46
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3727
GLenum GLsizei n
Definition: glext.h:5136
double distance(const TPoint2D &point) const
Distance to point.
static void getLines(const std::vector< TObject2D > &objs, std::vector< TLine2D > &lins)
Static method to retrieve all the lines in a vector of TObject2D.
This file implements several operations that operate element-wise on individual or pairs of container...
mrpt::math::CMatrixDouble33 getRotationMatrix() const
Definition: TPose3D.h:167
double x
X,Y,Z, coords.
Definition: TPose3D.h:31
double norm() const
Point norm: |v| = sqrt(x^2+y^2+z^2)
Definition: TPoint3D.h:177
double signedDistance(const TPoint2D &point) const
Distance with sign from a given point (sign indicates side).
Standard type for storing any lightweight 2D type.
Definition: TObject2D.h:24
bool getLine(TLine2D &r) const
Gets the content as a line, returning false if the type is inadequate.
Definition: TObject2D.h:136
void getAsSegmentList(std::vector< TSegment2D > &v) const
Gets as set of segments, instead of points.
static constexpr unsigned char GEOMETRIC_TYPE_POINT
Object type identifier for TPoint2D or TPoint3D.
Definition: TPoseOrPoint.h:101
bool getPlane(TPlane &p) const
Gets the content as a plane, returning false if the type is not adequate.
Definition: TObject3D.h:175
TPoint3D pBase
Base point.
Definition: TLine3D.h:23
Standard object for storing any 3D lightweight object.
Definition: TObject3D.h:25
STL namespace.
bool getSegment(TSegment2D &s) const
Gets the content as a segment, returning false if the type is inadequate.
Definition: TObject2D.h:123
bool contains(const TPoint2D &point) const
Check whether a point is inside the line.
void removeRepeatedVertices()
Erase repeated vertices.
double signedDistance(const TPoint2D &point) const
Distance with sign to point (sign indicates which side the point is).
void getInverseHomogeneousMatrix(mrpt::math::CMatrixDouble44 &HG) const
void generate3DObject(TLine3D &l) const
Project into 3D space, setting the z to 0.
void removeRepVertices(T &poly)
void keep_min(T &var, const K test_val)
If the second argument is below the first one, set the first argument to this lower value...
void fromString(const std::string &s)
Set the current object value from a string generated by &#39;asString&#39; (eg: "[0.02 1.04 -45...
void getAsPose2D(TPose2D &outPose) const
double distance(const TPoint3D &point) const
Distance to 3D point.
double yaw
Yaw coordinate (rotation angle over Z axis).
Definition: TPose3D.h:33
GLdouble s
Definition: glext.h:3682
GLsizei GLsizei GLuint * obj
Definition: glext.h:4085
void fromString(const std::string &s)
Set the current object value from a string generated by &#39;asString&#39; (eg: "[vx vy vz wx wy wz]" ) ...
mrpt::math::CMatrixDouble44 getHomogeneousMatrix() const
Definition: TPose3D.h:174
bool operator<(const TSegment3D &s) const
VALUE & operator[](const KEY &key)
Write/read via [i] operator, that creates an element if it didn&#39;t exist already.
Definition: ts_hash_map.h:215
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:4199
3D twist: 3D velocity vector (vx,vy,vz) + angular velocity (wx,wy,wz)
Definition: TTwist3D.h:18
void crossProduct3D(const T &v0, const U &v1, V &vOut)
Computes the cross product of two 3D vectors, returning a vector normal to both.
Definition: geometry.h:804
TPoint3D point1
Origin point.
Definition: TSegment3D.h:27
bool conformAPlane(const std::vector< TPoint3D > &points)
Checks whether this polygon or set of points acceptably fits a plane.
Definition: geometry.cpp:989
static void getSegments(const std::vector< TObject3D > &objs, std::vector< TSegment3D > &sgms)
Static method to retrieve every segment included in a vector of objects.
void asVector(VECTORLIKE &v) const
Transformation into vector.
Definition: TPoint3D.h:191
void generate2DObject(TLine2D &l) const
Project into 2D space, discarding the Z coordinate.
2D twist: 2D velocity vector (vx,vy) + planar angular velocity (omega)
Definition: TTwist2D.h:19
T square(const T x)
Inline function for the square of a number.
void getNormalVector(double(&vector)[2]) const
Get line&#39;s normal vector.
void generate3DObject(TPolygon3D &p) const
Projects into 3D space, zeroing the z.
bool contains(const TPoint3D &point) const
Check whether a point is inside the line.
double isLeft(const mrpt::math::TPoint2D &P0, const mrpt::math::TPoint2D &P1, const mrpt::math::TPoint2D &P2)
static constexpr unsigned char GEOMETRIC_TYPE_PLANE
Object type identifier for TPlane.
Definition: TPoseOrPoint.h:121
void loadFromArray(const VECTOR &vals)
Definition: CMatrixFixed.h:171
void unitarize()
Unitarize line&#39;s normal vector.
CMatrixFixed< double, 3, 3 > CMatrixDouble33
Definition: CMatrixFixed.h:352
std::string asString() const
Definition: TTwist2D.h:97
void getBoundingBox(TPoint2D &min_coords, TPoint2D &max_coords) const
Get polygon bounding box.
bool getPolygon(TPolygon3D &p) const
Gets the content as a polygon, returning false if the type is not adequate.
Definition: TObject3D.h:161
This base provides a set of functions for maths stuff.
2D segment, consisting of two points.
Definition: TSegment2D.h:20
double distance(const TPoint3D &point) const
Distance to point.
void getUnitaryNormalVector(double(&vec)[3]) const
Get normal vector.
void fromString(const std::string &s)
Set the current object value from a string generated by &#39;asString&#39; (eg: "[0.02 1.04 -45...
const GLfloat * tc
Definition: glext.h:6459
void keep_max(T &var, const K test_val)
If the second argument is above the first one, set the first argument to this higher value...
3D segment, consisting of two points.
Definition: TSegment3D.h:21
TSegment2D()=default
Fast default constructor.
double length() const
Segment length.
std::array< double, 3 > director
Director vector.
Definition: TLine3D.h:25
void fromHomogeneousMatrix(const mrpt::math::CMatrixDouble44 &HG)
TPoint3D point2
Destiny point.
Definition: TSegment3D.h:31
bool operator<(const TSegment2D &s) const
const GLubyte * c
Definition: glext.h:6406
double vx
Velocity components: X,Y (m/s)
Definition: TTwist2D.h:26
auto block(int start_row, int start_col)
non-const block(): Returns an Eigen::Block reference to the block
mrpt::math::TPose2D operator+(const mrpt::math::TPose2D &b) const
Operator "oplus" pose composition: "ret=this \oplus b".
mrpt::math::TPoint2D inverseComposePoint(const TPoint2D g) const
constexpr TPose3D()=default
Default fast constructor.
constexpr TPose2D()=default
Default fast constructor.
static void getLines(const std::vector< TObject3D > &objs, std::vector< TLine3D > &lins)
Static method to retrieve every line included in a vector of objects.
void unitarize()
Unitarize normal vector.
mrpt::math::TPose2D operator*(const double dt) const
Returns the pose increment of multiplying each twist component times "dt" seconds.
GLuint GLuint end
Definition: glext.h:3532
void getAsQuaternion(mrpt::math::CQuaternion< double > &q, mrpt::math::CMatrixFixed< double, 4, 3 > *out_dq_dr=nullptr) const
Returns the quaternion associated to the rotation of this object (NOTE: XYZ translation is ignored) ...
void generate2DObject(TPolygon2D &p) const
Projects into a 2D space, discarding the z.
static void getPolygons(const std::vector< TObject3D > &objs, std::vector< TPolygon3D > &polys)
Static method to retrieve every polygon included in a vector of objects.
GLubyte g
Definition: glext.h:6372
3D Plane, represented by its equation
Definition: TPlane.h:22
bool getPlane(TPlane &p) const
Gets a plane which contains the polygon.
GLubyte GLubyte b
Definition: glext.h:6372
double getRegressionPlane(const std::vector< TPoint3D > &points, TPlane &plane)
Using eigenvalues, gets the best fitting plane for a set of 3D points.
Definition: geometry.cpp:2150
TPolygon2D()
Default constructor.
Definition: TPolygon2D.h:48
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
void removeRedundantVertices()
Erase every redundant vertex, thus saving space.
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...
TPoint2D point2
Destiny point.
Definition: TSegment2D.h:30
constexpr TPoint2D()
Default constructor.
Definition: TPoint2D.h:40
void getCenter(TPoint2D &p) const
Polygon&#39;s central point.
void rotate(const mrpt::math::TPose3D &rot)
Transform all 6 components for a change of reference frame from "A" to another frame "B" whose rotati...
double getAngle(const TPlane &p1, const TPlane &p2)
Computes the angle between two planes.
Definition: geometry.cpp:866
GLsizei const GLchar ** string
Definition: glext.h:4116
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:50
TVector3D getNormalVector() const
size_type rows() const
Number of rows in the matrix.
size_type cols() const
Number of columns in the matrix.
void unitarize()
Unitarize director vector.
static constexpr unsigned char GEOMETRIC_TYPE_UNDEFINED
Object type identifier for empty TObject2D or TObject3D.
Definition: TPoseOrPoint.h:126
mrpt::serialization::CArchive & operator>>(mrpt::serialization::CArchive &in, CMatrixD::Ptr &pObj)
bool contains(const TPoint3D &point) const
Check whether a point is inside (or within geometryEpsilon of a polygon edge).
bool getPoint(TPoint3D &p) const
Gets the content as a point, returning false if the type is not adequate.
Definition: TObject3D.h:120
double pitch
Pitch coordinate (rotation angle over Y axis).
Definition: TPose3D.h:35
bool empty() const
Definition: ts_hash_map.h:190
Lightweight 3D pose (three spatial coordinates, plus a quaternion ).
Definition: TPose3DQuat.h:19
double distance(const TPoint3D &point) const
Distance to point (always >=0)
TPoint2D point1
Origin point.
Definition: TSegment2D.h:26
bool isConvex() const
Checks whether is convex.
void getHomogeneousMatrix(mrpt::math::CMatrixDouble44 &HG) const
#define ASSERTDEBMSG_(f, __ERROR_MSG)
Definition: exceptions.h:191
void generate3DObject(TSegment3D &s) const
Project into 3D space, setting the z to 0.
bool contains(const TPoint2D &point) const
Check whether a point is inside (or within geometryEpsilon of a polygon edge).
void removeUnusedVertices(T &poly)
double distance(const TPoint2D &point) const
Distance to a point (always >=0)
constexpr TPoint3D()
Default constructor.
Definition: TPoint3D.h:100
const GLdouble * v
Definition: glext.h:3684
std::array< double, 3 > coefs
Line coefficients, stored as an array: .
Definition: TLine2D.h:23
void getAsPose2DForcingOrigin(const TPoint2D &origin, TPose2D &outPose) const
constexpr std::size_t size() const
Definition: TPoseOrPoint.h:58
const_iterator begin() const
Definition: ts_hash_map.h:239
double wx
Angular velocity (rad/s)
Definition: TTwist3D.h:28
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
double vx
Velocity components: X,Y (m/s)
Definition: TTwist3D.h:26
void generate3DObject(TObject3D &obj) const
Project into 3D space.
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
mrpt::vision::TStereoCalibResults out
static void getPoints(const std::vector< TObject2D > &objs, std::vector< TPoint2D > &pnts)
Static method to retrieve all the points in a vector of TObject2D.
void getPrismBounds(const std::vector< TPoint3D > &poly, TPoint3D &pMin, TPoint3D &pMax)
Gets the prism bounds of a 3D polygon or set of 3D points.
Definition: geometry.cpp:2021
mrpt::serialization::CArchive & operator<<(mrpt::serialization::CArchive &s, const CVectorFloat &a)
Definition: math.cpp:627
double getEpsilon()
Gets the value of the geometric epsilon (default = 1e-5)
Definition: geometry.cpp:34
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...
#define ASSERT_ABOVE_(__A, __B)
Definition: exceptions.h:155
mrpt::math::CMatrixDouble44 getInverseHomogeneousMatrix() const
Definition: TPose3D.h:181
bool operator!=(const TTwist3D &o) const
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:23
void getAsPose3D(mrpt::math::TPose3D &outPose) const
GLuint in
Definition: glext.h:7391
Lightweight 2D pose.
Definition: TPose2D.h:22
void removeRedundantVertices()
Erase every redundant vertex from the polygon, saving space.
void removeRepeatedVertices()
Remove polygon&#39;s repeated vertices.
bool contains(const TPoint3D &point) const
Check whether a point is inside the segment.
bool getLine(TLine3D &r) const
Gets the content as a line, returning false if the type is not adequate.
Definition: TObject3D.h:147
double evaluatePoint(const TPoint3D &point) const
Evaluate a point in the plane&#39;s equation.
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
Definition: CMatrixFixed.h:251
std::string asString() const
Definition: TPose2D.h:108
void generate2DObject(TSegment2D &s) const
Projection into 2D space, discarding the z.
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...
void generate2DObject(TObject2D &obj) const
Projects into 2D space.
TLine3D()=default
Fast default constructor.
bool operator<(const TPoint3D &p) const
double evaluatePoint(const TPoint2D &point) const
Evaluate point in the line&#39;s equation.
GLenum GLint GLint y
Definition: glext.h:3542
GLfloat GLfloat GLfloat v2
Definition: glext.h:4123
unsigned char getType() const
Gets object type.
Definition: TObject3D.h:115
mrpt::math::TPose2D operator-(const mrpt::math::TPose2D &b) const
Operator "ominus" pose composition: "ret=this \ominus b".
GLuint res
Definition: glext.h:7385
static constexpr unsigned char GEOMETRIC_TYPE_SEGMENT
Object type identifier for TSegment2D or TSegment3D.
Definition: TPoseOrPoint.h:106
GLenum GLint x
Definition: glext.h:3542
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...
Definition: CQuaternion.h:44
Lightweight 3D point.
Definition: TPoint3D.h:90
double distance(const TPoint2D &point) const
Distance from a given point.
void getAsSegmentList(std::vector< TSegment3D > &v) const
Gets as set of segments, instead of set of points.
void getBestFittingPlane(TPlane &p) const
Gets the best fitting plane, disregarding whether the polygon actually fits inside or not...
void getDirectorVector(double(&vector)[2]) const
Get line&#39;s director vector.
std::string asString() const
Definition: TPose3D.h:135
CMatrixFixed< double, 4, 4 > CMatrixDouble44
Definition: CMatrixFixed.h:353
Lightweight 2D point.
Definition: TPoint2D.h:31
TLine2D()=default
Fast default constructor.
TPlane()=default
Fast default constructor.
std::array< double, 4 > coefs
Plane coefficients, stored as an array: .
Definition: TPlane.h:26
GLsizei GLsizei GLenum GLenum const GLvoid * data
Definition: glext.h:3550
images resize(NUM_IMGS)
std::string asString() const
Definition: TTwist3D.h:135
GLubyte GLubyte GLubyte a
Definition: glext.h:6372
GLfloat GLfloat p
Definition: glext.h:6398
static void getPlanes(const std::vector< TObject3D > &objs, std::vector< TPlane > &plns)
Static method to retrieve every plane included in a vector of objects.
double phi
Orientation (rads)
Definition: TPose2D.h:32
bool operator<(const TPoint2D &p) const
GLuint GLuint GLsizei GLenum type
Definition: glext.h:3532
static constexpr unsigned char GEOMETRIC_TYPE_LINE
Object type identifier for TLine2D or TLine3D.
Definition: TPoseOrPoint.h:111
void operator()(const T &o)
2D polygon, inheriting from std::vector<TPoint2D>.
Definition: TPolygon2D.h:21
3D polygon, inheriting from std::vector<TPoint3D>
Definition: TPolygon3D.h:18
double distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Definition: geometry.cpp:1891
void getCenter(TPoint3D &p) const
Get polygon&#39;s central point.
double omega
Angular velocity (rad/s)
Definition: TTwist2D.h:28
bool contains(const TPoint3D &point) const
Check whether a point is contained into the plane.
double length() const
Segment length.
void composePoint(const TPoint3D &l, TPoint3D &g) const
bool operator!=(const TTwist2D &o) const
void getAsPose3DForcingOrigin(const TPoint3D &center, TPose3D &pose) const
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...
bool operator==(const TTwist2D &o) const
3D line, represented by a base point and a director vector.
Definition: TLine3D.h:19
void composePose(const TPose3D other, TPose3D &result) const
2D line without bounds, represented by its equation .
Definition: TLine2D.h:19
static void getPoints(const std::vector< TObject3D > &objs, std::vector< TPoint3D > &pnts)
Static method to retrieve every point included in a vector of objects.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: dad381fd7 Sun Oct 20 13:36:46 2019 +0200 at dom oct 20 13:40:10 CEST 2019