Main MRPT website > C++ reference for MRPT 1.5.7
Miscellaneous.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 /* Plane-based Map (PbMap) library
11  * Construction of plane-based maps and localization in it from RGBD Images.
12  * Writen by Eduardo Fernandez-Moral. See docs for <a href="group__mrpt__pbmap__grp.html" >mrpt-pbmap</a>
13  */
14 
15 #include "pbmap-precomp.h" // Precompiled headers
16 
17 #if MRPT_HAS_PCL
18 
20 
21 using namespace mrpt::pbmap;
22 
23 #define SMALL_NUM 0.00000001 // anything that avoids division overflow
25 {
26  Eigen::Vector3f u = diffPoints(S1.P1, S1.P0);
27  Eigen::Vector3f v = diffPoints(S2.P1, S2.P0);
28  Eigen::Vector3f w = diffPoints(S1.P0, S2.P0);
29  float a = u.dot(u); // always >= 0
30  float b = u.dot(v);
31  float c = v.dot(v); // always >= 0
32  float d = u.dot(w);
33  float e = v.dot(w);
34  float D = a*c - b*b; // always >= 0
35  float sc, sN, sD = D; // sc = sN / sD, default sD = D >= 0
36  float tc, tN, tD = D; // tc = tN / tD, default tD = D >= 0
37 
38  // compute the line parameters of the two closest points
39  if (D < SMALL_NUM) { // the lines are almost parallel
40  sN = 0.0; // force using point P0 on segment S1
41  sD = 1.0; // to prevent possible division by 0.0 later
42  tN = e;
43  tD = c;
44  }
45  else { // get the closest points on the infinite lines
46  sN = (b*e - c*d);
47  tN = (a*e - b*d);
48  if (sN < 0.0) { // sc < 0 => the s=0 edge is visible
49  sN = 0.0;
50  tN = e;
51  tD = c;
52  }
53  else if (sN > sD) { // sc > 1 => the s=1 edge is visible
54  sN = sD;
55  tN = e + b;
56  tD = c;
57  }
58  }
59 
60  if (tN < 0.0) { // tc < 0 => the t=0 edge is visible
61  tN = 0.0;
62  // recompute sc for this edge
63  if (-d < 0.0)
64  sN = 0.0;
65  else if (-d > a)
66  sN = sD;
67  else {
68  sN = -d;
69  sD = a;
70  }
71  }
72  else if (tN > tD) { // tc > 1 => the t=1 edge is visible
73  tN = tD;
74  // recompute sc for this edge
75  if ((-d + b) < 0.0)
76  sN = 0;
77  else if ((-d + b) > a)
78  sN = sD;
79  else {
80  sN = (-d + b);
81  sD = a;
82  }
83  }
84  // finally do the division to get sc and tc
85  sc = (fabs(sN) < SMALL_NUM ? 0.0 : sN / sD);
86  tc = (fabs(tN) < SMALL_NUM ? 0.0 : tN / tD);
87 
88  // get the difference of the two closest points
89  Eigen::Vector3f dP = w + (sc * u) - (tc * v); // = S1(sc) - S2(tc)
90 
91  return dP.squaredNorm(); // return the closest distance
92 }
93 
94 bool mrpt::pbmap::isInHull(PointT &point3D, pcl::PointCloud<PointT>::Ptr hull3D)
95 {
96  Eigen::Vector2f normalLine; // This vector points inward the hull
97  Eigen::Vector2f r;
98  for(size_t i=1; i < hull3D->size(); i++)
99  {
100  normalLine[0] = hull3D->points[i-1].y - hull3D->points[i].y;
101  normalLine[1] = hull3D->points[i].x - hull3D->points[i-1].x;
102  r[0] = point3D.x - hull3D->points[i].x;
103  r[1] = point3D.y - hull3D->points[i].y;
104  if( (r .dot( normalLine) ) < 0)
105  return false;
106  }
107  return true;
108 }
109 
110 #endif
Eigen::Vector3f diffPoints(const POINT &P1, const POINT &P2)
Definition: Miscellaneous.h:44
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:3962
const GLfloat * tc
Definition: glext.h:5610
const GLubyte * c
Definition: glext.h:5590
GLubyte GLubyte b
Definition: glext.h:5575
const GLdouble * v
Definition: glext.h:3603
float PBMAP_IMPEXP dist3D_Segment_to_Segment2(Segment S1, Segment S2)
GLdouble GLdouble GLdouble r
Definition: glext.h:3618
#define SMALL_NUM
bool PBMAP_IMPEXP isInHull(PointT &point3D, pcl::PointCloud< PointT >::Ptr hull3D)
GLubyte GLubyte GLubyte a
Definition: glext.h:5575
pcl::PointXYZRGBA PointT
Definition: Miscellaneous.h:34



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019