Point Cloud Library (PCL)  1.9.1
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  hash_map<long long,int>::iterator iter;
334  int tIdx,eIdx,p[3];
335  p[0]=p1;
336  p[1]=p2;
337  p[2]=p3;
338  triangles.push_back(TriangulationTriangle());
339  tIdx=int(triangles.size())-1;
340 
341  for(int i=0;i<3;i++)
342  {
343  long long e = EdgeIndex(p[i],p[(i+1)%3]);
344  iter=edgeMap.find(e);
345  if(iter==edgeMap.end())
346  {
347  TriangulationEdge edge;
348  edge.pIndex[0]=p[i];
349  edge.pIndex[1]=p[(i+1)%3];
350  edges.push_back(edge);
351  eIdx=int(edges.size())-1;
352  edgeMap[e]=eIdx;
353  edges[eIdx].tIndex[0]=tIdx;
354  }
355  else{
356  eIdx=edgeMap[e];
357  if(edges[eIdx].pIndex[0]==p[i]){
358  if(edges[eIdx].tIndex[0]<0){edges[eIdx].tIndex[0]=tIdx;}
359  else{PCL_DEBUG("Edge Triangle in use 1\n");return 0;}
360  }
361  else{
362  if(edges[eIdx].tIndex[1]<0){edges[eIdx].tIndex[1]=tIdx;}
363  else{PCL_DEBUG("Edge Triangle in use 2\n");return 0;}
364  }
365 
366  }
367  triangles[tIdx].eIndex[i]=eIdx;
368  }
369  return tIdx;
370  }
371  template<class Real>
373  double oldArea,newArea;
374  int oldP[3],oldQ[3],newP[3],newQ[3];
375  TriangulationEdge newEdge;
376 
377  if(edges[eIndex].tIndex[0]<0 || edges[eIndex].tIndex[1]<0){return 0;}
378 
379  if(!factor(edges[eIndex].tIndex[0],oldP[0],oldP[1],oldP[2])){return 0;}
380  if(!factor(edges[eIndex].tIndex[1],oldQ[0],oldQ[1],oldQ[2])){return 0;}
381 
382  oldArea=area(oldP[0],oldP[1],oldP[2])+area(oldQ[0],oldQ[1],oldQ[2]);
383  int idxP,idxQ;
384  for(idxP=0;idxP<3;idxP++){
385  int i;
386  for(i=0;i<3;i++){if(oldP[idxP]==oldQ[i]){break;}}
387  if(i==3){break;}
388  }
389  for(idxQ=0;idxQ<3;idxQ++){
390  int i;
391  for(i=0;i<3;i++){if(oldP[i]==oldQ[idxQ]){break;}}
392  if(i==3){break;}
393  }
394  if(idxP==3 || idxQ==3){return 0;}
395  newP[0]=oldP[idxP];
396  newP[1]=oldP[(idxP+1)%3];
397  newP[2]=oldQ[idxQ];
398  newQ[0]=oldQ[idxQ];
399  newQ[1]=oldP[(idxP+2)%3];
400  newQ[2]=oldP[idxP];
401 
402  newArea=area(newP[0],newP[1],newP[2])+area(newQ[0],newQ[1],newQ[2]);
403  if(oldArea<=newArea){return 0;}
404 
405  // Remove the entry in the hash_table for the old edge
406  edgeMap.erase(EdgeIndex(edges[eIndex].pIndex[0],edges[eIndex].pIndex[1]));
407  // Set the new edge so that the zero-side is newQ
408  edges[eIndex].pIndex[0]=newP[0];
409  edges[eIndex].pIndex[1]=newQ[0];
410  // Insert the entry into the hash_table for the new edge
411  edgeMap[EdgeIndex(newP[0],newQ[0])]=eIndex;
412  // Update the triangle information
413  for(int i=0;i<3;i++){
414  int idx;
415  idx=edgeMap[EdgeIndex(newQ[i],newQ[(i+1)%3])];
416  triangles[edges[eIndex].tIndex[0]].eIndex[i]=idx;
417  if(idx!=eIndex){
418  if(edges[idx].tIndex[0]==edges[eIndex].tIndex[1]){edges[idx].tIndex[0]=edges[eIndex].tIndex[0];}
419  if(edges[idx].tIndex[1]==edges[eIndex].tIndex[1]){edges[idx].tIndex[1]=edges[eIndex].tIndex[0];}
420  }
421 
422  idx=edgeMap[EdgeIndex(newP[i],newP[(i+1)%3])];
423  triangles[edges[eIndex].tIndex[1]].eIndex[i]=idx;
424  if(idx!=eIndex){
425  if(edges[idx].tIndex[0]==edges[eIndex].tIndex[0]){edges[idx].tIndex[0]=edges[eIndex].tIndex[1];}
426  if(edges[idx].tIndex[1]==edges[eIndex].tIndex[0]){edges[idx].tIndex[1]=edges[eIndex].tIndex[1];}
427  }
428  }
429  return 1;
430  }
431 
432  }
433 }
pcl
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
pcl::poisson::EdgeCollapse
void EdgeCollapse(const Real &edgeRatio, std::vector< TriangleIndex > &triangles, std::vector< Point3D< Real > > &positions, std::vector< Point3D< Real > > *normals)
Definition: geometry.hpp:82
pcl::poisson::Triangulation::area
double area(void)
Definition: geometry.hpp:326
pcl::poisson::TriangleCollapse
void TriangleCollapse(const Real &edgeRatio, std::vector< TriangleIndex > &triangles, std::vector< Point3D< Real > > &positions, std::vector< Point3D< Real > > *normals)
Definition: geometry.hpp:177
pcl::poisson::Random
Real Random(void)
Definition: geometry.hpp:38
pcl::poisson::Point3D
Definition: geometry.h:50
pcl::poisson::RandomSpherePoint
Point3D< Real > RandomSpherePoint(void)
Definition: geometry.hpp:52
pcl::poisson::SquareLength
double SquareLength(const Point3D< Real > &p)
Definition: geometry.hpp:62
pcl::poisson::EdgeIndex
Definition: geometry.h:138
pcl::poisson::Length
double Length(const Point3D< Real > &p)
Definition: geometry.hpp:65
pcl::poisson::RandomBallPoint
Point3D< Real > RandomBallPoint(void)
Definition: geometry.hpp:41
pcl::poisson::Triangulation::flipMinimize
int flipMinimize(int eIndex)
Definition: geometry.hpp:372
pcl::poisson::Triangulation::factor
int factor(int tIndex, int &p1, int &p2, int &p3)
Definition: geometry.hpp:299
pcl::poisson::TriangulationEdge::pIndex
int pIndex[2]
Definition: geometry.h:155
pcl::poisson::Triangulation::addTriangle
int addTriangle(int p1, int p2, int p3)
Definition: geometry.hpp:332
pcl::poisson::Triangulation::EdgeIndex
static long long EdgeIndex(int p1, int p2)
Definition: geometry.hpp:292
pcl::poisson::Point3D::coords
Real coords[3]
Definition: geometry.h:52
pcl::poisson::SquareDistance
double SquareDistance(const Point3D< Real > &p1, const Point3D< Real > &p2)
Definition: geometry.hpp:68
pcl::poisson::Distance
double Distance(const Point3D< Real > &p1, const Point3D< Real > &p2)
Definition: geometry.hpp:73
pcl::poisson::Real
float Real
Definition: multi_grid_octree_data.h:85
pcl::poisson::TriangulationTriangle
Definition: geometry.h:159
pcl::poisson::CrossProduct
void CrossProduct(const Point3D< Real > &p1, const Point3D< Real > &p2, Point3D< Real > &p)
Definition: geometry.hpp:76
pcl::poisson::TriangulationEdge
Definition: geometry.h:151