Point Cloud Library (PCL)  1.10.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  template<class Real>
36  Real Random(void){return Real(rand())/RAND_MAX;}
37 
38  template<class Real>
40  Point3D<Real> p;
41  while(1){
42  p.coords[0]=Real(1.0-2.0*Random<Real>());
43  p.coords[1]=Real(1.0-2.0*Random<Real>());
44  p.coords[2]=Real(1.0-2.0*Random<Real>());
45  double l=SquareLength(p);
46  if(l<=1){return p;}
47  }
48  }
49  template<class Real>
51  Point3D<Real> p=RandomBallPoint<Real>();
52  Real l=Real(Length(p));
53  p.coords[0]/=l;
54  p.coords[1]/=l;
55  p.coords[2]/=l;
56  return p;
57  }
58 
59  template<class Real>
60  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];}
61 
62  template<class Real>
63  double Length(const Point3D<Real>& p){return sqrt(SquareLength(p));}
64 
65  template<class Real>
66  double SquareDistance(const Point3D<Real>& p1,const Point3D<Real>& p2){
67  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]);
68  }
69 
70  template<class Real>
71  double Distance(const Point3D<Real>& p1,const Point3D<Real>& p2){return sqrt(SquareDistance(p1,p2));}
72 
73  template <class Real>
74  void CrossProduct(const Point3D<Real>& p1,const Point3D<Real>& p2,Point3D<Real>& p){
75  p.coords[0]= p1.coords[1]*p2.coords[2]-p1.coords[2]*p2.coords[1];
76  p.coords[1]=-p1.coords[0]*p2.coords[2]+p1.coords[2]*p2.coords[0];
77  p.coords[2]= p1.coords[0]*p2.coords[1]-p1.coords[1]*p2.coords[0];
78  }
79 
80  template<class Real>
81  void EdgeCollapse(const Real& edgeRatio,std::vector<TriangleIndex>& triangles,std::vector< Point3D<Real> >& positions,std::vector< Point3D<Real> >* normals){
82  int i,j,*remapTable,*pointCount,idx[3];
83  Point3D<Real> p[3],q[2],c;
84  double d[3],a;
85  double Ratio=12.0/sqrt(3.0); // (Sum of Squares Length / Area) for and equilateral triangle
86 
87  remapTable=new int[positions.size()];
88  pointCount=new int[positions.size()];
89  for(i=0;i<int(positions.size());i++){
90  remapTable[i]=i;
91  pointCount[i]=1;
92  }
93  for(i=int(triangles.size()-1);i>=0;i--){
94  for(j=0;j<3;j++){
95  idx[j]=triangles[i].idx[j];
96  while(remapTable[idx[j]]<idx[j]){idx[j]=remapTable[idx[j]];}
97  }
98  if(idx[0]==idx[1] || idx[0]==idx[2] || idx[1]==idx[2]){
99  triangles[i]=triangles[triangles.size()-1];
100  triangles.pop_back();
101  continue;
102  }
103  for(j=0;j<3;j++){
104  p[j].coords[0]=positions[idx[j]].coords[0]/pointCount[idx[j]];
105  p[j].coords[1]=positions[idx[j]].coords[1]/pointCount[idx[j]];
106  p[j].coords[2]=positions[idx[j]].coords[2]/pointCount[idx[j]];
107  }
108  for(j=0;j<3;j++){
109  q[0].coords[j]=p[1].coords[j]-p[0].coords[j];
110  q[1].coords[j]=p[2].coords[j]-p[0].coords[j];
111  d[j]=SquareDistance(p[j],p[(j+1)%3]);
112  }
113  CrossProduct(q[0],q[1],c);
114  a=Length(c)/2;
115 
116  if((d[0]+d[1]+d[2])*edgeRatio > a*Ratio){
117  // Find the smallest edge
118  j=0;
119  if(d[1]<d[j]){j=1;}
120  if(d[2]<d[j]){j=2;}
121 
122  int idx1,idx2;
123  if(idx[j]<idx[(j+1)%3]){
124  idx1=idx[j];
125  idx2=idx[(j+1)%3];
126  }
127  else{
128  idx2=idx[j];
129  idx1=idx[(j+1)%3];
130  }
131  positions[idx1].coords[0]+=positions[idx2].coords[0];
132  positions[idx1].coords[1]+=positions[idx2].coords[1];
133  positions[idx1].coords[2]+=positions[idx2].coords[2];
134  if(normals){
135  (*normals)[idx1].coords[0]+=(*normals)[idx2].coords[0];
136  (*normals)[idx1].coords[1]+=(*normals)[idx2].coords[1];
137  (*normals)[idx1].coords[2]+=(*normals)[idx2].coords[2];
138  }
139  pointCount[idx1]+=pointCount[idx2];
140  remapTable[idx2]=idx1;
141  triangles[i]=triangles[triangles.size()-1];
142  triangles.pop_back();
143  }
144  }
145  int pCount=0;
146  for(i=0;i<int(positions.size());i++){
147  for(j=0;j<3;j++){positions[i].coords[j]/=pointCount[i];}
148  if(normals){
149  Real l=Real(Length((*normals)[i]));
150  for(j=0;j<3;j++){(*normals)[i].coords[j]/=l;}
151  }
152  if(remapTable[i]==i){ // If vertex i is being used
153  positions[pCount]=positions[i];
154  if(normals){(*normals)[pCount]=(*normals)[i];}
155  pointCount[i]=pCount;
156  pCount++;
157  }
158  }
159  positions.resize(pCount);
160  for(i=int(triangles.size()-1);i>=0;i--){
161  for(j=0;j<3;j++){
162  idx[j]=triangles[i].idx[j];
163  while(remapTable[idx[j]]<idx[j]){idx[j]=remapTable[idx[j]];}
164  triangles[i].idx[j]=pointCount[idx[j]];
165  }
166  if(idx[0]==idx[1] || idx[0]==idx[2] || idx[1]==idx[2]){
167  triangles[i]=triangles[triangles.size()-1];
168  triangles.pop_back();
169  }
170  }
171 
172  delete[] pointCount;
173  delete[] remapTable;
174  }
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 
310  template<class Real>
311  double Triangulation<Real>::area(int p1,int p2,int p3){
312  Point3D<Real> q1,q2,q;
313  for(int i=0;i<3;i++){
314  q1.coords[i]=points[p2].coords[i]-points[p1].coords[i];
315  q2.coords[i]=points[p3].coords[i]-points[p1].coords[i];
316  }
317  CrossProduct(q1,q2,q);
318  return Length(q);
319  }
320 
321  template<class Real>
322  double Triangulation<Real>::area(int tIndex){
323  int p1,p2,p3;
324  factor(tIndex,p1,p2,p3);
325  return area(p1,p2,p3);
326  }
327 
328  template<class Real>
330  double a=0;
331  for(int i=0;i<int(triangles.size());i++){a+=area(i);}
332  return a;
333  }
334 
335  template<class Real>
336  int Triangulation<Real>::addTriangle(int p1,int p2,int p3){
337  int tIdx,eIdx,p[3];
338  p[0]=p1;
339  p[1]=p2;
340  p[2]=p3;
341  triangles.push_back(TriangulationTriangle());
342  tIdx=int(triangles.size())-1;
343 
344  for(int i=0;i<3;i++)
345  {
346  long long e = EdgeIndex(p[i],p[(i+1)%3]);
347  if(edgeMap.count(e) == 0)
348  {
349  TriangulationEdge edge;
350  edge.pIndex[0]=p[i];
351  edge.pIndex[1]=p[(i+1)%3];
352  edges.push_back(edge);
353  eIdx=int(edges.size())-1;
354  edgeMap[e]=eIdx;
355  edges[eIdx].tIndex[0]=tIdx;
356  }
357  else{
358  eIdx=edgeMap[e];
359  if(edges[eIdx].pIndex[0]==p[i]){
360  if(edges[eIdx].tIndex[0]<0){edges[eIdx].tIndex[0]=tIdx;}
361  else{PCL_DEBUG("Edge Triangle in use 1\n");return 0;}
362  }
363  else{
364  if(edges[eIdx].tIndex[1]<0){edges[eIdx].tIndex[1]=tIdx;}
365  else{PCL_DEBUG("Edge Triangle in use 2\n");return 0;}
366  }
367 
368  }
369  triangles[tIdx].eIndex[i]=eIdx;
370  }
371  return tIdx;
372  }
373 
374  template<class Real>
376  double oldArea,newArea;
377  int oldP[3],oldQ[3],newP[3],newQ[3];
378  TriangulationEdge newEdge;
379 
380  if(edges[eIndex].tIndex[0]<0 || edges[eIndex].tIndex[1]<0){return 0;}
381 
382  if(!factor(edges[eIndex].tIndex[0],oldP[0],oldP[1],oldP[2])){return 0;}
383  if(!factor(edges[eIndex].tIndex[1],oldQ[0],oldQ[1],oldQ[2])){return 0;}
384 
385  oldArea=area(oldP[0],oldP[1],oldP[2])+area(oldQ[0],oldQ[1],oldQ[2]);
386  int idxP,idxQ;
387  for(idxP=0;idxP<3;idxP++){
388  int i;
389  for(i=0;i<3;i++){if(oldP[idxP]==oldQ[i]){break;}}
390  if(i==3){break;}
391  }
392  for(idxQ=0;idxQ<3;idxQ++){
393  int i;
394  for(i=0;i<3;i++){if(oldP[i]==oldQ[idxQ]){break;}}
395  if(i==3){break;}
396  }
397  if(idxP==3 || idxQ==3){return 0;}
398  newP[0]=oldP[idxP];
399  newP[1]=oldP[(idxP+1)%3];
400  newP[2]=oldQ[idxQ];
401  newQ[0]=oldQ[idxQ];
402  newQ[1]=oldP[(idxP+2)%3];
403  newQ[2]=oldP[idxP];
404 
405  newArea=area(newP[0],newP[1],newP[2])+area(newQ[0],newQ[1],newQ[2]);
406  if(oldArea<=newArea){return 0;}
407 
408  // Remove the entry in the hash_table for the old edge
409  edgeMap.erase(EdgeIndex(edges[eIndex].pIndex[0],edges[eIndex].pIndex[1]));
410  // Set the new edge so that the zero-side is newQ
411  edges[eIndex].pIndex[0]=newP[0];
412  edges[eIndex].pIndex[1]=newQ[0];
413  // Insert the entry into the hash_table for the new edge
414  edgeMap[EdgeIndex(newP[0],newQ[0])]=eIndex;
415  // Update the triangle information
416  for(int i=0;i<3;i++){
417  int idx;
418  idx=edgeMap[EdgeIndex(newQ[i],newQ[(i+1)%3])];
419  triangles[edges[eIndex].tIndex[0]].eIndex[i]=idx;
420  if(idx!=eIndex){
421  if(edges[idx].tIndex[0]==edges[eIndex].tIndex[1]){edges[idx].tIndex[0]=edges[eIndex].tIndex[0];}
422  if(edges[idx].tIndex[1]==edges[eIndex].tIndex[1]){edges[idx].tIndex[1]=edges[eIndex].tIndex[0];}
423  }
424 
425  idx=edgeMap[EdgeIndex(newP[i],newP[(i+1)%3])];
426  triangles[edges[eIndex].tIndex[1]].eIndex[i]=idx;
427  if(idx!=eIndex){
428  if(edges[idx].tIndex[0]==edges[eIndex].tIndex[0]){edges[idx].tIndex[0]=edges[eIndex].tIndex[1];}
429  if(edges[idx].tIndex[1]==edges[eIndex].tIndex[0]){edges[idx].tIndex[1]=edges[eIndex].tIndex[1];}
430  }
431  }
432  return 1;
433  }
434  }
435 }
double SquareLength(const Point3D< Real > &p)
Definition: geometry.hpp:60
int flipMinimize(int eIndex)
Definition: geometry.hpp:375
int addTriangle(int p1, int p2, int p3)
Definition: geometry.hpp:336
double Length(const Point3D< Real > &p)
Definition: geometry.hpp:63
void CrossProduct(const Point3D< Real > &p1, const Point3D< Real > &p2, Point3D< Real > &p)
Definition: geometry.hpp:74
Point3D< Real > RandomBallPoint(void)
Definition: geometry.hpp:39
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:71
Real Random(void)
Definition: geometry.hpp:36
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:81
Point3D< Real > RandomSpherePoint(void)
Definition: geometry.hpp:50
double SquareDistance(const Point3D< Real > &p1, const Point3D< Real > &p2)
Definition: geometry.hpp:66