Point Cloud Library (PCL)  1.14.1-dev
mat.hpp
1 /*
2 Copyright (c) 2007, Michael Kazhdan
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 // MinimalAreaTriangulation //
30 //////////////////////////////
31 
32 namespace pcl
33 {
34  namespace poisson
35  {
36 
37  template <class Real>
38  MinimalAreaTriangulation<Real>::MinimalAreaTriangulation(void)
39  {
40  bestTriangulation=NULL;
41  midPoint=NULL;
42  }
43  template <class Real>
44  MinimalAreaTriangulation<Real>::~MinimalAreaTriangulation(void)
45  {
46  delete[] bestTriangulation;
47  bestTriangulation=NULL;
48  delete[] midPoint;
49  midPoint=NULL;
50  }
51  template <class Real>
52  void MinimalAreaTriangulation<Real>::GetTriangulation(const std::vector<Point3D<Real> >& vertices,std::vector<TriangleIndex>& triangles)
53  {
54  if(vertices.size()==3)
55  {
56  triangles.resize(1);
57  triangles[0].idx[0]=0;
58  triangles[0].idx[1]=1;
59  triangles[0].idx[2]=2;
60  return;
61  }
62  else if(vertices.size()==4)
63  {
64  TriangleIndex tIndex[2][2];
65  Real area[2];
66 
67  area[0]=area[1]=0;
68  triangles.resize(2);
69 
70  tIndex[0][0].idx[0]=0;
71  tIndex[0][0].idx[1]=1;
72  tIndex[0][0].idx[2]=2;
73  tIndex[0][1].idx[0]=2;
74  tIndex[0][1].idx[1]=3;
75  tIndex[0][1].idx[2]=0;
76 
77  tIndex[1][0].idx[0]=0;
78  tIndex[1][0].idx[1]=1;
79  tIndex[1][0].idx[2]=3;
80  tIndex[1][1].idx[0]=3;
81  tIndex[1][1].idx[1]=1;
82  tIndex[1][1].idx[2]=2;
83 
84  Point3D<Real> n,p1,p2;
85  for(int i=0;i<2;i++)
86  for(int j=0;j<2;j++)
87  {
88  p1=vertices[tIndex[i][j].idx[1]]-vertices[tIndex[i][j].idx[0]];
89  p2=vertices[tIndex[i][j].idx[2]]-vertices[tIndex[i][j].idx[0]];
90  CrossProduct(p1,p2,n);
91  area[i] += Real( Length(n) );
92  }
93  if(area[0]>area[1])
94  {
95  triangles[0]=tIndex[1][0];
96  triangles[1]=tIndex[1][1];
97  }
98  else
99  {
100  triangles[0]=tIndex[0][0];
101  triangles[1]=tIndex[0][1];
102  }
103  return;
104  }
105  delete[] bestTriangulation;
106  delete[] midPoint;
107  bestTriangulation=NULL;
108  midPoint=NULL;
109  std::size_t eCount=vertices.size();
110  bestTriangulation=new Real[eCount*eCount];
111  midPoint=new int[eCount*eCount];
112  for(std::size_t i=0;i<eCount*eCount;i++)
113  bestTriangulation[i]=-1;
114  memset(midPoint,-1,sizeof(int)*eCount*eCount);
115  GetArea(0,1,vertices);
116  triangles.clear();
117  GetTriangulation(0,1,vertices,triangles);
118  }
119  template <class Real>
120  Real MinimalAreaTriangulation<Real>::GetArea(const std::vector<Point3D<Real> >& vertices)
121  {
122  delete[] bestTriangulation;
123  delete[] midPoint;
124  bestTriangulation=NULL;
125  midPoint=NULL;
126  int eCount=vertices.size();
127  bestTriangulation=new double[eCount*eCount];
128  midPoint=new int[eCount*eCount];
129  for(int i=0;i<eCount*eCount;i++)
130  bestTriangulation[i]=-1;
131  memset(midPoint,-1,sizeof(int)*eCount*eCount);
132  return GetArea(0,1,vertices);
133  }
134  template<class Real>
135  void MinimalAreaTriangulation<Real>::GetTriangulation(const std::size_t& i,const std::size_t& j,const std::vector<Point3D<Real> >& vertices,std::vector<TriangleIndex>& triangles)
136  {
137  TriangleIndex tIndex;
138  std::size_t eCount=vertices.size();
139  std::size_t ii=i;
140  if(i<j)
141  ii+=eCount;
142  if(j+1>=ii)
143  return;
144  ii=midPoint[i*eCount+j];
145  if(ii>=0)
146  {
147  tIndex.idx[0] = int( i );
148  tIndex.idx[1] = int( j );
149  tIndex.idx[2] = int( ii );
150  triangles.push_back(tIndex);
151  GetTriangulation(i,ii,vertices,triangles);
152  GetTriangulation(ii,j,vertices,triangles);
153  }
154  }
155 
156  template<class Real>
157  Real MinimalAreaTriangulation<Real>::GetArea(const std::size_t& i,const std::size_t& j,const std::vector<Point3D<Real> >& vertices)
158  {
159  Real a=FLT_MAX,temp;
160  std::size_t eCount=vertices.size();
161  std::size_t idx=i*eCount+j;
162  std::size_t ii=i;
163  if(i<j)
164  ii+=eCount;
165  if(j+1>=ii)
166  {
167  bestTriangulation[idx]=0;
168  return 0;
169  }
170  if(midPoint[idx]!=-1)
171  return bestTriangulation[idx];
172  int mid=-1;
173  for(std::size_t r=j+1;r<ii;r++)
174  {
175  std::size_t rr=r%eCount;
176  std::size_t idx1=i*eCount+rr,idx2=rr*eCount+j;
177  Point3D<Real> p,p1,p2;
178  p1=vertices[i]-vertices[rr];
179  p2=vertices[j]-vertices[rr];
180  CrossProduct(p1,p2,p);
181  temp = Real( Length(p) );
182  if(bestTriangulation[idx1]>=0)
183  {
184  temp+=bestTriangulation[idx1];
185  if(temp>a)
186  continue;
187  if(bestTriangulation[idx2]>0)
188  temp+=bestTriangulation[idx2];
189  else
190  temp+=GetArea(rr,j,vertices);
191  }
192  else
193  {
194  if(bestTriangulation[idx2]>=0)
195  temp+=bestTriangulation[idx2];
196  else
197  temp+=GetArea(rr,j,vertices);
198  if(temp>a)
199  continue;
200  temp+=GetArea(i,rr,vertices);
201  }
202 
203  if(temp<a)
204  {
205  a=temp;
206  mid=int(rr);
207  }
208  }
209  bestTriangulation[idx]=a;
210  midPoint[idx]=mid;
211 
212  return a;
213  }
214 
215 
216  }
217 }
pcl::poisson::CrossProduct
void CrossProduct(const Point3D< Real > &p1, const Point3D< Real > &p2, Point3D< Real > &p)
Definition: geometry.hpp:74
pcl::poisson::Length
double Length(const Point3D< Real > &p)
Definition: geometry.hpp:63