Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho 00003 All rights reserved. 00004 00005 Redistribution and use in source and binary forms, with or without modification, 00006 are permitted provided that the following conditions are met: 00007 00008 Redistributions of source code must retain the above copyright notice, this list of 00009 conditions and the following disclaimer. Redistributions in binary form must reproduce 00010 the above copyright notice, this list of conditions and the following disclaimer 00011 in the documentation and/or other materials provided with the distribution. 00012 00013 Neither the name of the Johns Hopkins University nor the names of its contributors 00014 may be used to endorse or promote products derived from this software without specific 00015 prior written permission. 00016 00017 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY 00018 EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO THE IMPLIED WARRANTIES 00019 OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT 00020 SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00021 INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 00022 TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR 00023 BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00024 CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00025 ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 00026 DAMAGE. 00027 */ 00028 00029 #ifndef GEOMETRY_INCLUDED 00030 #define GEOMETRY_INCLUDED 00031 00032 #if defined __GNUC__ 00033 # pragma GCC system_header 00034 #endif 00035 00036 #include <pcl/pcl_macros.h> 00037 #include <math.h> 00038 #include <vector> 00039 #include "hash.h" 00040 00041 namespace pcl 00042 { 00043 namespace poisson 00044 { 00045 00046 template<class Real> 00047 Real Random(void); 00048 00049 template< class Real > 00050 struct Point3D 00051 { 00052 Real coords[3]; 00053 Point3D( void ) { coords[0] = coords[1] = coords[2] = Real(0); } 00054 inline Real& operator[] ( int i ) { return coords[i]; } 00055 inline const Real& operator[] ( int i ) const { return coords[i]; } 00056 inline Point3D& operator += ( Point3D p ){ coords[0] += p.coords[0] , coords[1] += p.coords[1] , coords[2] += p.coords[2] ; return *this; } 00057 inline Point3D& operator -= ( Point3D p ){ coords[0] -= p.coords[0] , coords[1] -= p.coords[1] , coords[2] -= p.coords[2] ; return *this; } 00058 inline Point3D& operator *= ( Real r ){ coords[0] *= r , coords[1] *= r , coords[2] *= r ; return *this; } 00059 inline Point3D& operator /= ( Real r ){ coords[0] /= r , coords[1] /= r , coords[2] /= r ; return *this; } 00060 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; } 00061 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; } 00062 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; } 00063 inline Point3D operator / ( Real r ) const { return (*this) * ( Real(1.)/r ); } 00064 }; 00065 00066 template<class Real> 00067 Point3D<Real> RandomBallPoint(void); 00068 00069 template<class Real> 00070 Point3D<Real> RandomSpherePoint(void); 00071 00072 template<class Real> 00073 double Length(const Point3D<Real>& p); 00074 00075 template<class Real> 00076 double SquareLength(const Point3D<Real>& p); 00077 00078 template<class Real> 00079 double Distance(const Point3D<Real>& p1,const Point3D<Real>& p2); 00080 00081 template<class Real> 00082 double SquareDistance(const Point3D<Real>& p1,const Point3D<Real>& p2); 00083 00084 template <class Real> 00085 void CrossProduct(const Point3D<Real>& p1,const Point3D<Real>& p2,Point3D<Real>& p); 00086 00087 class Edge 00088 { 00089 public: 00090 double p[2][2]; 00091 double Length(void) const 00092 { 00093 double d[2]; 00094 d[0]=p[0][0]-p[1][0]; 00095 d[1]=p[0][1]-p[1][1]; 00096 00097 return sqrt(d[0]*d[0]+d[1]*d[1]); 00098 } 00099 }; 00100 class Triangle 00101 { 00102 public: 00103 double p[3][3]; 00104 double Area(void) const 00105 { 00106 double v1[3] , v2[3] , v[3]; 00107 for( int d=0 ; d<3 ; d++ ) 00108 { 00109 v1[d] = p[1][d] - p[0][d]; 00110 v2[d] = p[2][d] - p[0][d]; 00111 } 00112 v[0] = v1[1]*v2[2] - v1[2]*v2[1]; 00113 v[1] = -v1[0]*v2[2] + v1[2]*v2[0]; 00114 v[2] = v1[0]*v2[1] - v1[1]*v2[0]; 00115 return sqrt( v[0]*v[0] + v[1]*v[1] + v[2]*v[2] ) / 2; 00116 } 00117 double AspectRatio(void) const 00118 { 00119 double d=0; 00120 int i,j; 00121 for(i=0;i<3;i++){ 00122 for(i=0;i<3;i++) 00123 for(j=0;j<3;j++){d+=(p[(i+1)%3][j]-p[i][j])*(p[(i+1)%3][j]-p[i][j]);} 00124 } 00125 return Area()/d; 00126 } 00127 00128 }; 00129 class PCL_EXPORTS CoredPointIndex 00130 { 00131 public: 00132 int index; 00133 char inCore; 00134 00135 int operator == (const CoredPointIndex& cpi) const {return (index==cpi.index) && (inCore==cpi.inCore);}; 00136 int operator != (const CoredPointIndex& cpi) const {return (index!=cpi.index) || (inCore!=cpi.inCore);}; 00137 }; 00138 class EdgeIndex{ 00139 public: 00140 int idx[2]; 00141 }; 00142 class CoredEdgeIndex{ 00143 public: 00144 CoredPointIndex idx[2]; 00145 }; 00146 class TriangleIndex{ 00147 public: 00148 int idx[3]; 00149 }; 00150 00151 class TriangulationEdge 00152 { 00153 public: 00154 TriangulationEdge(void); 00155 int pIndex[2]; 00156 int tIndex[2]; 00157 }; 00158 00159 class TriangulationTriangle 00160 { 00161 public: 00162 TriangulationTriangle(void); 00163 int eIndex[3]; 00164 }; 00165 00166 template<class Real> 00167 class Triangulation 00168 { 00169 public: 00170 00171 std::vector<Point3D<Real> > points; 00172 std::vector<TriangulationEdge> edges; 00173 std::vector<TriangulationTriangle> triangles; 00174 00175 int factor( int tIndex,int& p1,int& p2,int& p3); 00176 double area(void); 00177 double area( int tIndex ); 00178 double area( int p1 , int p2 , int p3 ); 00179 int flipMinimize( int eIndex); 00180 int addTriangle( int p1 , int p2 , int p3 ); 00181 00182 protected: 00183 hash_map<long long,int> edgeMap; 00184 static long long EdgeIndex( int p1 , int p2 ); 00185 double area(const Triangle& t); 00186 }; 00187 00188 00189 template<class Real> 00190 void EdgeCollapse(const Real& edgeRatio,std::vector<TriangleIndex>& triangles,std::vector< Point3D<Real> >& positions,std::vector<Point3D<Real> >* normals); 00191 template<class Real> 00192 void TriangleCollapse(const Real& edgeRatio,std::vector<TriangleIndex>& triangles,std::vector<Point3D<Real> >& positions,std::vector<Point3D<Real> >* normals); 00193 00194 struct CoredVertexIndex 00195 { 00196 int idx; 00197 bool inCore; 00198 }; 00199 class PCL_EXPORTS CoredMeshData 00200 { 00201 public: 00202 std::vector<Point3D<float> > inCorePoints; 00203 virtual void resetIterator( void ) = 0; 00204 00205 virtual int addOutOfCorePoint( const Point3D<float>& p ) = 0; 00206 virtual int addPolygon( const std::vector< CoredVertexIndex >& vertices ) = 0; 00207 00208 virtual int nextOutOfCorePoint( Point3D<float>& p )=0; 00209 virtual int nextPolygon( std::vector< CoredVertexIndex >& vertices ) = 0; 00210 00211 virtual int outOfCorePointCount(void)=0; 00212 virtual int polygonCount( void ) = 0; 00213 }; 00214 // Stores the iso-span of each vertex, rather than it's position 00215 class PCL_EXPORTS CoredMeshData2 00216 { 00217 public: 00218 struct Vertex 00219 { 00220 Point3D< float > start , end; 00221 float value; 00222 Vertex( void ) { ; } 00223 Vertex( Point3D< float > s , Point3D< float > e , float v ) { start = s , end = e , value = v; } 00224 Vertex( Point3D< float > s , Point3D< float > e , Point3D< float > p ) 00225 { 00226 start = s , end = e; 00227 // < p , e-s > = < s + v*(e-s) , e-s > 00228 // < p , e-s > - < s , e-s > = v || e-s || ^2 00229 // v = < p-s , e-s > / || e-s ||^2 00230 Point3D< float > p1 = p-s , p2 = e-s; 00231 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] ); 00232 } 00233 }; 00234 std::vector< Vertex > inCorePoints; 00235 virtual void resetIterator( void ) = 0; 00236 00237 virtual int addOutOfCorePoint( const Vertex& v ) = 0; 00238 virtual int addPolygon( const std::vector< CoredVertexIndex >& vertices ) = 0; 00239 00240 virtual int nextOutOfCorePoint( Vertex& v ) = 0; 00241 virtual int nextPolygon( std::vector< CoredVertexIndex >& vertices ) = 0; 00242 00243 virtual int outOfCorePointCount( void )=0; 00244 virtual int polygonCount( void ) = 0; 00245 }; 00246 00247 class PCL_EXPORTS CoredVectorMeshData : public CoredMeshData 00248 { 00249 std::vector<Point3D<float> > oocPoints; 00250 std::vector< std::vector< int > > polygons; 00251 int polygonIndex; 00252 int oocPointIndex; 00253 public: 00254 CoredVectorMeshData(void); 00255 00256 void resetIterator(void); 00257 00258 int addOutOfCorePoint( const Point3D<float>& p ); 00259 int addPolygon( const std::vector< CoredVertexIndex >& vertices ); 00260 00261 int nextOutOfCorePoint( Point3D<float>& p ); 00262 int nextPolygon( std::vector< CoredVertexIndex >& vertices ); 00263 00264 int outOfCorePointCount(void); 00265 int polygonCount( void ); 00266 }; 00267 class PCL_EXPORTS CoredVectorMeshData2 : public CoredMeshData2 00268 { 00269 std::vector< CoredMeshData2::Vertex > oocPoints; 00270 std::vector< std::vector< int > > polygons; 00271 int polygonIndex; 00272 int oocPointIndex; 00273 public: 00274 CoredVectorMeshData2( void ); 00275 00276 void resetIterator(void); 00277 00278 int addOutOfCorePoint( const CoredMeshData2::Vertex& v ); 00279 int addPolygon( const std::vector< CoredVertexIndex >& vertices ); 00280 00281 int nextOutOfCorePoint( CoredMeshData2::Vertex& v ); 00282 int nextPolygon( std::vector< CoredVertexIndex >& vertices ); 00283 00284 int outOfCorePointCount( void ); 00285 int polygonCount( void ); 00286 }; 00287 class CoredFileMeshData : public CoredMeshData 00288 { 00289 FILE *oocPointFile , *polygonFile; 00290 int oocPoints , polygons; 00291 public: 00292 CoredFileMeshData(void); 00293 ~CoredFileMeshData(void); 00294 00295 void resetIterator(void); 00296 00297 int addOutOfCorePoint(const Point3D<float>& p); 00298 int addPolygon( const std::vector< CoredVertexIndex >& vertices ); 00299 00300 int nextOutOfCorePoint(Point3D<float>& p); 00301 int nextPolygon( std::vector< CoredVertexIndex >& vertices ); 00302 00303 int outOfCorePointCount(void); 00304 int polygonCount( void ); 00305 }; 00306 class CoredFileMeshData2 : public CoredMeshData2 00307 { 00308 FILE *oocPointFile , *polygonFile; 00309 int oocPoints , polygons; 00310 public: 00311 CoredFileMeshData2( void ); 00312 ~CoredFileMeshData2( void ); 00313 00314 void resetIterator( void ); 00315 00316 int addOutOfCorePoint( const CoredMeshData2::Vertex& v ); 00317 int addPolygon( const std::vector< CoredVertexIndex >& vertices ); 00318 00319 int nextOutOfCorePoint( CoredMeshData2::Vertex& v ); 00320 int nextPolygon( std::vector< CoredVertexIndex >& vertices ); 00321 00322 int outOfCorePointCount( void ); 00323 int polygonCount( void ); 00324 }; 00325 } 00326 } 00327 00328 #include "geometry.hpp" 00329 00330 00331 00332 00333 #endif // GEOMETRY_INCLUDED