Point Cloud Library (PCL)  1.9.1-dev
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>
68 
69  template<class Real>
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  virtual void resetIterator( void ) = 0;
204 
205  virtual int addOutOfCorePoint( const Point3D<float>& p ) = 0;
206  virtual int addPolygon( const std::vector< CoredVertexIndex >& vertices ) = 0;
207 
208  virtual int nextOutOfCorePoint( Point3D<float>& p )=0;
209  virtual int nextPolygon( std::vector< CoredVertexIndex >& vertices ) = 0;
210 
211  virtual int outOfCorePointCount(void)=0;
212  virtual int polygonCount( void ) = 0;
213  };
214  // Stores the iso-span of each vertex, rather than it's position
215  class PCL_EXPORTS CoredMeshData2
216  {
217  public:
218  struct Vertex
219  {
221  float value;
222  Vertex( void ) { ; }
223  Vertex( Point3D< float > s , Point3D< float > e , float v ) { start = s , end = e , value = v; }
225  {
226  start = s , end = e;
227  // < p , e-s > = < s + v*(e-s) , e-s >
228  // < p , e-s > - < s , e-s > = v || e-s || ^2
229  // v = < p-s , e-s > / || e-s ||^2
230  Point3D< float > p1 = p-s , p2 = e-s;
231  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] );
232  }
233  };
234  std::vector< Vertex > inCorePoints;
235  virtual void resetIterator( void ) = 0;
236 
237  virtual int addOutOfCorePoint( const Vertex& v ) = 0;
238  virtual int addPolygon( const std::vector< CoredVertexIndex >& vertices ) = 0;
239 
240  virtual int nextOutOfCorePoint( Vertex& v ) = 0;
241  virtual int nextPolygon( std::vector< CoredVertexIndex >& vertices ) = 0;
242 
243  virtual int outOfCorePointCount( void )=0;
244  virtual int polygonCount( void ) = 0;
245  };
246 
247  class PCL_EXPORTS CoredVectorMeshData : public CoredMeshData
248  {
249  std::vector<Point3D<float> > oocPoints;
250  std::vector< std::vector< int > > polygons;
251  int polygonIndex;
252  int oocPointIndex;
253  public:
254  CoredVectorMeshData(void);
255 
256  void resetIterator(void);
257 
258  int addOutOfCorePoint( const Point3D<float>& p );
259  int addPolygon( const std::vector< CoredVertexIndex >& vertices );
260 
261  int nextOutOfCorePoint( Point3D<float>& p );
262  int nextPolygon( std::vector< CoredVertexIndex >& vertices );
263 
264  int outOfCorePointCount(void);
265  int polygonCount( void );
266  };
267  class PCL_EXPORTS CoredVectorMeshData2 : public CoredMeshData2
268  {
269  std::vector< CoredMeshData2::Vertex > oocPoints;
270  std::vector< std::vector< int > > polygons;
271  int polygonIndex;
272  int oocPointIndex;
273  public:
274  CoredVectorMeshData2( void );
275 
276  void resetIterator(void);
277 
278  int addOutOfCorePoint( const CoredMeshData2::Vertex& v );
279  int addPolygon( const std::vector< CoredVertexIndex >& vertices );
280 
281  int nextOutOfCorePoint( CoredMeshData2::Vertex& v );
282  int nextPolygon( std::vector< CoredVertexIndex >& vertices );
283 
284  int outOfCorePointCount( void );
285  int polygonCount( void );
286  };
288  {
289  FILE *oocPointFile , *polygonFile;
290  int oocPoints , polygons;
291  public:
292  CoredFileMeshData(void);
293  ~CoredFileMeshData(void);
294 
295  void resetIterator(void);
296 
297  int addOutOfCorePoint(const Point3D<float>& p);
298  int addPolygon( const std::vector< CoredVertexIndex >& vertices );
299 
300  int nextOutOfCorePoint(Point3D<float>& p);
301  int nextPolygon( std::vector< CoredVertexIndex >& vertices );
302 
303  int outOfCorePointCount(void);
304  int polygonCount( void );
305  };
307  {
308  FILE *oocPointFile , *polygonFile;
309  int oocPoints , polygons;
310  public:
311  CoredFileMeshData2( void );
312  ~CoredFileMeshData2( void );
313 
314  void resetIterator( void );
315 
316  int addOutOfCorePoint( const CoredMeshData2::Vertex& v );
317  int addPolygon( const std::vector< CoredVertexIndex >& vertices );
318 
319  int nextOutOfCorePoint( CoredMeshData2::Vertex& v );
320  int nextPolygon( std::vector< CoredVertexIndex >& vertices );
321 
322  int outOfCorePointCount( void );
323  int polygonCount( void );
324  };
325  }
326 }
327 
328 #include "geometry.hpp"
329 
330 
331 
332 
333 #endif // GEOMETRY_INCLUDED
double SquareLength(const Point3D< Real > &p)
Definition: geometry.hpp:62
Vertex(Point3D< float > s, Point3D< float > e, float v)
Definition: geometry.h:223
Point3D & operator-=(Point3D p)
Definition: geometry.h:57
double Length(const Point3D< Real > &p)
Definition: geometry.hpp:65
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:44
void CrossProduct(const Point3D< Real > &p1, const Point3D< Real > &p2, Point3D< Real > &p)
Definition: geometry.hpp:76
std::vector< TriangulationTriangle > triangles
Definition: geometry.h:173
double Area(void) const
Definition: geometry.h:104
Point3D< Real > RandomBallPoint(void)
Definition: geometry.hpp:41
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:224
std::unordered_map< long long, int > edgeMap
Definition: geometry.h:183
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 Distance(const Point3D< Real > &p1, const Point3D< Real > &p2)
Definition: geometry.hpp:73
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:38
bool operator==(const PCLHeader &lhs, const PCLHeader &rhs)
Definition: PCLHeader.h:45
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:82
Point3D< Real > RandomSpherePoint(void)
Definition: geometry.hpp:52
std::vector< Vertex > inCorePoints
Definition: geometry.h:234
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:68