Main MRPT website > C++ reference for MRPT 1.5.6
lightweight_geom_data.cpp
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 
10 #include "base-precomp.h" // Precompiled headers
11 
13 #include <mrpt/poses/CPoint2D.h>
14 #include <mrpt/poses/CPoint3D.h>
15 #include <mrpt/poses/CPose2D.h>
16 #include <mrpt/poses/CPose3DQuat.h>
17 #include <mrpt/math/CQuaternion.h>
18 #include <mrpt/math/geometry.h>
20 #include <mrpt/utils/CStream.h>
22 
23 using namespace std; // For min/max, etc...
24 
25 namespace mrpt { namespace math {
26  using namespace mrpt::poses; // For the +,- operators
27  using namespace mrpt::utils;
28 
29 
30  namespace detail
31  {
32  // Convert a pose into a light-weight structure (functional form, needed for forward declarations)
38 
39  } // end detail
40 
41 
42 TPoint2D::TPoint2D(const TPose2D &p):x(p.x),y(p.y) {}
43 TPoint2D::TPoint2D(const TPoint3D &p):x(p.x),y(p.y) {}
44 TPoint2D::TPoint2D(const TPose3D &p):x(p.x),y(p.y) {}
46 bool TPoint2D::operator<(const TPoint2D &p) const {
47  if (x<p.x) return true;
48  else if (x>p.x) return false;
49  else return y<p.y;
50 }
52 {
54  if (!m.fromMatlabStringFormat(s)) THROW_EXCEPTION("Malformed expression in ::fromString");
55  ASSERTMSG_(mrpt::math::size(m,1)==1 && mrpt::math::size(m,2)==2, "Wrong size of vector in ::fromString");
56  x = m.get_unsafe(0,0);
57  y = m.get_unsafe(0,1);
58 }
59 
60 TPose2D::TPose2D(const TPoint2D &p):x(p.x),y(p.y),phi(0.0) {}
61 TPose2D::TPose2D(const TPoint3D &p):x(p.x),y(p.y),phi(0.0) {}
62 TPose2D::TPose2D(const TPose3D &p):x(p.x),y(p.y),phi(p.yaw) {}
63 TPose2D::TPose2D(const mrpt::poses::CPose2D &p):x(p.x()),y(p.y()),phi(p.phi()) {}
65  s = mrpt::format("[%f %f %f]",x,y,mrpt::utils::RAD2DEG(phi));
66 }
68 {
70  if (!m.fromMatlabStringFormat(s)) THROW_EXCEPTION("Malformed expression in ::fromString");
71  ASSERTMSG_(mrpt::math::size(m,1)==1 && mrpt::math::size(m,2)==3, "Wrong size of vector in ::fromString");
72  x = m.get_unsafe(0,0);
73  y = m.get_unsafe(0,1);
74  phi = DEG2RAD(m.get_unsafe(0,2));
75 }
77 {
78  const double A_cosphi = cos(this->phi), A_sinphi = sin(this->phi);
79  // Use temporary variables for the cases (A==this) or (B==this)
80  const double new_x = this->x + b.x * A_cosphi - b.y * A_sinphi;
81  const double new_y = this->y + b.x * A_cosphi + b.y * A_sinphi;
82  const double new_phi = mrpt::math::wrapToPi(this->phi + b.phi);
83 
84  return mrpt::math::TPose2D(new_x, new_y, new_phi);
85 }
86 
88 {
89  const double B_cosphi = cos(b.phi), B_sinphi = sin(b.phi);
90 
91  const double new_x = (this->x - b.x) * B_cosphi + (this->y - b.y) * B_sinphi;
92  const double new_y = -(this->x - b.x) * B_sinphi + (this->y - b.y) * B_cosphi;
93  const double new_phi = mrpt::math::wrapToPi(this->phi - b.phi);
94 
95  return mrpt::math::TPose2D(new_x, new_y, new_phi);
96 }
97 
98 // ----
100  s = mrpt::format("[%f %f %f]",vx,vy,mrpt::utils::RAD2DEG(omega));
101 }
103 {
105  if (!m.fromMatlabStringFormat(s)) THROW_EXCEPTION("Malformed expression in ::fromString");
106  ASSERTMSG_(mrpt::math::size(m,1)==1 && mrpt::math::size(m,2)==3, "Wrong size of vector in ::fromString");
107  vx = m.get_unsafe(0,0);
108  vy = m.get_unsafe(0,1);
109  omega = DEG2RAD(m.get_unsafe(0,2));
110 }
111 // Transform the (vx,vy) components for a counterclockwise rotation of `ang` radians
112 void TTwist2D::rotate(const double ang)
113 {
114  const double nvx = vx * cos(ang) - vy * sin(ang);
115  const double nvy = vx * sin(ang) + vy * cos(ang);
116  vx = nvx;
117  vy = nvy;
118 }
119 bool TTwist2D::operator ==(const TTwist2D&o) const
120 {
121  return vx == o.vx && vy == o.vy && omega == o.omega;
122 }
123 bool TTwist2D::operator !=(const TTwist2D&o) const {
124  return !(*this==o);
125 }
127 {
128  return mrpt::math::TPose2D(vx*dt,vy*dt,omega*dt);
129 }
130 
131 // ----
134 }
136 {
138  if (!m.fromMatlabStringFormat(s)) THROW_EXCEPTION("Malformed expression in ::fromString");
139  ASSERTMSG_(mrpt::math::size(m,1)==1 && mrpt::math::size(m,2)==6, "Wrong size of vector in ::fromString");
140  for (int i=0;i<3;i++) (*this)[i] = m.get_unsafe(0,i);
141  for (int i=0;i<3;i++) (*this)[3+i] = DEG2RAD(m.get_unsafe(0,3+i));
142 }
143 // Transform all 6 components for a change of reference frame from "A" to
144 // another frame "B" whose rotation with respect to "A" is given by `rot`. The translational part of the pose is ignored
146 {
147  const TTwist3D t = *this;
149  vx=R(0,0)*t.vx+R(0,1)*t.vy+R(0,2)*t.vz;
150  vy=R(1,0)*t.vx+R(1,1)*t.vy+R(1,2)*t.vz;
151  vz=R(2,0)*t.vx+R(2,1)*t.vy+R(2,2)*t.vz;
152 
153  wx=R(0,0)*t.wx+R(0,1)*t.wy+R(0,2)*t.wz;
154  wy=R(1,0)*t.wx+R(1,1)*t.wy+R(1,2)*t.wz;
155  wz=R(2,0)*t.wx+R(2,1)*t.wy+R(2,2)*t.wz;
156 }
157 bool TTwist3D::operator ==(const TTwist3D&o) const
158 {
159  return vx == o.vx && vy == o.vy && vz == o.vz && wx == o.wx && wy == o.wy && wz == o.wz;
160 }
161 bool TTwist3D::operator !=(const TTwist3D&o) const {
162  return !(*this == o);
163 }
164 
165 TPoint3D::TPoint3D(const TPoint2D &p):x(p.x),y(p.y),z(0.0) {}
166 TPoint3D::TPoint3D(const TPose2D &p):x(p.x),y(p.y),z(0.0) {}
167 TPoint3D::TPoint3D(const TPose3D &p):x(p.x),y(p.y),z(p.z) {}
168 TPoint3D::TPoint3D(const mrpt::poses::CPoint3D &p):x(p.x()),y(p.y()),z(p.z()) {}
169 TPoint3D::TPoint3D(const mrpt::poses::CPose3D &p):x(p.x()),y(p.y()),z(p.z()) {}
170 bool TPoint3D::operator<(const TPoint3D &p) const {
171  if (x<p.x) return true;
172  else if (x>p.x) return false;
173  else if (y<p.y) return true;
174  else if (y>p.y) return false;
175  else return z<p.z;
176 }
178 {
180  if (!m.fromMatlabStringFormat(s)) THROW_EXCEPTION("Malformed expression in ::fromString");
181  ASSERTMSG_(mrpt::math::size(m,1)==1 && mrpt::math::size(m,2)==3, "Wrong size of vector in ::fromString");
182  x = m.get_unsafe(0,0);
183  y = m.get_unsafe(0,1);
184  z = m.get_unsafe(0,2);
185 }
186 
187 TPose3D::TPose3D(const TPoint2D &p):x(p.x),y(p.y),z(0.0),yaw(0.0),pitch(0.0),roll(0.0) {}
188 TPose3D::TPose3D(const TPose2D &p):x(p.x),y(p.y),z(0.0),yaw(p.phi),pitch(0.0),roll(0.0) {}
189 TPose3D::TPose3D(const TPoint3D &p):x(p.x),y(p.y),z(p.z),yaw(0.0),pitch(0.0),roll(0.0) {}
190 TPose3D::TPose3D(const mrpt::poses::CPose3D &p):x(p.x()),y(p.y()),z(p.z()),yaw(p.yaw()),pitch(p.pitch()),roll(p.roll()) {}
192  s = mrpt::format("[%f %f %f %f %f %f]",x,y,z,RAD2DEG(yaw),RAD2DEG(pitch),RAD2DEG(roll));
193 }
195 {
196  // See: http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
197  const double cy = cos(yaw*0.5), sy = sin(yaw*0.5);
198  const double cp = cos(pitch*0.5),sp = sin(pitch*0.5);
199  const double cr = cos(roll*0.5), sr = sin(roll*0.5);
200 
201  const double ccc = cr*cp*cy;
202  const double ccs = cr*cp*sy;
203  const double css = cr*sp*sy;
204  const double sss = sr*sp*sy;
205  const double scc = sr*cp*cy;
206  const double ssc = sr*sp*cy;
207  const double csc = cr*sp*cy;
208  const double scs = sr*cp*sy;
209 
210  q[0] = ccc + sss;
211  q[1] = scc - css;
212  q[2] = csc + scs;
213  q[3] = ccs - ssc;
214 
215  // Compute 4x3 Jacobian: for details, see technical report:
216  // Parameterizations of SE(3) transformations: equivalences, compositions and uncertainty, J.L. Blanco (2010).
217  // http://www.mrpt.org/6D_poses:equivalences_compositions_and_uncertainty
218  if (out_dq_dr)
219  {
220  MRPT_ALIGN16 const double nums[4 * 3] = {
221  -0.5*q[3], 0.5*(-csc + scs), -0.5*q[1],
222  -0.5*q[2], 0.5*(-ssc - ccs), 0.5* q[0],
223  0.5*q[1], 0.5*(ccc - sss), 0.5*q[3],
224  0.5* q[0], 0.5*(-css - scc), -0.5*q[2]
225  };
226  out_dq_dr->loadFromArray(nums);
227  }
228 }
230 {
232  if (!m.fromMatlabStringFormat(s)) THROW_EXCEPTION("Malformed expression in ::fromString");
233  ASSERTMSG_(mrpt::math::size(m,1)==1 && mrpt::math::size(m,2)==6, "Wrong size of vector in ::fromString");
234  x=m.get_unsafe(0,0);
235  y=m.get_unsafe(0,1);
236  z=m.get_unsafe(0,2);
237  yaw=DEG2RAD(m.get_unsafe(0,3));
238  pitch=DEG2RAD(m.get_unsafe(0,4));
239  roll=DEG2RAD(m.get_unsafe(0,5));
240 }
241 
242 TPose3DQuat::TPose3DQuat(const mrpt::poses::CPose3DQuat &p):x(p.x()),y(p.y()),z(p.z()),qr(p.quat().r()),qx(p.quat().x()),qy(p.quat().y()),qz(p.quat().z())
243 {
244 }
246 {
248  if (!m.fromMatlabStringFormat(s)) THROW_EXCEPTION("Malformed expression in ::fromString");
249  ASSERTMSG_(mrpt::math::size(m,1)==1 && mrpt::math::size(m,2)==7, "Wrong size of vector in ::fromString");
250  for (size_t i=0;i<m.getColCount();i++)
251  (*this)[i] = m.get_unsafe(0,i);
252 }
253 
254 
255 // Text streaming:
256 std::ostream & operator << (std::ostream& o, const TPoint2D & p) { return (o << CPoint2D(p)); }
257 std::ostream & operator << (std::ostream& o, const TPoint3D & p) { return (o << CPoint3D(p)); }
258 std::ostream & operator << (std::ostream& o, const TPose2D & p) { return (o << CPose2D(p)); }
259 std::ostream & operator << (std::ostream& o, const TPose3D & p) { return (o << CPose3D(p)); }
260 std::ostream & operator << (std::ostream& o, const TPose3DQuat & p) { return (o << CPose3DQuat(p)); }
261 
265 CStream &operator<<(mrpt::utils::CStream &out,const mrpt::math::TLine2D &l) { return out<<l.coefs[0]<<l.coefs[1]<<l.coefs[2]; }
266 
272 mrpt::utils::CStream &operator<<(mrpt::utils::CStream &out,const mrpt::math::TPlane &p) { return out<<p.coefs[0]<<p.coefs[1]<<p.coefs[2]<<p.coefs[3]; }
273 
274 double TSegment2D::length() const {
275  return math::distance(point1,point2);
276 }
277 double TSegment2D::distance(const TPoint2D &point) const {
278  return std::abs(signedDistance(point));
279 }
280 double TSegment2D::signedDistance(const TPoint2D &point) const {
281  //It is reckoned whether the perpendicular line to the TSegment2D which passes through point crosses or not the referred segment,
282  //or what is the same, whether point makes an obtuse triangle with the segment or not (being the longest segment one between the point and either end of TSegment2D).
283  const double d1 = math::distance(point,point1);
284  if (point1 == point2)
285  return d1;
286 
287  const double d2 = math::distance(point,point2);
288  const double d3 = length();
289  const double ds1 = square(d1);
290  const double ds2 = square(d2);
291  const double ds3 = square(d3);
292  if ( ds1 > (ds2 + ds3) || ds2 > (ds1 + ds3) )
293  // Fix sign:
294  return min(d1,d2) * ( TLine2D(*this).signedDistance(point)<0 ? -1:1);
295  else return TLine2D(*this).signedDistance(point);
296 }
297 bool TSegment2D::contains(const TPoint2D &point) const {
299 }
301  s=TSegment3D(*this);
302 }
306  if (point1==point2) throw std::logic_error("Segment is normal to projection plane");
307 }
308 
309 bool TSegment2D::operator<(const TSegment2D &s) const {
310  if (point1<s.point1) return true;
311  else if (s.point1<point1) return false;
312  else return point2<s.point2;
313 }
314 
315 double TSegment3D::length() const {
316  return math::distance(point1,point2);
317 }
318 double TSegment3D::distance(const TPoint3D &point) const {
319  return min(min(math::distance(point,point1),math::distance(point,point2)),TLine3D(*this).distance(point));
320 }
321 double TSegment3D::distance(const TSegment3D &segment) const {
322  Eigen::Vector3d u, v, w;
323  TPoint3D diff_vect = point2 - point1;
324  diff_vect.getAsVector(u);
325  diff_vect = segment.point2 - segment.point1;
326  diff_vect.getAsVector(v);
327  diff_vect = point1 - segment.point1;
328  diff_vect.getAsVector(w);
329  double a = u.dot(u); // always >= 0
330  double b = u.dot(v);
331  double c = v.dot(v); // always >= 0
332  double d = u.dot(w);
333  double e = v.dot(w);
334  double D = a*c - b*b; // always >= 0
335  double sc, sN, sD = D; // sc = sN / sD, default sD = D >= 0
336  double tc, tN, tD = D; // tc = tN / tD, default tD = D >= 0
337 
338  // compute the line parameters of the two closest points
339  if (D < 0.00000001) { // the lines are almost parallel
340  sN = 0.0; // force using point P0 on segment S1
341  sD = 1.0; // to prevent possible division by 0.0 later
342  tN = e;
343  tD = c;
344  }
345  else { // get the closest points on the infinite lines
346  sN = (b*e - c*d);
347  tN = (a*e - b*d);
348  if (sN < 0.0) { // sc < 0 => the s=0 edge is visible
349  sN = 0.0;
350  tN = e;
351  tD = c;
352  }
353  else if (sN > sD) { // sc > 1 => the s=1 edge is visible
354  sN = sD;
355  tN = e + b;
356  tD = c;
357  }
358  }
359 
360  if (tN < 0.0) { // tc < 0 => the t=0 edge is visible
361  tN = 0.0;
362  // recompute sc for this edge
363  if (-d < 0.0)
364  sN = 0.0;
365  else if (-d > a)
366  sN = sD;
367  else {
368  sN = -d;
369  sD = a;
370  }
371  }
372  else if (tN > tD) { // tc > 1 => the t=1 edge is visible
373  tN = tD;
374  // recompute sc for this edge
375  if ((-d + b) < 0.0)
376  sN = 0;
377  else if ((-d + b) > a)
378  sN = sD;
379  else {
380  sN = (-d + b);
381  sD = a;
382  }
383  }
384  // finally do the division to get sc and tc
385  sc = (fabs(sN) < 0.00000001 ? 0.0 : sN / sD);
386  tc = (fabs(tN) < 0.00000001 ? 0.0 : tN / tD);
387 
388  // get the difference of the two closest points
389  CVectorDouble dP = w + (sc * u) - (tc * v); // = S1(sc) - S2(tc)
390 
391  return dP.norm(); // return the closest distance
392 }
393 bool TSegment3D::contains(const TPoint3D &point) const {
394  //Not very intuitive, but very fast, method.
396 }
397 
398 bool TSegment3D::operator<(const TSegment3D &s) const {
399  if (point1<s.point1) return true;
400  else if (s.point1<point1) return false;
401  else return point2<s.point2;
402 }
403 
404 double TLine2D::evaluatePoint(const TPoint2D &point) const {
405  return coefs[0]*point.x+coefs[1]*point.y+coefs[2];
406 }
407 bool TLine2D::contains(const TPoint2D &point) const {
408  return abs(distance(point))<geometryEpsilon;
409 }
410 double TLine2D::distance(const TPoint2D &point) const {
411  return abs(evaluatePoint(point))/sqrt(coefs[0]*coefs[0]+coefs[1]*coefs[1]);
412 }
413 double TLine2D::signedDistance(const TPoint2D &point) const {
414  return evaluatePoint(point)/sqrt(coefs[0]*coefs[0]+coefs[1]*coefs[1]);
415 }
416 void TLine2D::getNormalVector(double (&vector)[2]) const {
417  vector[0]=coefs[0];
418  vector[1]=coefs[1];
419 }
421  double s=sqrt(coefs[0]*coefs[0]+coefs[1]*coefs[1]);
422  for (size_t i=0;i<3;i++) coefs[i]/=s;
423 }
424 void TLine2D::getDirectorVector(double (&vector)[2]) const {
425  vector[0]=-coefs[1];
426  vector[1]=coefs[0];
427 }
429  l=TLine3D(*this);
430 }
432  //Line's director vector is (-coefs[1],coefs[0]).
433  //If line is horizontal, force x=0. Else, force y=0. In both cases, we'll find a suitable point.
434  outPose.phi( atan2(coefs[0],-coefs[1]) );
435  if (abs(coefs[0])<geometryEpsilon) {
436  outPose.x(0);
437  outPose.y(-coefs[2]/coefs[1]);
438  } else {
439  outPose.x(-coefs[2]/coefs[0]);
440  outPose.y(0);
441  }
442 }
444  if (!contains(origin)) throw std::logic_error("Base point is not contained in the line");
445  outPose=mrpt::poses::CPose2D(TPose2D(origin));
446  //Line's director vector is (-coefs[1],coefs[0]).
447  outPose.phi( atan2(coefs[0],-coefs[1]) );
448 }
449 TLine2D::TLine2D(const TPoint2D &p1,const TPoint2D &p2) {
450  if (p1==p2) throw logic_error("Both points are the same");
451  coefs[0]=p2.y-p1.y;
452  coefs[1]=p1.x-p2.x;
453  coefs[2]=p2.x*p1.y-p2.y*p1.x;
454 }
456  coefs[0]=s.point2.y-s.point1.y;
457  coefs[1]=s.point1.x-s.point2.x;
458  coefs[2]=s.point2.x*s.point1.y-s.point2.y*s.point1.x;
459  //unitarize(); //¿?
460 }
462  //Line's projection to Z plane may be a point.
463  if (hypot(l.director[0],l.director[1])<geometryEpsilon) throw std::logic_error("Line is normal to projection plane");
464  coefs[0]=-l.director[1];
465  coefs[1]=l.director[0];
466  coefs[2]=l.pBase.x*l.director[1]-l.pBase.y*l.director[0];
467 }
468 
469 bool TLine3D::contains(const TPoint3D &point) const {
470  double dx=point.x-pBase.x;
471  double dy=point.y-pBase.y;
472  double dz=point.z-pBase.z;
473  if (abs(dx)<geometryEpsilon&&abs(dy)<geometryEpsilon&&abs(dz)<geometryEpsilon) return true;
474  // dx dy dz
475  //if -----------=-----------=-----------, point is inside the line.
476  // director[0] director[1] director[2]
477  return (abs(dx*director[1]-dy*director[0])<geometryEpsilon)&&(abs(dx*director[2]-dz*director[0])<geometryEpsilon)&&(abs(dy*director[2]-dz*director[1])<geometryEpsilon);
478 }
479 double TLine3D::distance(const TPoint3D &point) const {
480  //Let d be line's base point minus the argument. Then, d·director/(|d|·|director|) equals both vector's cosine.
481  //So, d·director/|director| equals d's projection over director. Then, distance is sqrt(|d|²-(d·director/|director|)²).
482  double d[3]={point.x-pBase.x,point.y-pBase.y,point.z-pBase.z};
483  double dv=0,d2=0,v2=0;
484  for (size_t i=0;i<3;i++) {
485  dv+=d[i]*director[i];
486  d2+=d[i]*d[i];
487  v2+=director[i]*director[i];
488  }
489  return sqrt(d2-(dv*dv)/v2);
490 }
492  double s=sqrt(squareNorm<3,double>(director));
493  for (size_t i=0;i<3;i++) director[i]/=s;
494 }
495 TLine3D::TLine3D(const TPoint3D &p1,const TPoint3D &p2){
496  if (abs(math::distance(p1,p2))<geometryEpsilon) throw logic_error("Both points are the same");
497  pBase=p1;
498  director[0]=p2.x-p1.x;
499  director[1]=p2.y-p1.y;
500  director[2]=p2.z-p1.z;
501 }
503  pBase=s.point1;
504  director[0]=s.point2.x-s.point1.x;
505  director[1]=s.point2.y-s.point1.y;
506  director[2]=s.point2.z-s.point1.z;
507 }
509  director[0]=-l.coefs[1];
510  director[1]=l.coefs[0];
511  director[2]=0;
512  //We assume that either l.coefs[0] or l.coefs[1] is not null. Respectively, either y or x can be used as free cordinate.
513  if (abs(l.coefs[0])>=geometryEpsilon) {
514  pBase.x=-l.coefs[2]/l.coefs[0];
515  pBase.y=0;
516  } else {
517  pBase.x=0;
518  pBase.y=-l.coefs[1]/l.coefs[0];
519  }
520  pBase.z=0;
521 }
522 
523 double TPlane::evaluatePoint(const TPoint3D &point) const {
524  return dotProduct<3,double>(coefs,point)+coefs[3];
525 }
526 bool TPlane::contains(const TPoint3D &point) const {
527  return distance(point)<geometryEpsilon;
528 }
529 bool TPlane::contains(const TLine3D &line) const {
530  if (!contains(line.pBase)) return false; //Base point must be contained
531  return abs(getAngle(*this,line))<geometryEpsilon; //Plane's normal must be normal to director vector
532 }
533 double TPlane::distance(const TPoint3D &point) const {
534  return abs(evaluatePoint(point))/sqrt(squareNorm<3,double>(coefs));
535 }
536 double TPlane::distance(const TLine3D &line) const {
537  if (abs(getAngle(*this,line))>=geometryEpsilon) return 0; //Plane crosses with line
538  else return distance(line.pBase);
539 }
540 void TPlane::getNormalVector(double (&vector)[3]) const {
541  vector[0]=coefs[0];
542  vector[1]=coefs[1];
543  vector[2]=coefs[2];
544 }
546  double s=sqrt(squareNorm<3,double>(coefs));
547  for (size_t i=0;i<4;i++) coefs[i]/=s;
548 }
549 
550 // Returns a 6D pose such as its XY plane coincides with the plane
552  double normal[3];
553  getUnitaryNormalVector(normal);
554  CMatrixDouble AXIS;
556  for (size_t i=0;i<3;i++) if (abs(coefs[i])>=geometryEpsilon) {
557  AXIS.set_unsafe(i,3,-coefs[3]/coefs[i]);
558  break;
559  }
560  outPose=mrpt::poses::CPose3D(AXIS);
561 }
563  if (!contains(newOrigin)) throw std::logic_error("Base point is not in the plane.");
564  double normal[3];
565  getUnitaryNormalVector(normal);
566  CMatrixDouble AXIS;
568  for (size_t i=0;i<3;i++) AXIS.set_unsafe(i,3,newOrigin[i]);
569  pose=mrpt::poses::CPose3D(AXIS);
570 }
571 TPlane::TPlane(const TPoint3D &p1,const TPoint3D &p2,const TPoint3D &p3) {
572  double dx1=p2.x-p1.x;
573  double dy1=p2.y-p1.y;
574  double dz1=p2.z-p1.z;
575  double dx2=p3.x-p1.x;
576  double dy2=p3.y-p1.y;
577  double dz2=p3.z-p1.z;
578  coefs[0]=dy1*dz2-dy2*dz1;
579  coefs[1]=dz1*dx2-dz2*dx1;
580  coefs[2]=dx1*dy2-dx2*dy1;
581  if (abs(coefs[0])<geometryEpsilon&&abs(coefs[1])<geometryEpsilon&&abs(coefs[2])<geometryEpsilon) throw logic_error("Points are linearly dependent");
582  coefs[3]=-coefs[0]*p1.x-coefs[1]*p1.y-coefs[2]*p1.z;
583 }
584 TPlane::TPlane(const TPoint3D &p1,const TLine3D &r2) {
585  double dx1=p1.x-r2.pBase.x;
586  double dy1=p1.y-r2.pBase.y;
587  double dz1=p1.z-r2.pBase.z;
588  coefs[0]=dy1*r2.director[2]-dz1*r2.director[1];
589  coefs[1]=dz1*r2.director[0]-dx1*r2.director[2];
590  coefs[2]=dx1*r2.director[1]-dy1*r2.director[0];
591  if (abs(coefs[0])<geometryEpsilon&&abs(coefs[1])<geometryEpsilon&&abs(coefs[2])<geometryEpsilon) throw logic_error("Point is contained in the line");
592  coefs[3]=-coefs[0]*p1.x-coefs[1]*p1.y-coefs[2]*p1.z;
593 }
594 TPlane::TPlane(const TLine3D &r1,const TLine3D &r2) {
596  coefs[3]=-coefs[0]*r1.pBase.x-coefs[1]*r1.pBase.y-coefs[2]*r1.pBase.z;
597  if (abs(coefs[0])<geometryEpsilon&&abs(coefs[1])<geometryEpsilon&&abs(coefs[2])<geometryEpsilon) {
598  //Lines are parallel
599  if (r1.contains(r2.pBase)) throw std::logic_error("Lines are the same");
600  //Use a line's director vector and both pBase's difference to create the plane.
601  double d[3];
602  for (size_t i=0;i<3;i++) d[i]=r1.pBase[i]-r2.pBase[i];
604  coefs[3]=-coefs[0]*r1.pBase.x-coefs[1]*r1.pBase.y-coefs[2]*r1.pBase.z;
605  } else if (abs(evaluatePoint(r2.pBase))>=geometryEpsilon) throw logic_error("Lines do not intersect");
606 }
607 
608 template<class T>
609 inline void removeUnusedVertices(T &poly) {
610  size_t N=poly.size();
611  if (N<3) return;
612  std::vector<size_t> unused;
613  if (abs(mrpt::math::distance(poly[N-1],poly[0])+mrpt::math::distance(poly[0],poly[1])-mrpt::math::distance(poly[N-1],poly[1]))<mrpt::math::geometryEpsilon) unused.push_back(0);
614  for (size_t i=1;i<N-1;i++) if (abs(mrpt::math::distance(poly[i-1],poly[i])+mrpt::math::distance(poly[i],poly[i+1])-mrpt::math::distance(poly[i-1],poly[i+1]))<mrpt::math::geometryEpsilon) unused.push_back(i);
615  if (abs(mrpt::math::distance(poly[N-2],poly[N-1])+mrpt::math::distance(poly[N-1],poly[0])-mrpt::math::distance(poly[N-2],poly[0]))<mrpt::math::geometryEpsilon) unused.push_back(N-1);
616  unused.push_back(N);
617  size_t diff=1;
618  for (size_t i=0;i<unused.size()-1;i++) {
619  size_t last=unused[i+1];
620  for (size_t j=unused[i]+1-diff;j<last-diff;j++) poly[j]=poly[j+diff];
621  }
622  poly.resize(N+1-unused.size());
623 }
624 template<class T>
625 inline void removeRepVertices(T &poly) {
626  size_t N=poly.size();
627  if (N<3) return;
628  std::vector<size_t> rep;
629  for (size_t i=0;i<N-1;i++) if (mrpt::math::distance(poly[i],poly[i+1])<geometryEpsilon) rep.push_back(i);
630  if (mrpt::math::distance(poly[N-1],poly[0])<geometryEpsilon) rep.push_back(N-1);
631  rep.push_back(N);
632  size_t diff=1;
633  for (size_t i=0;i<rep.size()-1;i++) {
634  size_t last=rep[i+1];
635  for (size_t j=rep[i]+1-diff;j<last-diff;j++) poly[j]=poly[j+diff];
636  }
637  poly.resize(N+1-rep.size());
638 }
639 
640 double TPolygon2D::distance(const TPoint2D &point) const {
641  if ( contains(point) )
642  return 0;
643  std::vector<TSegment2D> sgs;
644  getAsSegmentList(sgs);
645 
646  if (sgs.empty())
647  THROW_EXCEPTION("Cannot compute distance to an empty polygon.")
648 
649  double distance = std::numeric_limits<double>::max();
650 
651  for (vector<TSegment2D>::const_iterator it=sgs.begin();it!=sgs.end();++it)
652  {
653  double d = (*it).distance(point);
654  if( d < distance )
655  distance = d;
656  }
657  return distance;
658 }
659 
660 void TPolygon2D::getBoundingBox(TPoint2D &min_coords, TPoint2D&max_coords) const
661 {
662  ASSERTMSG_(!this->empty(),"getBoundingBox() called on an empty polygon!");
663  min_coords.x = min_coords.y = std::numeric_limits<double>::max();
664  max_coords.x = max_coords.y = -std::numeric_limits<double>::max();
665  for (size_t i=0;i<size();i++)
666  {
667  mrpt::utils::keep_min( min_coords.x, (*this)[i].x);
668  mrpt::utils::keep_min( min_coords.y, (*this)[i].y);
669  mrpt::utils::keep_max( max_coords.x, (*this)[i].x);
670  mrpt::utils::keep_max( max_coords.y, (*this)[i].y);
671  }
672 }
673 
674 // isLeft(): tests if a point is Left|On|Right of an infinite line.
675 // Input: three points P0, P1, and P2
676 // Return: >0 for P2 left of the line through P0 and P1
677 // =0 for P2 on the line
678 // <0 for P2 right of the line
679 // See: Algorithm 1 "Area of Triangles and Polygons"
680 inline double isLeft( const mrpt::math::TPoint2D &P0, const mrpt::math::TPoint2D &P1, const mrpt::math::TPoint2D &P2 )
681 {
682  return ( (P1.x - P0.x) * (P2.y - P0.y) - (P2.x - P0.x) * (P1.y - P0.y) );
683 }
684 
685 bool TPolygon2D::contains(const TPoint2D &P) const
686 {
687  int wn = 0; // the winding number counter
688 
689  // loop through all edges of the polygon
690  const size_t n = this->size();
691  for (size_t i=0; i<n; i++) // edge from V[i] to V[i+1]
692  {
693  if ((*this)[i].y <= P.y)
694  {
695  // start y <= P.y
696  if ((*this)[(i+1) % n].y > P.y) // an upward crossing
697  if (isLeft( (*this)[i], (*this)[(i+1)%n], P) > 0) // P left of edge
698  ++wn; // have a valid up intersect
699  }
700  else
701  {
702  // start y > P.y (no test needed)
703  if ((*this)[(i+1)%n].y <= P.y) // a downward crossing
704  if (isLeft( (*this)[i], (*this)[(i+1)%n], P) < 0) // P right of edge
705  --wn; // have a valid down intersect
706  }
707  }
708 
709  return wn!=0;
710 
711 }
712 void TPolygon2D::getAsSegmentList(vector<TSegment2D> &v) const {
713  size_t N=size();
714  v.resize(N);
715  for (size_t i=0;i<N-1;i++) v[i]=TSegment2D(operator[](i),operator[](i+1));
716  v[N-1]=TSegment2D(operator[](N-1),operator[](0));
717 }
718 
720  p=TPolygon3D(*this);
721 }
722 //Auxiliar functor class to compute polygon's center
723 template<class T,int N>
724 class FAddPoint {
725 public:
726  T &object;
727  FAddPoint(T &o):object(o) {
728  for (size_t i=0;i<N;i++) object[i]=0.0;
729  }
730  void operator()(const T &o) {
731  for (size_t i=0;i<N;i++) object[i]+=o[i];
732  }
733 };
735  for_each(begin(),end(),FAddPoint<TPoint2D,2>(p));
736  size_t N=size();
737  p.x/=N;
738  p.y/=N;
739 }
740 bool TPolygon2D::isConvex() const {
741  size_t N=size();
742  if (N<=3) return true;
743  vector<TSegment2D> sgms;
744  getAsSegmentList(sgms);
745  for (size_t i=0;i<N;i++) {
746  char s=0;
747  TLine2D l=TLine2D(sgms[i]);
748  for (size_t j=0;j<N;j++) {
749  double d=l.evaluatePoint(operator[](j));
750  if (abs(d)<geometryEpsilon) continue;
751  else if (!s) s=(d>0)?1:-1;
752  else if (s!=((d>0)?1:-1)) return false;
753  }
754  }
755  return true;
756 }
758  removeRepVertices(*this);
759 }
762  removeUnusedVertices(*this);
763 }
764 void TPolygon2D::getPlotData(std::vector<double> &x,std::vector<double> &y) const {
765  size_t N=size();
766  x.resize(N+1);
767  y.resize(N+1);
768  for (size_t i=0;i<N;i++) {
769  x[i]=operator[](i).x;
770  y[i]=operator[](i).y;
771  }
772  x[N]=operator[](0).x;
773  y[N]=operator[](0).y;
774 }
775 TPolygon2D::TPolygon2D(const TPolygon3D &p):std::vector<TPoint2D>() {
776  size_t N=p.size();
777  resize(N);
778  for (size_t i=0;i<N;i++) operator[](i)=TPoint2D(p[i]);
779 }
780 void TPolygon2D::createRegularPolygon(size_t numEdges,double radius,TPolygon2D &poly) {
781  if (numEdges<3||abs(radius)<geometryEpsilon) throw std::logic_error("Invalid arguments for regular polygon creations");
782  poly.resize(numEdges);
783  for (size_t i=0;i<numEdges;i++) {
784  double angle=i*M_PI*2/numEdges;
785  poly[i]=TPoint2D(radius*cos(angle),radius*sin(angle));
786  }
787 }
788 inline void TPolygon2D::createRegularPolygon(size_t numEdges,double radius,TPolygon2D &poly,const mrpt::poses::CPose2D &pose) {
789  createRegularPolygon(numEdges,radius,poly);
790  for (size_t i=0;i<numEdges;i++) poly[i]=pose+poly[i];
791 }
792 
793 double TPolygon3D::distance(const TPoint3D &point) const {
794  TPlane pl;
795  if (!getPlane(pl)) throw std::logic_error("Polygon does not conform a plane");
796  TPoint3D newPoint;
797  TPolygon3D newPoly;
799  pl.getAsPose3DForcingOrigin(operator[](0),pose);
800  project3D(point,pose,newPoint);
801  project3D(*this,pose,newPoly);
802  double distance2D=TPolygon2D(newPoly).distance(TPoint2D(newPoint));
803  return sqrt(newPoint.z*newPoint.z+distance2D*distance2D);
804 }
805 bool TPolygon3D::contains(const TPoint3D &point) const {
806  TPoint3D pMin,pMax;
807  getPrismBounds(*this,pMin,pMax);
808  if (point.x+geometryEpsilon<pMin.x||point.y+geometryEpsilon<pMin.y||point.z+geometryEpsilon<pMin.z||point.x>pMax.x+geometryEpsilon||point.y>pMax.y+geometryEpsilon||point.z>pMax.z+geometryEpsilon) return false;
809  TPlane plane;
810  if (!getPlane(plane)) throw std::logic_error("Polygon does not conform a plane");
811  TPolygon3D projectedPoly;
812  TPoint3D projectedPoint;
814  //plane.getAsPose3DForcingOrigin(operator[](0),pose);
815  plane.getAsPose3D(pose);
816  pose=mrpt::poses::CPose3D(0,0,0,0,0,0)-pose;
817  project3D(point,pose,projectedPoint);
818  if (abs(projectedPoint.z)>=geometryEpsilon) return false; //Point is not inside the polygon's plane.
819  project3D(*this,pose,projectedPoly);
820  return TPolygon2D(projectedPoly).contains(TPoint2D(projectedPoint));
821 }
822 void TPolygon3D::getAsSegmentList(vector<TSegment3D> &v) const {
823  size_t N=size();
824  v.resize(N);
825  for (size_t i=0;i<N-1;i++) v[i]=TSegment3D(operator[](i),operator[](i+1));
826  v[N-1]=TSegment3D(operator[](N-1),operator[](0));
827 }
829  return conformAPlane(*this,p);
830 }
832  getRegressionPlane(*this,p);
833 }
835  for_each(begin(),end(),FAddPoint<TPoint3D,3>(p));
836  size_t N=size();
837  p.x/=N;
838  p.y/=N;
839  p.z/=N;
840 }
841 bool TPolygon3D::isSkew() const {
842  return !mrpt::math::conformAPlane(*this);
843 }
845  removeRepVertices(*this);
846 }
849  removeUnusedVertices(*this);
850 }
851 TPolygon3D::TPolygon3D(const TPolygon2D &p):std::vector<TPoint3D>() {
852  size_t N=p.size();
853  resize(N);
854  for (size_t i=0;i<N;i++) operator[](i)=p[i];
855 }
856 void TPolygon3D::createRegularPolygon(size_t numEdges,double radius,TPolygon3D &poly) {
857  if (numEdges<3||abs(radius)<geometryEpsilon) throw std::logic_error("Invalid arguments for regular polygon creations");
858  poly.resize(numEdges);
859  for (size_t i=0;i<numEdges;i++) {
860  double angle=i*2*M_PI/numEdges;
861  poly[i]=TPoint3D(radius*cos(angle),radius*sin(angle),0);
862  }
863 }
864 inline void TPolygon3D::createRegularPolygon(size_t numEdges,double radius,TPolygon3D &poly,const mrpt::poses::CPose3D &pose) {
865  createRegularPolygon(numEdges,radius,poly);
866  for (size_t i=0;i<numEdges;i++) pose.composePoint(poly[i][0],poly[i][1],poly[i][2],poly[i][0],poly[i][1],poly[i][2]);
867 }
868 
870  switch (type) {
872  obj=TPoint3D(data.point);
873  break;
875  obj=TSegment3D(data.segment);
876  break;
877  case GEOMETRIC_TYPE_LINE:
878  obj=TLine3D(data.line);
879  break;
881  obj=TPolygon3D(*(data.polygon));
882  break;
883  default:
884  obj=TObject3D();
885  break;
886  }
887 }
888 void TObject2D::getPoints(const std::vector<TObject2D> &objs,std::vector<TPoint2D> &pnts) {
889  for (vector<TObject2D>::const_iterator it=objs.begin();it!=objs.end();++it) if (it->isPoint()) pnts.push_back(it->data.point);
890 }
891 void TObject2D::getSegments(const std::vector<TObject2D> &objs,std::vector<TSegment2D> &sgms) {
892  for (vector<TObject2D>::const_iterator it=objs.begin();it!=objs.end();++it) if (it->isSegment()) sgms.push_back(it->data.segment);
893 }
894 void TObject2D::getLines(const std::vector<TObject2D> &objs,std::vector<TLine2D> &lins) {
895  for (vector<TObject2D>::const_iterator it=objs.begin();it!=objs.end();++it) if (it->isLine()) lins.push_back(it->data.line);
896 }
897 void TObject2D::getPolygons(const std::vector<TObject2D> &objs,std::vector<TPolygon2D> &polys) {
898  for (vector<TObject2D>::const_iterator it=objs.begin();it!=objs.end();++it) if (it->isPolygon()) polys.push_back(*(it->data.polygon));
899 }
900 void TObject2D::getPoints(const std::vector<TObject2D> &objs,std::vector<TPoint2D> &pnts,std::vector<TObject2D> &remainder) {
901  for (vector<TObject2D>::const_iterator it=objs.begin();it!=objs.end();++it) if (it->isPoint()) pnts.push_back(it->data.point);
902  else remainder.push_back(*it);
903 }
904 void TObject2D::getSegments(const std::vector<TObject2D> &objs,std::vector<TSegment2D> &sgms,std::vector<TObject2D> &remainder) {
905  for (vector<TObject2D>::const_iterator it=objs.begin();it!=objs.end();++it) if (it->isSegment()) sgms.push_back(it->data.segment);
906  else remainder.push_back(*it);
907 }
908 void TObject2D::getLines(const std::vector<TObject2D> &objs,std::vector<TLine2D> &lins,std::vector<TObject2D> &remainder) {
909  for (vector<TObject2D>::const_iterator it=objs.begin();it!=objs.end();++it) if (it->isLine()) lins.push_back(it->data.line);
910  else remainder.push_back(*it);
911 }
912 void TObject2D::getPolygons(const std::vector<TObject2D> &objs,std::vector<TPolygon2D> &polys,vector<TObject2D> &remainder) {
913  for (vector<TObject2D>::const_iterator it=objs.begin();it!=objs.end();++it) if (it->isPolygon()) polys.push_back(*(it->data.polygon));
914  else remainder.push_back(*it);
915 }
916 void TObject3D::getPoints(const std::vector<TObject3D> &objs,std::vector<TPoint3D> &pnts) {
917  for (vector<TObject3D>::const_iterator it=objs.begin();it!=objs.end();++it) if (it->isPoint()) pnts.push_back(it->data.point);
918 }
919 void TObject3D::getSegments(const std::vector<TObject3D> &objs,std::vector<TSegment3D> &sgms) {
920  for (vector<TObject3D>::const_iterator it=objs.begin();it!=objs.end();++it) if (it->isSegment()) sgms.push_back(it->data.segment);
921 }
922 void TObject3D::getLines(const std::vector<TObject3D> &objs,std::vector<TLine3D> &lins) {
923  for (vector<TObject3D>::const_iterator it=objs.begin();it!=objs.end();++it) if (it->isLine()) lins.push_back(it->data.line);
924 }
925 void TObject3D::getPlanes(const std::vector<TObject3D> &objs,std::vector<TPlane> &plns) {
926  for (vector<TObject3D>::const_iterator it=objs.begin();it!=objs.end();++it) if (it->isPlane()) plns.push_back(it->data.plane);
927 }
928 void TObject3D::getPolygons(const std::vector<TObject3D> &objs,std::vector<TPolygon3D> &polys) {
929  for (vector<TObject3D>::const_iterator it=objs.begin();it!=objs.end();++it) if (it->isPolygon()) polys.push_back(*(it->data.polygon));
930 }
931 void TObject3D::getPoints(const std::vector<TObject3D> &objs,std::vector<TPoint3D> &pnts,std::vector<TObject3D> &remainder) {
932  for (vector<TObject3D>::const_iterator it=objs.begin();it!=objs.end();++it) if (it->isPoint()) pnts.push_back(it->data.point);
933  else remainder.push_back(*it);
934 }
935 void TObject3D::getSegments(const std::vector<TObject3D> &objs,std::vector<TSegment3D> &sgms,std::vector<TObject3D> &remainder) {
936  for (vector<TObject3D>::const_iterator it=objs.begin();it!=objs.end();++it) if (it->isSegment()) sgms.push_back(it->data.segment);
937  else remainder.push_back(*it);
938 }
939 void TObject3D::getLines(const std::vector<TObject3D> &objs,std::vector<TLine3D> &lins,std::vector<TObject3D> &remainder) {
940  for (vector<TObject3D>::const_iterator it=objs.begin();it!=objs.end();++it) if (it->isLine()) lins.push_back(it->data.line);
941  else remainder.push_back(*it);
942 }
943 void TObject3D::getPlanes(const std::vector<TObject3D> &objs,std::vector<TPlane> &plns,std::vector<TObject3D> &remainder) {
944  for (vector<TObject3D>::const_iterator it=objs.begin();it!=objs.end();++it) if (it->isPlane()) plns.push_back(it->data.plane);
945  else remainder.push_back(*it);
946 }
947 void TObject3D::getPolygons(const std::vector<TObject3D> &objs,std::vector<TPolygon3D> &polys,vector<TObject3D> &remainder) {
948  for (vector<TObject3D>::const_iterator it=objs.begin();it!=objs.end();++it) if (it->isPolygon()) polys.push_back(*(it->data.polygon));
949  else remainder.push_back(*it);
950 }
951 
953 {
954  in >> o.x >> o.y;
955  return in;
956 }
958 {
959  out << o.x << o.y;
960  return out;
961 }
962 
964 {
965  in >> o.x >> o.y >> o.z;
966  return in;
967 }
969 {
970  out << o.x << o.y << o.z;
971  return out;
972 }
973 
975 {
976  in >> o.x >> o.y >> o.phi;
977  return in;
978 }
980 {
981  out << o.x << o.y << o.phi;
982  return out;
983 }
984 
986 {
987  for (unsigned int i=0;i<o.size();i++) in >> o[i];
988  return in;
989 }
991 {
992  for (unsigned int i=0;i<o.size();i++) out << o[i];
993  return out;
994 }
995 
997 {
998  for (unsigned int i=0;i<o.size();i++) in >> o[i];
999  return in;
1000 }
1002 {
1003  for (unsigned int i=0;i<o.size();i++) out << o[i];
1004  return out;
1005 }
1006 
1009 
1012 
1013 
1015 {
1016  in >> o.x >> o.y >> o.z >> o.yaw >> o.pitch >> o.roll;
1017  return in;
1018 }
1020 {
1021  out << o.x << o.y << o.z << o.yaw << o.pitch << o.roll;
1022  return out;
1023 }
1024 
1026  uint16_t type;
1027  in>>type;
1028  switch (static_cast<unsigned char>(type)) {
1029  case GEOMETRIC_TYPE_POINT: {
1030  TPoint2D p;
1031  in>>p;
1032  o=p;
1033  } break;
1034  case GEOMETRIC_TYPE_SEGMENT: {
1035  TSegment2D s;
1036  in>>s;
1037  o=s;
1038  } break;
1039  case GEOMETRIC_TYPE_LINE: {
1040  TLine2D l;
1041  in>>l;
1042  o=l;
1043  } break;
1044  case GEOMETRIC_TYPE_POLYGON: {
1045  TPolygon2D p;
1046  in>>p;
1047  o=p;
1048  } break;
1049  case GEOMETRIC_TYPE_UNDEFINED: {
1050  o=TObject2D();
1051  } break;
1052  default:
1053  throw std::logic_error("Unknown TObject2D type found while reading stream");
1054  }
1055  return in;
1056 }
1058  out<<static_cast<uint16_t>(o.getType());
1059  switch (o.getType()) {
1060  case GEOMETRIC_TYPE_POINT: {
1061  TPoint2D p;
1062  o.getPoint(p);
1063  return out<<p;
1064  };
1065  case GEOMETRIC_TYPE_SEGMENT: {
1066  TSegment2D s;
1067  o.getSegment(s);
1068  return out<<s;
1069  };
1070  case GEOMETRIC_TYPE_LINE: {
1071  TLine2D l;
1072  o.getLine(l);
1073  return out<<l;
1074  };
1075  case GEOMETRIC_TYPE_POLYGON: {
1076  TPolygon2D p;
1077  o.getPolygon(p);
1078  return out<<p;
1079  };
1080  }
1081  return out;
1082 }
1083 
1085  uint16_t type;
1086  in>>type;
1087  switch (static_cast<unsigned char>(type)) {
1088  case GEOMETRIC_TYPE_POINT: {
1089  TPoint3D p;
1090  in>>p;
1091  o=p;
1092  } break;
1093  case GEOMETRIC_TYPE_SEGMENT: {
1094  TSegment3D s;
1095  in>>s;
1096  o=s;
1097  } break;
1098  case GEOMETRIC_TYPE_LINE: {
1099  TLine3D l;
1100  in>>l;
1101  o=l;
1102  } break;
1103  case GEOMETRIC_TYPE_PLANE: {
1104  TPlane p;
1105  in>>p;
1106  o=p;
1107  } break;
1108  case GEOMETRIC_TYPE_POLYGON: {
1109  TPolygon3D p;
1110  in>>p;
1111  o=p;
1112  } break;
1113  case GEOMETRIC_TYPE_UNDEFINED: {
1114  o=TObject3D();
1115  } break;
1116  default:
1117  throw std::logic_error("Unknown TObject3D type found while reading stream");
1118  }
1119  return in;
1120 }
1122  out<<static_cast<uint16_t>(o.getType());
1123  switch (o.getType()) {
1124  case GEOMETRIC_TYPE_POINT: {
1125  TPoint3D p;
1126  o.getPoint(p);
1127  return out<<p;
1128  };
1129  case GEOMETRIC_TYPE_SEGMENT: {
1130  TSegment3D s;
1131  o.getSegment(s);
1132  return out<<s;
1133  };
1134  case GEOMETRIC_TYPE_LINE: {
1135  TLine3D l;
1136  o.getLine(l);
1137  return out<<l;
1138  };
1139  case GEOMETRIC_TYPE_PLANE: {
1140  TPlane p;
1141  o.getPlane(p);
1142  return out<<p;
1143  };
1144  case GEOMETRIC_TYPE_POLYGON: {
1145  TPolygon3D p;
1146  o.getPolygon(p);
1147  return out<<p;
1148  };
1149  }
1150  return out;
1151 }
1152 
1153 }} //end of namespaces
GLboolean GLboolean GLboolean GLboolean a
Definition: glew.h:5406
bool contains(const TPoint2D &point) const
Check whether a point is inside the line.
double signedDistance(const TPoint2D &point) const
Distance with sign to point (sign indicates which side the point is).
const GLdouble * v
Definition: glew.h:1296
const unsigned char GEOMETRIC_TYPE_PLANE
Object type identifier for TPlane.
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 getPlane(TPlane &p) const
Gets the content as a plane, returning false if the type is not adequate.
static void createRegularPolygon(size_t numEdges, double radius, TPolygon3D &poly)
Static method to create a regular polygon, given its size and radius.
GLint GLint GLint GLint GLint GLint y
Definition: glew.h:1166
static void createRegularPolygon(size_t numEdges, double radius, TPolygon2D &poly)
Static method to create a regular polygon, given its size and radius.
void fromString(const std::string &s)
Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04]" ) ...
void generate3DObject(TObject3D &obj) const
Project into 3D space.
void rotate(const double ang)
Transform the (vx,vy) components for a counterclockwise rotation of ang radians.
double y
X,Y coordinates.
void getBestFittingPlane(TPlane &p) const
Gets the best fitting plane, disregarding whether the polygon actually fits inside or not...
const unsigned char GEOMETRIC_TYPE_LINE
Object type identifier for TLine2D or TLine3D.
#define min(a, b)
unsigned __int16 uint16_t
Definition: rptypes.h:46
mrpt::math::TPose2D operator+(const mrpt::math::TPose2D &b) const
Operator "oplus" pose composition: "ret=this \oplus b".
void BASE_IMPEXP generateAxisBaseFromDirectionAndAxis(const double(&vec)[3], char coord, CMatrixDouble &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:1652
TPoint2D BASE_IMPEXP lightFromPose(const mrpt::poses::CPoint2D &p)
Convert a pose into a light-weight structure (functional form, needed for forward declarations) ...
std::string asString() const
const unsigned char GEOMETRIC_TYPE_POINT
Object type identifier for TPoint2D or TPoint3D.
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:84
double wz
Angular velocity (rad/s)
bool getLine(TLine3D &r) const
Gets the content as a line, returning false if the type is not adequate.
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)
Definition: bits.h:43
TLine2D()
Fast default constructor.
double roll
Roll coordinate (rotation angle over X coordinate).
static void getPolygons(const std::vector< TObject2D > &objs, std::vector< TPolygon2D > &polys)
Static method to retrieve all the polygons in a vector of TObject2D.
const GLfloat * c
Definition: glew.h:10088
TPolygon3D()
Default constructor.
#define THROW_EXCEPTION(msg)
bool contains(const TPoint2D &point) const
Check whether a point is inside a segment.
double signedDistance(const TPoint2D &point) const
Distance with sign from a given point (sign indicates side).
double distance(const TPoint2D &point) const
Distance from a given 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...
GLubyte GLubyte GLubyte GLubyte w
Definition: glew.h:1797
GLdouble GLdouble t
Definition: glew.h:1303
std::string asString() const
double distance(const TPoint3D &point) const
Distance to point.
void getAsPose2D(mrpt::poses::CPose2D &outPose) const
Get a pose2D whose X axis corresponds to the line.
bool getPoint(TPoint2D &p) const
Gets the content as a point, returning false if the type is inadequate.
EIGEN_STRONG_INLINE iterator begin()
Definition: eigen_plugins.h:26
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:35
TPoint3D()
Default fast constructor.
GLfloat GLfloat GLfloat v2
Definition: glew.h:1763
Standard type for storing any lightweight 2D type.
TLine3D()
Fast default constructor.
void getAsPose3D(mrpt::poses::CPose3D &outPose)
Gets a pose whose XY plane corresponds to this plane.
TPoint3D pBase
Base point.
Standard object for storing any 3D lightweight object.
#define M_PI
Definition: bits.h:78
void getNormalVector(double(&vector)[2]) const
Get line's normal vector.
void removeRepeatedVertices()
Erase repeated vertices.
const Scalar * const_iterator
Definition: eigen_plugins.h:24
void removeRepVertices(T &poly)
void fromString(const std::string &s)
Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -45...
double z
X,Y,Z coordinates.
void getCenter(TPoint2D &p) const
Polygon's central point.
struct BASE_IMPEXP TObject3D
double yaw
Yaw coordinate (rotation angle over Z axis).
bool contains(const TPoint3D &point) const
Check whether a point is inside the segment.
bool contains(const TPoint3D &point) const
Check whether a point is inside the line.
double vz
Velocity components: X,Y (m/s)
bool operator==(const TTwist2D &o) const
void fromString(const std::string &s)
Set the current object value from a string generated by 'asString' (eg: "[vx vy vz wx wy wz]" ) ...
bool getPlane(TPlane &p) const
Gets a plane which contains the polygon. Returns false if the polygon is skew and cannot be fit insid...
struct BASE_IMPEXP TSegment3D
void getRotationMatrix(mrpt::math::CMatrixDouble33 &ROT) const
Get the 3x3 rotation matrix.
Definition: CPose3D.h:176
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...
GLuint in
Definition: glew.h:7146
3D twist: 3D velocity vector (vx,vy,vz) + angular velocity (wx,wy,wz)
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:614
GLdouble l
Definition: glew.h:5092
bool isConvex() const
Checks whether is convex.
TPoint3D point1
Origin point.
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
bool operator<(const TSegment3D &s) const
bool BASE_IMPEXP conformAPlane(const std::vector< TPoint3D > &points)
Checks whether this polygon or set of points acceptably fits a plane.
Definition: geometry.cpp:829
static void getSegments(const std::vector< TObject3D > &objs, std::vector< TSegment3D > &sgms)
Static method to retrieve every segment included in a vector of objects.
TPose3D()
Default fast constructor.
2D twist: 2D velocity vector (vx,vy) + planar angular velocity (omega)
double distance(const TPoint3D &point) const
Distance to point (always >=0)
void getAsPose2DForcingOrigin(const TPoint2D &origin, mrpt::poses::CPose2D &outPose) const
Get a pose2D whose X axis corresponds to the line, forcing the base point to one given.
double isLeft(const mrpt::math::TPoint2D &P0, const mrpt::math::TPoint2D &P1, const mrpt::math::TPoint2D &P2)
GLdouble s
Definition: glew.h:1295
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
void unitarize()
Unitarize line's normal vector.
2D segment, consisting of two points.
void getAsVector(VECTORLIKE &v) const
Transformation into vector.
GLuint GLuint end
Definition: glew.h:1167
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
void getAsSegmentList(std::vector< TSegment2D > &v) const
Gets as set of segments, instead of points.
void fromString(const std::string &s)
Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -45...
double RAD2DEG(const double x)
Radians to degrees.
GLsizei n
Definition: glew.h:5051
3D segment, consisting of two points.
bool operator<(const TSegment2D &s) const
TPoint3D point2
Destiny point.
std::ostream BASE_IMPEXP & operator<<(std::ostream &o, const TPoint2D &p)
EIGEN_STRONG_INLINE bool empty() const
void rotate(const mrpt::poses::CPose3D &rot)
Transform all 6 components for a change of reference frame from "A" to another frame "B" whose rotati...
bool getSegment(TSegment3D &s) const
Gets the content as a segment, returning false if the type is not adequate.
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.
bool getPoint(TPoint3D &p) const
Gets the content as a point, returning false if the type is not adequate.
const unsigned char GEOMETRIC_TYPE_SEGMENT
Object type identifier for TSegment2D or TSegment3D.
VALUE & operator[](const KEY &key)
Write/read via [i] operator, that creates an element if it didn't exist already.
Definition: ts_hash_map.h:123
static void getPolygons(const std::vector< TObject3D > &objs, std::vector< TPolygon3D > &polys)
Static method to retrieve every polygon included in a vector of objects.
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
3D Plane, represented by its equation
double vy
Velocity components: X,Y (m/s)
GLhandleARB obj
Definition: glew.h:3276
double coefs[3]
Line coefficients, stored as an array: .
class BASE_IMPEXP TPolygon3D
double BASE_IMPEXP getRegressionPlane(const std::vector< TPoint3D > &points, TPlane &plane)
Using eigenvalues, gets the best fitting plane for a set of 3D points.
Definition: geometry.cpp:1710
TPolygon2D()
Default constructor.
void removeRedundantVertices()
Erase every redundant vertex, thus saving space.
void generate3DObject(TPolygon3D &p) const
Projects into 3D space, zeroing the z.
TPose3DQuat()
Default fast constructor.
TPoint2D point2
Destiny point.
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:41
GLint GLenum GLsizei GLint GLsizei const GLvoid * data
Definition: glew.h:1284
void getNormalVector(double(&vec)[3]) const
Get plane's normal vector.
#define DEG2RAD
GLfloat GLfloat p
Definition: glew.h:10113
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
double BASE_IMPEXP getAngle(const TPlane &p1, const TPlane &p2)
Computes the angle between two planes.
Definition: geometry.cpp:726
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:51
A class used to store a 2D point.
Definition: CPoint2D.h:36
A class used to store a 3D point.
Definition: CPoint3D.h:32
void unitarize()
Unitarize director vector.
double distance(const TPoint3D &point) const
Distance to 3D point.
void getAsQuaternion(mrpt::math::CQuaternion< double > &q, mrpt::math::CMatrixFixedNumeric< double, 4, 3 > *out_dq_dr=NULL) const
Returns the quaternion associated to the rotation of this object (NOTE: XYZ translation is ignored) ...
double pitch
Pitch coordinate (rotation angle over Y axis).
Lightweight 3D pose (three spatial coordinates, plus a quaternion ).
double director[3]
Director vector.
double length() const
Segment length.
TPoint2D point1
Origin point.
unsigned char getType() const
Gets content type.
TPoint2D()
Default fast constructor.
double y
X,Y coordinates.
std::string asString() const
BASE_IMPEXP::mrpt::utils::CStream & operator>>(mrpt::utils::CStream &in, CMatrixPtr &pObj)
void removeUnusedVertices(T &poly)
mrpt::math::TPose2D operator*(const double dt) const
Returns the pose increment of multiplying each twist component times "dt" seconds.
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacobian_df_dpoint=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dpose=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dse3=NULL, bool use_small_rot_approx=false) const
An alternative, slightly more efficient way of doing with G and L being 3D points and P this 6D pose...
Definition: CPose3D.cpp:427
#define RAD2DEG
bool contains(const TPoint2D &point) const
Check whether a point is inside (or within geometryEpsilon of a polygon edge). This works for concave...
GLdouble GLdouble z
Definition: glew.h:1464
TPlane()
Fast default constructor.
GLsizei const GLcharARB ** string
Definition: glew.h:3293
size_t size(const MATRIXLIKE &m, const int dim)
mrpt::math::TPose2D operator-(const mrpt::math::TPose2D &b) const
Operator "ominus" pose composition: "ret=this \ominus b".
double evaluatePoint(const TPoint3D &point) const
Evaluate a point in the plane's equation.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
const float R
bool operator==(const TTwist3D &o) const
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
struct BASE_IMPEXP TLine3D
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 BASE_IMPEXP 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:1606
void getUnitaryNormalVector(double(&vec)[3])
Unitarize, then get normal vector.
bool isSkew() const
Check whether the polygon is skew. Returns true if there doesn't exist a plane in which the polygon c...
std::string asString() const
bool getSegment(TSegment2D &s) const
Gets the content as a segment, returning false if the type is inadequate.
double coefs[4]
Plane coefficients, stored as an array: .
double evaluatePoint(const TPoint2D &point) const
Evaluate point in the line's equation.
bool contains(const TPoint3D &point) const
Check whether a point is inside (or within geometryEpsilon of a polygon edge). This works for concave...
TSegment2D()
Fast default constructor.
bool operator!=(const TTwist2D &o) const
void getAsSegmentList(std::vector< TSegment3D > &v) const
Gets as set of segments, instead of set of points.
void fromString(const std::string &s)
Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -0...
void project3D(const TPoint3D &point, const mrpt::poses::CPose3D &newXYpose, TPoint3D &newPoint)
Uses the given pose 3D to project a point into a new base.
Definition: geometry.h:270
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Lightweight 2D pose.
void removeRedundantVertices()
Erase every redundant vertex from the polygon, saving space.
void removeRepeatedVertices()
Remove polygon's repeated vertices.
const GLfloat * tc
Definition: glew.h:10096
GLdouble GLdouble GLdouble r
Definition: glew.h:1311
void fromString(const std::string &s)
Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -0...
double length() const
Segment length.
bool operator<(const TPoint2D &p) const
bool getPolygon(TPolygon2D &p) const
Gets the content as a polygon, returning false if the type is inadequate.
double distance(const TPoint3D &point) const
Distance between the line and a point.
const GLdouble * m
Definition: glew.h:5094
double distance(const TPoint2D &point) const
Distance to point.
bool getPolygon(TPolygon3D &p) const
Gets the content as a polygon, returning false if the type is not adequate.
TPose2D()
Default fast constructor.
INT64 INT64 INT64 remainder
Definition: wglew.h:919
GLdouble angle
Definition: glew.h:5105
const unsigned char GEOMETRIC_TYPE_UNDEFINED
Object type identifier for empty TObject2D or TObject3D.
void getCenter(TPoint3D &p) const
Get polygon's central point.
void getBoundingBox(TPoint2D &min_coords, TPoint2D &max_coords) const
Get polygon bounding box. empty polygon.
GLdouble GLdouble GLdouble b
Definition: glew.h:5092
GLuint GLuint GLsizei GLenum type
Definition: glew.h:1167
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...
Definition: CQuaternion.h:43
GLdouble GLdouble GLdouble GLdouble q
Definition: glew.h:1319
Lightweight 3D point.
double distance(const TPoint2D &point) const
Distance to a point (always >=0)
Lightweight 2D point.
const unsigned char GEOMETRIC_TYPE_POLYGON
Object type identifier for TPolygon2D or TPolygon3D.
void getAsPose3DForcingOrigin(const TPoint3D &newOrigin, mrpt::poses::CPose3D &pose)
Gets a pose whose XY plane corresponds to this, forcing an exact point as its spatial coordinates...
bool operator<(const TPoint3D &p) const
#define ASSERTMSG_(f, __ERROR_MSG)
unsigned char getType() const
Gets object type.
double z
X,Y,Z, coords.
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)
bool getLine(TLine2D &r) const
Gets the content as a line, returning false if the type is inadequate.
#define MRPT_ALIGN16
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...
2D polygon, inheriting from std::vector<TPoint2D>.
3D polygon, inheriting from std::vector<TPoint3D>
double BASE_IMPEXP distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Definition: geometry.cpp:1511
void getDirectorVector(double(&vector)[2]) const
Get line's director vector.
double omega
Angular velocity (rad/s)
bool contains(const TPoint3D &point) const
Check whether a point is contained into the plane.
void generate3DObject(TSegment3D &s) const
Project into 3D space, setting the z to 0.
double BASE_IMPEXP geometryEpsilon
Global epsilon to overcome small precision errors (Default=1e-5)
Definition: geometry.cpp:28
bool operator!=(const TTwist3D &o) const
void fromString(const std::string &s)
Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -0...
3D line, represented by a base point and a director vector.
void generate3DObject(TLine3D &l) const
Project into 3D space, setting the z to 0.
2D line without bounds, represented by its equation .
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.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018