Point Cloud Library (PCL)  1.9.1-dev
geometry.hpp
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 #include <pcl/console/print.h>
30 
31 namespace pcl
32 {
33  namespace poisson
34  {
35 
36 
37  template<class Real>
38  Real Random(void){return Real(rand())/RAND_MAX;}
39 
40  template<class Real>
42  Point3D<Real> p;
43  while(1){
44  p.coords[0]=Real(1.0-2.0*Random<Real>());
45  p.coords[1]=Real(1.0-2.0*Random<Real>());
46  p.coords[2]=Real(1.0-2.0*Random<Real>());
47  double l=SquareLength(p);
48  if(l<=1){return p;}
49  }
50  }
51  template<class Real>
53  Point3D<Real> p=RandomBallPoint<Real>();
54  Real l=Real(Length(p));
55  p.coords[0]/=l;
56  p.coords[1]/=l;
57  p.coords[2]/=l;
58  return p;
59  }
60 
61  template<class Real>
62  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];}
63 
64  template<class Real>
65  double Length(const Point3D<Real>& p){return sqrt(SquareLength(p));}
66 
67  template<class Real>
68  double SquareDistance(const Point3D<Real>& p1,const Point3D<Real>& p2){
69  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]);
70  }
71 
72  template<class Real>
73  double Distance(const Point3D<Real>& p1,const Point3D<Real>& p2){return sqrt(SquareDistance(p1,p2));}
74 
75  template <class Real>
76  void CrossProduct(const Point3D<Real>& p1,const Point3D<Real>& p2,Point3D<Real>& p){
77  p.coords[0]= p1.coords[1]*p2.coords[2]-p1.coords[2]*p2.coords[1];
78  p.coords[1]=-p1.coords[0]*p2.coords[2]+p1.coords[2]*p2.coords[0];
79  p.coords[2]= p1.coords[0]*p2.coords[1]-p1.coords[1]*p2.coords[0];
80  }
81  template<class Real>
82  void EdgeCollapse(const Real& edgeRatio,std::vector<TriangleIndex>& triangles,std::vector< Point3D<Real> >& positions,std::vector< Point3D<Real> >* normals){
83  int i,j,*remapTable,*pointCount,idx[3];
84  Point3D<Real> p[3],q[2],c;
85  double d[3],a;
86  double Ratio=12.0/sqrt(3.0); // (Sum of Squares Length / Area) for and equilateral triangle
87 
88  remapTable=new int[positions.size()];
89  pointCount=new int[positions.size()];
90  for(i=0;i<int(positions.size());i++){
91  remapTable[i]=i;
92  pointCount[i]=1;
93  }
94  for(i=int(triangles.size()-1);i>=0;i--){
95  for(j=0;j<3;j++){
96  idx[j]=triangles[i].idx[j];
97  while(remapTable[idx[j]]<idx[j]){idx[j]=remapTable[idx[j]];}
98  }
99  if(idx[0]==idx[1] || idx[0]==idx[2] || idx[1]==idx[2]){
100  triangles[i]=triangles[triangles.size()-1];
101  triangles.pop_back();
102  continue;
103  }
104  for(j=0;j<3;j++){
105  p[j].coords[0]=positions[idx[j]].coords[0]/pointCount[idx[j]];
106  p[j].coords[1]=positions[idx[j]].coords[1]/pointCount[idx[j]];
107  p[j].coords[2]=positions[idx[j]].coords[2]/pointCount[idx[j]];
108  }
109  for(j=0;j<3;j++){
110  q[0].coords[j]=p[1].coords[j]-p[0].coords[j];
111  q[1].coords[j]=p[2].coords[j]-p[0].coords[j];
112  d[j]=SquareDistance(p[j],p[(j+1)%3]);
113  }
114  CrossProduct(q[0],q[1],c);
115  a=Length(c)/2;
116 
117  if((d[0]+d[1]+d[2])*edgeRatio > a*Ratio){
118  // Find the smallest edge
119  j=0;
120  if(d[1]<d[j]){j=1;}
121  if(d[2]<d[j]){j=2;}
122 
123  int idx1,idx2;
124  if(idx[j]<idx[(j+1)%3]){
125  idx1=idx[j];
126  idx2=idx[(j+1)%3];
127  }
128  else{
129  idx2=idx[j];
130  idx1=idx[(j+1)%3];
131  }
132  positions[idx1].coords[0]+=positions[idx2].coords[0];
133  positions[idx1].coords[1]+=positions[idx2].coords[1];
134  positions[idx1].coords[2]+=positions[idx2].coords[2];
135  if(normals){
136  (*normals)[idx1].coords[0]+=(*normals)[idx2].coords[0];
137  (*normals)[idx1].coords[1]+=(*normals)[idx2].coords[1];
138  (*normals)[idx1].coords[2]+=(*normals)[idx2].coords[2];
139  }
140  pointCount[idx1]+=pointCount[idx2];
141  remapTable[idx2]=idx1;
142  triangles[i]=triangles[triangles.size()-1];
143  triangles.pop_back();
144  }
145  }
146  int pCount=0;
147  for(i=0;i<int(positions.size());i++){
148  for(j=0;j<3;j++){positions[i].coords[j]/=pointCount[i];}
149  if(normals){
150  Real l=Real(Length((*normals)[i]));
151  for(j=0;j<3;j++){(*normals)[i].coords[j]/=l;}
152  }
153  if(remapTable[i]==i){ // If vertex i is being used
154  positions[pCount]=positions[i];
155  if(normals){(*normals)[pCount]=(*normals)[i];}
156  pointCount[i]=pCount;
157  pCount++;
158  }
159  }
160  positions.resize(pCount);
161  for(i=int(triangles.size()-1);i>=0;i--){
162  for(j=0;j<3;j++){
163  idx[j]=triangles[i].idx[j];
164  while(remapTable[idx[j]]<idx[j]){idx[j]=remapTable[idx[j]];}
165  triangles[i].idx[j]=pointCount[idx[j]];
166  }
167  if(idx[0]==idx[1] || idx[0]==idx[2] || idx[1]==idx[2]){
168  triangles[i]=triangles[triangles.size()-1];
169  triangles.pop_back();
170  }
171  }
172 
173  delete[] pointCount;
174  delete[] remapTable;
175  }
176  template<class Real>
177  void TriangleCollapse(const Real& edgeRatio,std::vector<TriangleIndex>& triangles,std::vector< Point3D<Real> >& positions,std::vector< Point3D<Real> >* normals){
178  int i,j,*remapTable,*pointCount,idx[3];
179  Point3D<Real> p[3],q[2],c;
180  double d[3],a;
181  double Ratio=12.0/sqrt(3.0); // (Sum of Squares Length / Area) for and equilateral triangle
182 
183  remapTable=new int[positions.size()];
184  pointCount=new int[positions.size()];
185  for(i=0;i<int(positions.size());i++){
186  remapTable[i]=i;
187  pointCount[i]=1;
188  }
189  for(i=int(triangles.size()-1);i>=0;i--){
190  for(j=0;j<3;j++){
191  idx[j]=triangles[i].idx[j];
192  while(remapTable[idx[j]]<idx[j]){idx[j]=remapTable[idx[j]];}
193  }
194  if(idx[0]==idx[1] || idx[0]==idx[2] || idx[1]==idx[2]){
195  triangles[i]=triangles[triangles.size()-1];
196  triangles.pop_back();
197  continue;
198  }
199  for(j=0;j<3;j++){
200  p[j].coords[0]=positions[idx[j]].coords[0]/pointCount[idx[j]];
201  p[j].coords[1]=positions[idx[j]].coords[1]/pointCount[idx[j]];
202  p[j].coords[2]=positions[idx[j]].coords[2]/pointCount[idx[j]];
203  }
204  for(j=0;j<3;j++){
205  q[0].coords[j]=p[1].coords[j]-p[0].coords[j];
206  q[1].coords[j]=p[2].coords[j]-p[0].coords[j];
207  d[j]=SquareDistance(p[j],p[(j+1)%3]);
208  }
209  CrossProduct(q[0],q[1],c);
210  a=Length(c)/2;
211 
212  if((d[0]+d[1]+d[2])*edgeRatio > a*Ratio){
213  // Find the smallest edge
214  j=0;
215  if(d[1]<d[j]){j=1;}
216  if(d[2]<d[j]){j=2;}
217 
218  int idx1,idx2,idx3;
219  if(idx[0]<idx[1]){
220  if(idx[0]<idx[2]){
221  idx1=idx[0];
222  idx2=idx[2];
223  idx3=idx[1];
224  }
225  else{
226  idx1=idx[2];
227  idx2=idx[0];
228  idx3=idx[1];
229  }
230  }
231  else{
232  if(idx[1]<idx[2]){
233  idx1=idx[1];
234  idx2=idx[2];
235  idx3=idx[0];
236  }
237  else{
238  idx1=idx[2];
239  idx2=idx[1];
240  idx3=idx[0];
241  }
242  }
243  positions[idx1].coords[0]+=positions[idx2].coords[0]+positions[idx3].coords[0];
244  positions[idx1].coords[1]+=positions[idx2].coords[1]+positions[idx3].coords[1];
245  positions[idx1].coords[2]+=positions[idx2].coords[2]+positions[idx3].coords[2];
246  if(normals){
247  (*normals)[idx1].coords[0]+=(*normals)[idx2].coords[0]+(*normals)[idx3].coords[0];
248  (*normals)[idx1].coords[1]+=(*normals)[idx2].coords[1]+(*normals)[idx3].coords[1];
249  (*normals)[idx1].coords[2]+=(*normals)[idx2].coords[2]+(*normals)[idx3].coords[2];
250  }
251  pointCount[idx1]+=pointCount[idx2]+pointCount[idx3];
252  remapTable[idx2]=idx1;
253  remapTable[idx3]=idx1;
254  triangles[i]=triangles[triangles.size()-1];
255  triangles.pop_back();
256  }
257  }
258  int pCount=0;
259  for(i=0;i<int(positions.size());i++){
260  for(j=0;j<3;j++){positions[i].coords[j]/=pointCount[i];}
261  if(normals){
262  Real l=Real(Length((*normals)[i]));
263  for(j=0;j<3;j++){(*normals)[i].coords[j]/=l;}
264  }
265  if(remapTable[i]==i){ // If vertex i is being used
266  positions[pCount]=positions[i];
267  if(normals){(*normals)[pCount]=(*normals)[i];}
268  pointCount[i]=pCount;
269  pCount++;
270  }
271  }
272  positions.resize(pCount);
273  for(i=int(triangles.size()-1);i>=0;i--){
274  for(j=0;j<3;j++){
275  idx[j]=triangles[i].idx[j];
276  while(remapTable[idx[j]]<idx[j]){idx[j]=remapTable[idx[j]];}
277  triangles[i].idx[j]=pointCount[idx[j]];
278  }
279  if(idx[0]==idx[1] || idx[0]==idx[2] || idx[1]==idx[2]){
280  triangles[i]=triangles[triangles.size()-1];
281  triangles.pop_back();
282  }
283  }
284  delete[] pointCount;
285  delete[] remapTable;
286  }
287 
288  ///////////////////
289  // Triangulation //
290  ///////////////////
291  template<class Real>
292  long long Triangulation<Real>::EdgeIndex( int p1 , int p2 )
293  {
294  if(p1>p2) {return ((long long)(p1)<<32) | ((long long)(p2));}
295  else {return ((long long)(p2)<<32) | ((long long)(p1));}
296  }
297 
298  template<class Real>
299  int Triangulation<Real>::factor(int tIndex,int& p1,int& p2,int & p3){
300  if(triangles[tIndex].eIndex[0]<0 || triangles[tIndex].eIndex[1]<0 || triangles[tIndex].eIndex[2]<0){return 0;}
301  if(edges[triangles[tIndex].eIndex[0]].tIndex[0]==tIndex){p1=edges[triangles[tIndex].eIndex[0]].pIndex[0];}
302  else {p1=edges[triangles[tIndex].eIndex[0]].pIndex[1];}
303  if(edges[triangles[tIndex].eIndex[1]].tIndex[0]==tIndex){p2=edges[triangles[tIndex].eIndex[1]].pIndex[0];}
304  else {p2=edges[triangles[tIndex].eIndex[1]].pIndex[1];}
305  if(edges[triangles[tIndex].eIndex[2]].tIndex[0]==tIndex){p3=edges[triangles[tIndex].eIndex[2]].pIndex[0];}
306  else {p3=edges[triangles[tIndex].eIndex[2]].pIndex[1];}
307  return 1;
308  }
309  template<class Real>
310  double Triangulation<Real>::area(int p1,int p2,int p3){
311  Point3D<Real> q1,q2,q;
312  for(int i=0;i<3;i++){
313  q1.coords[i]=points[p2].coords[i]-points[p1].coords[i];
314  q2.coords[i]=points[p3].coords[i]-points[p1].coords[i];
315  }
316  CrossProduct(q1,q2,q);
317  return Length(q);
318  }
319  template<class Real>
320  double Triangulation<Real>::area(int tIndex){
321  int p1,p2,p3;
322  factor(tIndex,p1,p2,p3);
323  return area(p1,p2,p3);
324  }
325  template<class Real>
327  double a=0;
328  for(int i=0;i<int(triangles.size());i++){a+=area(i);}
329  return a;
330  }
331  template<class Real>
332  int Triangulation<Real>::addTriangle(int p1,int p2,int p3){
333  int tIdx,eIdx,p[3];
334  p[0]=p1;
335  p[1]=p2;
336  p[2]=p3;
337  triangles.push_back(TriangulationTriangle());
338  tIdx=int(triangles.size())-1;
339 
340  for(int i=0;i<3;i++)
341  {
342  long long e = EdgeIndex(p[i],p[(i+1)%3]);
343  if(edgeMap.count(e) == 0)
344  {
345  TriangulationEdge edge;
346  edge.pIndex[0]=p[i];
347  edge.pIndex[1]=p[(i+1)%3];
348  edges.push_back(edge);
349  eIdx=int(edges.size())-1;
350  edgeMap[e]=eIdx;
351  edges[eIdx].tIndex[0]=tIdx;
352  }
353  else{
354  eIdx=edgeMap[e];
355  if(edges[eIdx].pIndex[0]==p[i]){
356  if(edges[eIdx].tIndex[0]<0){edges[eIdx].tIndex[0]=tIdx;}
357  else{PCL_DEBUG("Edge Triangle in use 1\n");return 0;}
358  }
359  else{
360  if(edges[eIdx].tIndex[1]<0){edges[eIdx].tIndex[1]=tIdx;}
361  else{PCL_DEBUG("Edge Triangle in use 2\n");return 0;}
362  }
363 
364  }
365  triangles[tIdx].eIndex[i]=eIdx;
366  }
367  return tIdx;
368  }
369  template<class Real>
371  double oldArea,newArea;
372  int oldP[3],oldQ[3],newP[3],newQ[3];
373  TriangulationEdge newEdge;
374 
375  if(edges[eIndex].tIndex[0]<0 || edges[eIndex].tIndex[1]<0){return 0;}
376 
377  if(!factor(edges[eIndex].tIndex[0],oldP[0],oldP[1],oldP[2])){return 0;}
378  if(!factor(edges[eIndex].tIndex[1],oldQ[0],oldQ[1],oldQ[2])){return 0;}
379 
380  oldArea=area(oldP[0],oldP[1],oldP[2])+area(oldQ[0],oldQ[1],oldQ[2]);
381  int idxP,idxQ;
382  for(idxP=0;idxP<3;idxP++){
383  int i;
384  for(i=0;i<3;i++){if(oldP[idxP]==oldQ[i]){break;}}
385  if(i==3){break;}
386  }
387  for(idxQ=0;idxQ<3;idxQ++){
388  int i;
389  for(i=0;i<3;i++){if(oldP[i]==oldQ[idxQ]){break;}}
390  if(i==3){break;}
391  }
392  if(idxP==3 || idxQ==3){return 0;}
393  newP[0]=oldP[idxP];
394  newP[1]=oldP[(idxP+1)%3];
395  newP[2]=oldQ[idxQ];
396  newQ[0]=oldQ[idxQ];
397  newQ[1]=oldP[(idxP+2)%3];
398  newQ[2]=oldP[idxP];
399 
400  newArea=area(newP[0],newP[1],newP[2])+area(newQ[0],newQ[1],newQ[2]);
401  if(oldArea<=newArea){return 0;}
402 
403  // Remove the entry in the hash_table for the old edge
404  edgeMap.erase(EdgeIndex(edges[eIndex].pIndex[0],edges[eIndex].pIndex[1]));
405  // Set the new edge so that the zero-side is newQ
406  edges[eIndex].pIndex[0]=newP[0];
407  edges[eIndex].pIndex[1]=newQ[0];
408  // Insert the entry into the hash_table for the new edge
409  edgeMap[EdgeIndex(newP[0],newQ[0])]=eIndex;
410  // Update the triangle information
411  for(int i=0;i<3;i++){
412  int idx;
413  idx=edgeMap[EdgeIndex(newQ[i],newQ[(i+1)%3])];
414  triangles[edges[eIndex].tIndex[0]].eIndex[i]=idx;
415  if(idx!=eIndex){
416  if(edges[idx].tIndex[0]==edges[eIndex].tIndex[1]){edges[idx].tIndex[0]=edges[eIndex].tIndex[0];}
417  if(edges[idx].tIndex[1]==edges[eIndex].tIndex[1]){edges[idx].tIndex[1]=edges[eIndex].tIndex[0];}
418  }
419 
420  idx=edgeMap[EdgeIndex(newP[i],newP[(i+1)%3])];
421  triangles[edges[eIndex].tIndex[1]].eIndex[i]=idx;
422  if(idx!=eIndex){
423  if(edges[idx].tIndex[0]==edges[eIndex].tIndex[0]){edges[idx].tIndex[0]=edges[eIndex].tIndex[1];}
424  if(edges[idx].tIndex[1]==edges[eIndex].tIndex[0]){edges[idx].tIndex[1]=edges[eIndex].tIndex[1];}
425  }
426  }
427  return 1;
428  }
429 
430  }
431 }
double SquareLength(const Point3D< Real > &p)
Definition: geometry.hpp:62
int flipMinimize(int eIndex)
Definition: geometry.hpp:370
int addTriangle(int p1, int p2, int p3)
Definition: geometry.hpp:332
double Length(const Point3D< Real > &p)
Definition: geometry.hpp:65
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
void CrossProduct(const Point3D< Real > &p1, const Point3D< Real > &p2, Point3D< Real > &p)
Definition: geometry.hpp:76
Point3D< Real > RandomBallPoint(void)
Definition: geometry.hpp:41
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
double Distance(const Point3D< Real > &p1, const Point3D< Real > &p2)
Definition: geometry.hpp:73
Real Random(void)
Definition: geometry.hpp:38
int factor(int tIndex, int &p1, int &p2, int &p3)
Definition: geometry.hpp:299
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
double SquareDistance(const Point3D< Real > &p1, const Point3D< Real > &p2)
Definition: geometry.hpp:68