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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020