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 #include <pcl/console/print.h> 00030 00031 namespace pcl 00032 { 00033 namespace poisson 00034 { 00035 00036 00037 template<class Real> 00038 Real Random(void){return Real(rand())/RAND_MAX;} 00039 00040 template<class Real> 00041 Point3D<Real> RandomBallPoint(void){ 00042 Point3D<Real> p; 00043 while(1){ 00044 p.coords[0]=Real(1.0-2.0*Random<Real>()); 00045 p.coords[1]=Real(1.0-2.0*Random<Real>()); 00046 p.coords[2]=Real(1.0-2.0*Random<Real>()); 00047 double l=SquareLength(p); 00048 if(l<=1){return p;} 00049 } 00050 } 00051 template<class Real> 00052 Point3D<Real> RandomSpherePoint(void){ 00053 Point3D<Real> p=RandomBallPoint<Real>(); 00054 Real l=Real(Length(p)); 00055 p.coords[0]/=l; 00056 p.coords[1]/=l; 00057 p.coords[2]/=l; 00058 return p; 00059 } 00060 00061 template<class Real> 00062 double SquareLength(const Point3D<Real>& p){return p.coords[0]*p.coords[0]+p.coords[1]*p.coords[1]+p.coords[2]*p.coords[2];} 00063 00064 template<class Real> 00065 double Length(const Point3D<Real>& p){return sqrt(SquareLength(p));} 00066 00067 template<class Real> 00068 double SquareDistance(const Point3D<Real>& p1,const Point3D<Real>& p2){ 00069 return (p1.coords[0]-p2.coords[0])*(p1.coords[0]-p2.coords[0])+(p1.coords[1]-p2.coords[1])*(p1.coords[1]-p2.coords[1])+(p1.coords[2]-p2.coords[2])*(p1.coords[2]-p2.coords[2]); 00070 } 00071 00072 template<class Real> 00073 double Distance(const Point3D<Real>& p1,const Point3D<Real>& p2){return sqrt(SquareDistance(p1,p2));} 00074 00075 template <class Real> 00076 void CrossProduct(const Point3D<Real>& p1,const Point3D<Real>& p2,Point3D<Real>& p){ 00077 p.coords[0]= p1.coords[1]*p2.coords[2]-p1.coords[2]*p2.coords[1]; 00078 p.coords[1]=-p1.coords[0]*p2.coords[2]+p1.coords[2]*p2.coords[0]; 00079 p.coords[2]= p1.coords[0]*p2.coords[1]-p1.coords[1]*p2.coords[0]; 00080 } 00081 template<class Real> 00082 void EdgeCollapse(const Real& edgeRatio,std::vector<TriangleIndex>& triangles,std::vector< Point3D<Real> >& positions,std::vector< Point3D<Real> >* normals){ 00083 int i,j,*remapTable,*pointCount,idx[3]; 00084 Point3D<Real> p[3],q[2],c; 00085 double d[3],a; 00086 double Ratio=12.0/sqrt(3.0); // (Sum of Squares Length / Area) for and equilateral triangle 00087 00088 remapTable=new int[positions.size()]; 00089 pointCount=new int[positions.size()]; 00090 for(i=0;i<int(positions.size());i++){ 00091 remapTable[i]=i; 00092 pointCount[i]=1; 00093 } 00094 for(i=int(triangles.size()-1);i>=0;i--){ 00095 for(j=0;j<3;j++){ 00096 idx[j]=triangles[i].idx[j]; 00097 while(remapTable[idx[j]]<idx[j]){idx[j]=remapTable[idx[j]];} 00098 } 00099 if(idx[0]==idx[1] || idx[0]==idx[2] || idx[1]==idx[2]){ 00100 triangles[i]=triangles[triangles.size()-1]; 00101 triangles.pop_back(); 00102 continue; 00103 } 00104 for(j=0;j<3;j++){ 00105 p[j].coords[0]=positions[idx[j]].coords[0]/pointCount[idx[j]]; 00106 p[j].coords[1]=positions[idx[j]].coords[1]/pointCount[idx[j]]; 00107 p[j].coords[2]=positions[idx[j]].coords[2]/pointCount[idx[j]]; 00108 } 00109 for(j=0;j<3;j++){ 00110 q[0].coords[j]=p[1].coords[j]-p[0].coords[j]; 00111 q[1].coords[j]=p[2].coords[j]-p[0].coords[j]; 00112 d[j]=SquareDistance(p[j],p[(j+1)%3]); 00113 } 00114 CrossProduct(q[0],q[1],c); 00115 a=Length(c)/2; 00116 00117 if((d[0]+d[1]+d[2])*edgeRatio > a*Ratio){ 00118 // Find the smallest edge 00119 j=0; 00120 if(d[1]<d[j]){j=1;} 00121 if(d[2]<d[j]){j=2;} 00122 00123 int idx1,idx2; 00124 if(idx[j]<idx[(j+1)%3]){ 00125 idx1=idx[j]; 00126 idx2=idx[(j+1)%3]; 00127 } 00128 else{ 00129 idx2=idx[j]; 00130 idx1=idx[(j+1)%3]; 00131 } 00132 positions[idx1].coords[0]+=positions[idx2].coords[0]; 00133 positions[idx1].coords[1]+=positions[idx2].coords[1]; 00134 positions[idx1].coords[2]+=positions[idx2].coords[2]; 00135 if(normals){ 00136 (*normals)[idx1].coords[0]+=(*normals)[idx2].coords[0]; 00137 (*normals)[idx1].coords[1]+=(*normals)[idx2].coords[1]; 00138 (*normals)[idx1].coords[2]+=(*normals)[idx2].coords[2]; 00139 } 00140 pointCount[idx1]+=pointCount[idx2]; 00141 remapTable[idx2]=idx1; 00142 triangles[i]=triangles[triangles.size()-1]; 00143 triangles.pop_back(); 00144 } 00145 } 00146 int pCount=0; 00147 for(i=0;i<int(positions.size());i++){ 00148 for(j=0;j<3;j++){positions[i].coords[j]/=pointCount[i];} 00149 if(normals){ 00150 Real l=Real(Length((*normals)[i])); 00151 for(j=0;j<3;j++){(*normals)[i].coords[j]/=l;} 00152 } 00153 if(remapTable[i]==i){ // If vertex i is being used 00154 positions[pCount]=positions[i]; 00155 if(normals){(*normals)[pCount]=(*normals)[i];} 00156 pointCount[i]=pCount; 00157 pCount++; 00158 } 00159 } 00160 positions.resize(pCount); 00161 for(i=int(triangles.size()-1);i>=0;i--){ 00162 for(j=0;j<3;j++){ 00163 idx[j]=triangles[i].idx[j]; 00164 while(remapTable[idx[j]]<idx[j]){idx[j]=remapTable[idx[j]];} 00165 triangles[i].idx[j]=pointCount[idx[j]]; 00166 } 00167 if(idx[0]==idx[1] || idx[0]==idx[2] || idx[1]==idx[2]){ 00168 triangles[i]=triangles[triangles.size()-1]; 00169 triangles.pop_back(); 00170 } 00171 } 00172 00173 delete[] pointCount; 00174 delete[] remapTable; 00175 } 00176 template<class Real> 00177 void TriangleCollapse(const Real& edgeRatio,std::vector<TriangleIndex>& triangles,std::vector< Point3D<Real> >& positions,std::vector< Point3D<Real> >* normals){ 00178 int i,j,*remapTable,*pointCount,idx[3]; 00179 Point3D<Real> p[3],q[2],c; 00180 double d[3],a; 00181 double Ratio=12.0/sqrt(3.0); // (Sum of Squares Length / Area) for and equilateral triangle 00182 00183 remapTable=new int[positions.size()]; 00184 pointCount=new int[positions.size()]; 00185 for(i=0;i<int(positions.size());i++){ 00186 remapTable[i]=i; 00187 pointCount[i]=1; 00188 } 00189 for(i=int(triangles.size()-1);i>=0;i--){ 00190 for(j=0;j<3;j++){ 00191 idx[j]=triangles[i].idx[j]; 00192 while(remapTable[idx[j]]<idx[j]){idx[j]=remapTable[idx[j]];} 00193 } 00194 if(idx[0]==idx[1] || idx[0]==idx[2] || idx[1]==idx[2]){ 00195 triangles[i]=triangles[triangles.size()-1]; 00196 triangles.pop_back(); 00197 continue; 00198 } 00199 for(j=0;j<3;j++){ 00200 p[j].coords[0]=positions[idx[j]].coords[0]/pointCount[idx[j]]; 00201 p[j].coords[1]=positions[idx[j]].coords[1]/pointCount[idx[j]]; 00202 p[j].coords[2]=positions[idx[j]].coords[2]/pointCount[idx[j]]; 00203 } 00204 for(j=0;j<3;j++){ 00205 q[0].coords[j]=p[1].coords[j]-p[0].coords[j]; 00206 q[1].coords[j]=p[2].coords[j]-p[0].coords[j]; 00207 d[j]=SquareDistance(p[j],p[(j+1)%3]); 00208 } 00209 CrossProduct(q[0],q[1],c); 00210 a=Length(c)/2; 00211 00212 if((d[0]+d[1]+d[2])*edgeRatio > a*Ratio){ 00213 // Find the smallest edge 00214 j=0; 00215 if(d[1]<d[j]){j=1;} 00216 if(d[2]<d[j]){j=2;} 00217 00218 int idx1,idx2,idx3; 00219 if(idx[0]<idx[1]){ 00220 if(idx[0]<idx[2]){ 00221 idx1=idx[0]; 00222 idx2=idx[2]; 00223 idx3=idx[1]; 00224 } 00225 else{ 00226 idx1=idx[2]; 00227 idx2=idx[0]; 00228 idx3=idx[1]; 00229 } 00230 } 00231 else{ 00232 if(idx[1]<idx[2]){ 00233 idx1=idx[1]; 00234 idx2=idx[2]; 00235 idx3=idx[0]; 00236 } 00237 else{ 00238 idx1=idx[2]; 00239 idx2=idx[1]; 00240 idx3=idx[0]; 00241 } 00242 } 00243 positions[idx1].coords[0]+=positions[idx2].coords[0]+positions[idx3].coords[0]; 00244 positions[idx1].coords[1]+=positions[idx2].coords[1]+positions[idx3].coords[1]; 00245 positions[idx1].coords[2]+=positions[idx2].coords[2]+positions[idx3].coords[2]; 00246 if(normals){ 00247 (*normals)[idx1].coords[0]+=(*normals)[idx2].coords[0]+(*normals)[idx3].coords[0]; 00248 (*normals)[idx1].coords[1]+=(*normals)[idx2].coords[1]+(*normals)[idx3].coords[1]; 00249 (*normals)[idx1].coords[2]+=(*normals)[idx2].coords[2]+(*normals)[idx3].coords[2]; 00250 } 00251 pointCount[idx1]+=pointCount[idx2]+pointCount[idx3]; 00252 remapTable[idx2]=idx1; 00253 remapTable[idx3]=idx1; 00254 triangles[i]=triangles[triangles.size()-1]; 00255 triangles.pop_back(); 00256 } 00257 } 00258 int pCount=0; 00259 for(i=0;i<int(positions.size());i++){ 00260 for(j=0;j<3;j++){positions[i].coords[j]/=pointCount[i];} 00261 if(normals){ 00262 Real l=Real(Length((*normals)[i])); 00263 for(j=0;j<3;j++){(*normals)[i].coords[j]/=l;} 00264 } 00265 if(remapTable[i]==i){ // If vertex i is being used 00266 positions[pCount]=positions[i]; 00267 if(normals){(*normals)[pCount]=(*normals)[i];} 00268 pointCount[i]=pCount; 00269 pCount++; 00270 } 00271 } 00272 positions.resize(pCount); 00273 for(i=int(triangles.size()-1);i>=0;i--){ 00274 for(j=0;j<3;j++){ 00275 idx[j]=triangles[i].idx[j]; 00276 while(remapTable[idx[j]]<idx[j]){idx[j]=remapTable[idx[j]];} 00277 triangles[i].idx[j]=pointCount[idx[j]]; 00278 } 00279 if(idx[0]==idx[1] || idx[0]==idx[2] || idx[1]==idx[2]){ 00280 triangles[i]=triangles[triangles.size()-1]; 00281 triangles.pop_back(); 00282 } 00283 } 00284 delete[] pointCount; 00285 delete[] remapTable; 00286 } 00287 00288 /////////////////// 00289 // Triangulation // 00290 /////////////////// 00291 template<class Real> 00292 long long Triangulation<Real>::EdgeIndex( int p1 , int p2 ) 00293 { 00294 if(p1>p2) {return ((long long)(p1)<<32) | ((long long)(p2));} 00295 else {return ((long long)(p2)<<32) | ((long long)(p1));} 00296 } 00297 00298 template<class Real> 00299 int Triangulation<Real>::factor(int tIndex,int& p1,int& p2,int & p3){ 00300 if(triangles[tIndex].eIndex[0]<0 || triangles[tIndex].eIndex[1]<0 || triangles[tIndex].eIndex[2]<0){return 0;} 00301 if(edges[triangles[tIndex].eIndex[0]].tIndex[0]==tIndex){p1=edges[triangles[tIndex].eIndex[0]].pIndex[0];} 00302 else {p1=edges[triangles[tIndex].eIndex[0]].pIndex[1];} 00303 if(edges[triangles[tIndex].eIndex[1]].tIndex[0]==tIndex){p2=edges[triangles[tIndex].eIndex[1]].pIndex[0];} 00304 else {p2=edges[triangles[tIndex].eIndex[1]].pIndex[1];} 00305 if(edges[triangles[tIndex].eIndex[2]].tIndex[0]==tIndex){p3=edges[triangles[tIndex].eIndex[2]].pIndex[0];} 00306 else {p3=edges[triangles[tIndex].eIndex[2]].pIndex[1];} 00307 return 1; 00308 } 00309 template<class Real> 00310 double Triangulation<Real>::area(int p1,int p2,int p3){ 00311 Point3D<Real> q1,q2,q; 00312 for(int i=0;i<3;i++){ 00313 q1.coords[i]=points[p2].coords[i]-points[p1].coords[i]; 00314 q2.coords[i]=points[p3].coords[i]-points[p1].coords[i]; 00315 } 00316 CrossProduct(q1,q2,q); 00317 return Length(q); 00318 } 00319 template<class Real> 00320 double Triangulation<Real>::area(int tIndex){ 00321 int p1,p2,p3; 00322 factor(tIndex,p1,p2,p3); 00323 return area(p1,p2,p3); 00324 } 00325 template<class Real> 00326 double Triangulation<Real>::area(void){ 00327 double a=0; 00328 for(int i=0;i<int(triangles.size());i++){a+=area(i);} 00329 return a; 00330 } 00331 template<class Real> 00332 int Triangulation<Real>::addTriangle(int p1,int p2,int p3){ 00333 hash_map<long long,int>::iterator iter; 00334 int tIdx,eIdx,p[3]; 00335 p[0]=p1; 00336 p[1]=p2; 00337 p[2]=p3; 00338 triangles.push_back(TriangulationTriangle()); 00339 tIdx=int(triangles.size())-1; 00340 00341 for(int i=0;i<3;i++) 00342 { 00343 long long e = EdgeIndex(p[i],p[(i+1)%3]); 00344 iter=edgeMap.find(e); 00345 if(iter==edgeMap.end()) 00346 { 00347 TriangulationEdge edge; 00348 edge.pIndex[0]=p[i]; 00349 edge.pIndex[1]=p[(i+1)%3]; 00350 edges.push_back(edge); 00351 eIdx=int(edges.size())-1; 00352 edgeMap[e]=eIdx; 00353 edges[eIdx].tIndex[0]=tIdx; 00354 } 00355 else{ 00356 eIdx=edgeMap[e]; 00357 if(edges[eIdx].pIndex[0]==p[i]){ 00358 if(edges[eIdx].tIndex[0]<0){edges[eIdx].tIndex[0]=tIdx;} 00359 else{PCL_DEBUG("Edge Triangle in use 1\n");return 0;} 00360 } 00361 else{ 00362 if(edges[eIdx].tIndex[1]<0){edges[eIdx].tIndex[1]=tIdx;} 00363 else{PCL_DEBUG("Edge Triangle in use 2\n");return 0;} 00364 } 00365 00366 } 00367 triangles[tIdx].eIndex[i]=eIdx; 00368 } 00369 return tIdx; 00370 } 00371 template<class Real> 00372 int Triangulation<Real>::flipMinimize(int eIndex){ 00373 double oldArea,newArea; 00374 int oldP[3],oldQ[3],newP[3],newQ[3]; 00375 TriangulationEdge newEdge; 00376 00377 if(edges[eIndex].tIndex[0]<0 || edges[eIndex].tIndex[1]<0){return 0;} 00378 00379 if(!factor(edges[eIndex].tIndex[0],oldP[0],oldP[1],oldP[2])){return 0;} 00380 if(!factor(edges[eIndex].tIndex[1],oldQ[0],oldQ[1],oldQ[2])){return 0;} 00381 00382 oldArea=area(oldP[0],oldP[1],oldP[2])+area(oldQ[0],oldQ[1],oldQ[2]); 00383 int idxP,idxQ; 00384 for(idxP=0;idxP<3;idxP++){ 00385 int i; 00386 for(i=0;i<3;i++){if(oldP[idxP]==oldQ[i]){break;}} 00387 if(i==3){break;} 00388 } 00389 for(idxQ=0;idxQ<3;idxQ++){ 00390 int i; 00391 for(i=0;i<3;i++){if(oldP[i]==oldQ[idxQ]){break;}} 00392 if(i==3){break;} 00393 } 00394 if(idxP==3 || idxQ==3){return 0;} 00395 newP[0]=oldP[idxP]; 00396 newP[1]=oldP[(idxP+1)%3]; 00397 newP[2]=oldQ[idxQ]; 00398 newQ[0]=oldQ[idxQ]; 00399 newQ[1]=oldP[(idxP+2)%3]; 00400 newQ[2]=oldP[idxP]; 00401 00402 newArea=area(newP[0],newP[1],newP[2])+area(newQ[0],newQ[1],newQ[2]); 00403 if(oldArea<=newArea){return 0;} 00404 00405 // Remove the entry in the hash_table for the old edge 00406 edgeMap.erase(EdgeIndex(edges[eIndex].pIndex[0],edges[eIndex].pIndex[1])); 00407 // Set the new edge so that the zero-side is newQ 00408 edges[eIndex].pIndex[0]=newP[0]; 00409 edges[eIndex].pIndex[1]=newQ[0]; 00410 // Insert the entry into the hash_table for the new edge 00411 edgeMap[EdgeIndex(newP[0],newQ[0])]=eIndex; 00412 // Update the triangle information 00413 for(int i=0;i<3;i++){ 00414 int idx; 00415 idx=edgeMap[EdgeIndex(newQ[i],newQ[(i+1)%3])]; 00416 triangles[edges[eIndex].tIndex[0]].eIndex[i]=idx; 00417 if(idx!=eIndex){ 00418 if(edges[idx].tIndex[0]==edges[eIndex].tIndex[1]){edges[idx].tIndex[0]=edges[eIndex].tIndex[0];} 00419 if(edges[idx].tIndex[1]==edges[eIndex].tIndex[1]){edges[idx].tIndex[1]=edges[eIndex].tIndex[0];} 00420 } 00421 00422 idx=edgeMap[EdgeIndex(newP[i],newP[(i+1)%3])]; 00423 triangles[edges[eIndex].tIndex[1]].eIndex[i]=idx; 00424 if(idx!=eIndex){ 00425 if(edges[idx].tIndex[0]==edges[eIndex].tIndex[0]){edges[idx].tIndex[0]=edges[eIndex].tIndex[1];} 00426 if(edges[idx].tIndex[1]==edges[eIndex].tIndex[0]){edges[idx].tIndex[1]=edges[eIndex].tIndex[1];} 00427 } 00428 } 00429 return 1; 00430 } 00431 00432 } 00433 }