pcl: geometry.hpp Source File

Go to the documentation of this file.
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 
00289     // Triangulation //
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 }