Point Cloud Library (PCL)  1.14.1
geometry.h
1 /*
2 Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without modification,
6 are permitted provided that the following conditions are met:
7 
8 Redistributions of source code must retain the above copyright notice, this list of
9 conditions and the following disclaimer. Redistributions in binary form must reproduce
10 the above copyright notice, this list of conditions and the following disclaimer
11 in the documentation and/or other materials provided with the distribution.
12 
13 Neither the name of the Johns Hopkins University nor the names of its contributors
14 may be used to endorse or promote products derived from this software without specific
15 prior written permission.
16 
17 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
18 EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO THE IMPLIED WARRANTIES
19 OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
20 SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
21 INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
22 TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
23 BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
24 CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
25 ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
26 DAMAGE.
27 */
28 
29 #ifndef GEOMETRY_INCLUDED
30 #define GEOMETRY_INCLUDED
31 
32 #if defined __GNUC__
33 # pragma GCC system_header
34 #endif
35 
36 #include <pcl/pcl_macros.h>
37 #include <math.h>
38 #include <vector>
39 #include <unordered_map>
40 
41 namespace pcl
42 {
43  namespace poisson
44  {
45 
46  template<class Real>
47  Real Random(void);
48 
49  template< class Real >
50  struct Point3D
51  {
53  Point3D( void ) { coords[0] = coords[1] = coords[2] = Real(0); }
54  inline Real& operator[] ( int i ) { return coords[i]; }
55  inline const Real& operator[] ( int i ) const { return coords[i]; }
56  inline Point3D& operator += ( Point3D p ){ coords[0] += p.coords[0] , coords[1] += p.coords[1] , coords[2] += p.coords[2] ; return *this; }
57  inline Point3D& operator -= ( Point3D p ){ coords[0] -= p.coords[0] , coords[1] -= p.coords[1] , coords[2] -= p.coords[2] ; return *this; }
58  inline Point3D& operator *= ( Real r ){ coords[0] *= r , coords[1] *= r , coords[2] *= r ; return *this; }
59  inline Point3D& operator /= ( Real r ){ coords[0] /= r , coords[1] /= r , coords[2] /= r ; return *this; }
60  inline Point3D operator + ( Point3D p ) const { Point3D q ; q.coords[0] = coords[0] + p.coords[0] , q.coords[1] = coords[1] + p.coords[1] , q.coords[2] = coords[2] + p.coords[2] ; return q; }
61  inline Point3D operator - ( Point3D p ) const { Point3D q ; q.coords[0] = coords[0] - p.coords[0] , q.coords[1] = coords[1] - p.coords[1] , q.coords[2] = coords[2] - p.coords[2] ; return q; }
62  inline Point3D operator * ( Real r ) const { Point3D q ; q.coords[0] = coords[0] * r , q.coords[1] = coords[1] * r , q.coords[2] = coords[2] * r ; return q; }
63  inline Point3D operator / ( Real r ) const { return (*this) * ( Real(1.)/r ); }
64  };
65 
66  template<class Real>
67  Point3D<Real> RandomBallPoint(void);
68 
69  template<class Real>
70  Point3D<Real> RandomSpherePoint(void);
71 
72  template<class Real>
73  double Length(const Point3D<Real>& p);
74 
75  template<class Real>
76  double SquareLength(const Point3D<Real>& p);
77 
78  template<class Real>
79  double Distance(const Point3D<Real>& p1,const Point3D<Real>& p2);
80 
81  template<class Real>
82  double SquareDistance(const Point3D<Real>& p1,const Point3D<Real>& p2);
83 
84  template <class Real>
85  void CrossProduct(const Point3D<Real>& p1,const Point3D<Real>& p2,Point3D<Real>& p);
86 
87  class Edge
88  {
89  public:
90  double p[2][2];
91  double Length(void) const
92  {
93  double d[2];
94  d[0]=p[0][0]-p[1][0];
95  d[1]=p[0][1]-p[1][1];
96 
97  return sqrt(d[0]*d[0]+d[1]*d[1]);
98  }
99  };
100  class Triangle
101  {
102  public:
103  double p[3][3];
104  double Area(void) const
105  {
106  double v1[3] , v2[3] , v[3];
107  for( int d=0 ; d<3 ; d++ )
108  {
109  v1[d] = p[1][d] - p[0][d];
110  v2[d] = p[2][d] - p[0][d];
111  }
112  v[0] = v1[1]*v2[2] - v1[2]*v2[1];
113  v[1] = -v1[0]*v2[2] + v1[2]*v2[0];
114  v[2] = v1[0]*v2[1] - v1[1]*v2[0];
115  return sqrt( v[0]*v[0] + v[1]*v[1] + v[2]*v[2] ) / 2;
116  }
117  double AspectRatio(void) const
118  {
119  double d=0;
120  int i,j;
121  for(i=0;i<3;i++){
122  for(i=0;i<3;i++)
123  for(j=0;j<3;j++){d+=(p[(i+1)%3][j]-p[i][j])*(p[(i+1)%3][j]-p[i][j]);}
124  }
125  return Area()/d;
126  }
127 
128  };
129  class PCL_EXPORTS CoredPointIndex
130  {
131  public:
132  int index;
133  char inCore;
134 
135  int operator == (const CoredPointIndex& cpi) const {return (index==cpi.index) && (inCore==cpi.inCore);};
136  int operator != (const CoredPointIndex& cpi) const {return (index!=cpi.index) || (inCore!=cpi.inCore);};
137  };
138  class EdgeIndex{
139  public:
140  int idx[2];
141  };
143  public:
145  };
147  public:
148  int idx[3];
149  };
150 
152  {
153  public:
154  TriangulationEdge(void);
155  int pIndex[2];
156  int tIndex[2];
157  };
158 
160  {
161  public:
162  TriangulationTriangle(void);
163  int eIndex[3];
164  };
165 
166  template<class Real>
168  {
169  public:
170 
171  std::vector<Point3D<Real> > points;
172  std::vector<TriangulationEdge> edges;
173  std::vector<TriangulationTriangle> triangles;
174 
175  int factor( int tIndex,int& p1,int& p2,int& p3);
176  double area(void);
177  double area( int tIndex );
178  double area( int p1 , int p2 , int p3 );
179  int flipMinimize( int eIndex);
180  int addTriangle( int p1 , int p2 , int p3 );
181 
182  protected:
183  std::unordered_map<long long,int> edgeMap;
184  static long long EdgeIndex( int p1 , int p2 );
185  double area(const Triangle& t);
186  };
187 
188 
189  template<class Real>
190  void EdgeCollapse(const Real& edgeRatio,std::vector<TriangleIndex>& triangles,std::vector< Point3D<Real> >& positions,std::vector<Point3D<Real> >* normals);
191  template<class Real>
192  void TriangleCollapse(const Real& edgeRatio,std::vector<TriangleIndex>& triangles,std::vector<Point3D<Real> >& positions,std::vector<Point3D<Real> >* normals);
193 
195  {
196  int idx;
197  bool inCore;
198  };
199  class PCL_EXPORTS CoredMeshData
200  {
201  public:
202  std::vector<Point3D<float> > inCorePoints;
203 
204  virtual ~CoredMeshData() = default;
205  virtual void resetIterator( void ) = 0;
206 
207  virtual int addOutOfCorePoint( const Point3D<float>& p ) = 0;
208  virtual int addPolygon( const std::vector< CoredVertexIndex >& vertices ) = 0;
209 
210  virtual int nextOutOfCorePoint( Point3D<float>& p )=0;
211  virtual int nextPolygon( std::vector< CoredVertexIndex >& vertices ) = 0;
212 
213  virtual int outOfCorePointCount(void)=0;
214  virtual int polygonCount( void ) = 0;
215  };
216  // Stores the iso-span of each vertex, rather than it's position
217  class PCL_EXPORTS CoredMeshData2
218  {
219  public:
220  struct Vertex
221  {
223  float value;
224  Vertex( void ) { ; }
225  Vertex( Point3D< float > s , Point3D< float > e , float v ) { start = s , end = e , value = v; }
227  {
228  start = s , end = e;
229  // < p , e-s > = < s + v*(e-s) , e-s >
230  // < p , e-s > - < s , e-s > = v || e-s || ^2
231  // v = < p-s , e-s > / || e-s ||^2
232  Point3D< float > p1 = p-s , p2 = e-s;
233  value = ( p1[0] * p2[0] + p1[1] * p2[1] + p1[2] * p2[2] ) / ( p2[0] * p2[0] + p2[1] * p2[1] + p2[2] * p2[2] );
234  }
235  };
236 
237  virtual ~CoredMeshData2() = default;
238 
239  std::vector< Vertex > inCorePoints;
240  virtual void resetIterator( void ) = 0;
241 
242  virtual int addOutOfCorePoint( const Vertex& v ) = 0;
243  virtual int addPolygon( const std::vector< CoredVertexIndex >& vertices ) = 0;
244 
245  virtual int nextOutOfCorePoint( Vertex& v ) = 0;
246  virtual int nextPolygon( std::vector< CoredVertexIndex >& vertices ) = 0;
247 
248  virtual int outOfCorePointCount( void )=0;
249  virtual int polygonCount( void ) = 0;
250  };
251 
252  class PCL_EXPORTS CoredVectorMeshData : public CoredMeshData
253  {
254  std::vector<Point3D<float> > oocPoints;
255  std::vector< std::vector< int > > polygons;
256  int polygonIndex;
257  int oocPointIndex;
258  public:
259  CoredVectorMeshData(void);
260 
261  void resetIterator(void);
262 
263  int addOutOfCorePoint( const Point3D<float>& p );
264  int addPolygon( const std::vector< CoredVertexIndex >& vertices );
265 
266  int nextOutOfCorePoint( Point3D<float>& p );
267  int nextPolygon( std::vector< CoredVertexIndex >& vertices );
268 
269  int outOfCorePointCount(void);
270  int polygonCount( void );
271  };
272  class PCL_EXPORTS CoredVectorMeshData2 : public CoredMeshData2
273  {
274  std::vector< CoredMeshData2::Vertex > oocPoints;
275  std::vector< std::vector< int > > polygons;
276  int polygonIndex;
277  int oocPointIndex;
278  public:
279  CoredVectorMeshData2( void );
280 
281  void resetIterator(void);
282 
283  int addOutOfCorePoint( const CoredMeshData2::Vertex& v );
284  int addPolygon( const std::vector< CoredVertexIndex >& vertices );
285 
286  int nextOutOfCorePoint( CoredMeshData2::Vertex& v );
287  int nextPolygon( std::vector< CoredVertexIndex >& vertices );
288 
289  int outOfCorePointCount( void );
290  int polygonCount( void );
291  };
293  {
294  FILE *oocPointFile , *polygonFile;
295  int oocPoints , polygons;
296  public:
297  CoredFileMeshData(void);
298  ~CoredFileMeshData(void);
299 
300  void resetIterator(void);
301 
302  int addOutOfCorePoint(const Point3D<float>& p);
303  int addPolygon( const std::vector< CoredVertexIndex >& vertices );
304 
306  int nextPolygon( std::vector< CoredVertexIndex >& vertices );
307 
308  int outOfCorePointCount(void);
309  int polygonCount( void );
310  };
312  {
313  FILE *oocPointFile , *polygonFile;
314  int oocPoints , polygons;
315  public:
316  CoredFileMeshData2( void );
317  ~CoredFileMeshData2( void );
318 
319  void resetIterator( void );
320 
322  int addPolygon( const std::vector< CoredVertexIndex >& vertices );
323 
325  int nextPolygon( std::vector< CoredVertexIndex >& vertices );
326 
327  int outOfCorePointCount( void );
328  int polygonCount( void );
329  };
330  }
331 }
332 
333 #include "geometry.hpp"
334 
335 
336 
337 
338 #endif // GEOMETRY_INCLUDED
int nextPolygon(std::vector< CoredVertexIndex > &vertices)
double SquareLength(const Point3D< Real > &p)
Definition: geometry.hpp:60
int addOutOfCorePoint(const CoredMeshData2::Vertex &v)
int addPolygon(const std::vector< CoredVertexIndex > &vertices)
int flipMinimize(int eIndex)
Definition: geometry.hpp:375
Vertex(Point3D< float > s, Point3D< float > e, float v)
Definition: geometry.h:225
Point3D & operator-=(Point3D p)
Definition: geometry.h:57
int addTriangle(int p1, int p2, int p3)
Definition: geometry.hpp:336
int addOutOfCorePoint(const Point3D< float > &p)
double Length(const Point3D< Real > &p)
Definition: geometry.hpp:63
void CrossProduct(const Point3D< Real > &p1, const Point3D< Real > &p2, Point3D< Real > &p)
Definition: geometry.hpp:74
CoredPointIndex idx[2]
Definition: geometry.h:144
std::vector< TriangulationTriangle > triangles
Definition: geometry.h:173
int nextPolygon(std::vector< CoredVertexIndex > &vertices)
double Area(void) const
Definition: geometry.h:104
Point3D< Real > RandomBallPoint(void)
Definition: geometry.hpp:39
int nextOutOfCorePoint(CoredMeshData2::Vertex &v)
int nextOutOfCorePoint(Point3D< float > &p)
Point3D operator-(Point3D p) const
Definition: geometry.h:61
Point3D & operator*=(Real r)
Definition: geometry.h:58
Vertex(Point3D< float > s, Point3D< float > e, Point3D< float > p)
Definition: geometry.h:226
std::unordered_map< long long, int > edgeMap
Definition: geometry.h:183
static long long EdgeIndex(int p1, int p2)
Definition: geometry.hpp:292
void TriangleCollapse(const Real &edgeRatio, std::vector< TriangleIndex > &triangles, std::vector< Point3D< Real > > &positions, std::vector< Point3D< Real > > *normals)
Definition: geometry.hpp:177
Point3D & operator+=(Point3D p)
Definition: geometry.h:56
Point3D & operator/=(Real r)
Definition: geometry.h:59
double p[3][3]
Definition: geometry.h:103
double Distance(const Point3D< Real > &p1, const Point3D< Real > &p2)
Definition: geometry.hpp:71
std::vector< Point3D< Real > > points
Definition: geometry.h:171
Point3D operator/(Real r) const
Definition: geometry.h:63
std::vector< TriangulationEdge > edges
Definition: geometry.h:172
double AspectRatio(void) const
Definition: geometry.h:117
Real Random(void)
Definition: geometry.hpp:36
bool operator==(const PCLHeader &lhs, const PCLHeader &rhs)
Definition: PCLHeader.h:37
int addPolygon(const std::vector< CoredVertexIndex > &vertices)
int factor(int tIndex, int &p1, int &p2, int &p3)
Definition: geometry.hpp:299
double Length(void) const
Definition: geometry.h:91
Point3D operator*(Real r) const
Definition: geometry.h:62
void EdgeCollapse(const Real &edgeRatio, std::vector< TriangleIndex > &triangles, std::vector< Point3D< Real > > &positions, std::vector< Point3D< Real > > *normals)
Definition: geometry.hpp:81
Point3D< Real > RandomSpherePoint(void)
Definition: geometry.hpp:50
std::vector< Vertex > inCorePoints
Definition: geometry.h:239
double p[2][2]
Definition: geometry.h:90
Real & operator[](int i)
Definition: geometry.h:54
std::vector< Point3D< float > > inCorePoints
Definition: geometry.h:202
Point3D operator+(Point3D p) const
Definition: geometry.h:60
double SquareDistance(const Point3D< Real > &p1, const Point3D< Real > &p2)
Definition: geometry.hpp:66