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