Debian Codegraph @

plugins/core/Standard/qRANSAC_SD/extern/RANSAC_SD/PointCloud.cpp

raw
1 #include "PointCloud.h"
2 #include <MiscLib/Vector.h>
3 #include <iostream>
4 #include <iterator>
5 #include <fstream>
6 #include <limits>
7 #include <algorithm>
8 #ifndef _USE_MATH_DEFINES
9 #define _USE_MATH_DEFINES
10 #endif
11 #include <cmath>
12 #include "Plane.h"
13 #include <GfxTL/KdTree.h>
14 #include <GfxTL/CellRangeDataTreeStrategy.h>
15 #include <GfxTL/IndexedTreeDataKernels.h>
16 #include <GfxTL/VectorKernel.h>
17 #include <GfxTL/NullTreeStrategy.h>
18 #include <GfxTL/BBoxDistanceKdTreeStrategy.h>
19 #include <GfxTL/IncrementalDistanceKdTreeStrategy.h>
20 #include <GfxTL/MaxIntervalSplittingKdTreeStrategy.h>
21 #include <GfxTL/CellBBoxBuildInformationKdTreeStrategy.h>
22 #include <GfxTL/BBoxBuildInformationTreeStrategy.h>
23 #include <GfxTL/BucketSizeMaxLevelSubdivisionTreeStrategy.h>
24 #include <GfxTL/CellLevelTreeStrategy.h>
25 #include <GfxTL/L2Norm.h>
26 #include <GfxTL/Plane.h>
27 #include <GfxTL/VectorXD.h>
28 #include <GfxTL/IndexedIterator.h>
29 #include <MiscLib/AlignedAllocator.h>
30 #include <MiscLib/NoShrinkVector.h>
31
32 typedef GfxTL::KdTree
33 <
34 GfxTL::IncrementalDistanceKdTreeStrategy
35 <
36 GfxTL::MaxIntervalSplittingKdTreeStrategy
37 <
38 GfxTL::CellBBoxBuildInformationKdTreeStrategy
39 <
40 GfxTL::BBoxBuildInformationTreeStrategy
41 <
42 GfxTL::BucketSizeMaxLevelSubdivisionTreeStrategy
43 <
44 GfxTL::CellLevelTreeStrategy
45 <
46 GfxTL::BaseKdTreeStrategy
47 <
48 GfxTL::CellRangeDataTreeStrategy
49 <
50 GfxTL::NullTreeStrategy,
51 GfxTL::IndexedIteratorTreeDataKernel
52 <
53 PointCloud::const_iterator,
54 MiscLib::Vector< size_t >
55 >
56 >
57 >
58 >
59 >
60 >
61 >
62 >
63 >,
64 GfxTL::L2Norm,
65 GfxTL::VectorKernelD< 3 >::VectorKernelType
66 > KdTree3Df;
67
68 //#define LMS_NORMALS
69 #define PCA_NORMALS
70
71 using namespace std;
72
73 /*
74 * * This Quickselect routine is based on the algorithm described in
75 * * "Numerical recipes in C", Second Edition,
76 * * Cambridge University Press, 1992, Section 8.5, ISBN 0-521-43108-5
77 * * This code by Nicolas Devillard - 1998. Public domain.
78 * */
79 //#define ELEM_SWAP(a,b) { register float t=(a);(a)=(b);(b)=t; }
80 #define ELEM_SWAP(a,b) std::swap(a,b);
81 float quick_select(float arr[], int n)
82 {
83 int low, high ;
84 int median;
85 int middle, ll, hh;
86 low = 0 ; high = n-1 ; median = (low + high) / 2;
87 for (;;) {
88 if (high <= low) /* One element only */
89 return arr[median] ;
90 if (high == low + 1) { /* Two elements only */
91 if (arr[low] > arr[high])
92 ELEM_SWAP(arr[low], arr[high]) ;
93 return arr[median] ;
94 }
95 /* Find median of low, middle and high items; swap into position low */
96 middle = (low + high) / 2;
97 if (arr[middle] > arr[high]) ELEM_SWAP(arr[middle], arr[high]) ;
98 if (arr[low] > arr[high]) ELEM_SWAP(arr[low], arr[high]) ;
99 if (arr[middle] > arr[low]) ELEM_SWAP(arr[middle], arr[low]) ;
100 /* Swap low item (now in position middle) into position (low+1) */
101 ELEM_SWAP(arr[middle], arr[low+1]) ;
102 /* Nibble from each end towards middle, swapping items when stuck */
103 ll = low + 1;
104 hh = high;
105 for (;;) {
106 do ll++; while (arr[low] > arr[ll]) ;
107 do hh--; while (arr[hh] > arr[low]) ;
108 if (hh < ll)
109 break;
110 ELEM_SWAP(arr[ll], arr[hh]) ;
111 }
112 /* Swap middle item (in position low) back into correct position */
113 ELEM_SWAP(arr[low], arr[hh]) ;
114 /* Re-set active partition */
115 if (hh <= median)
116 low = ll;
117 if (hh >= median)
118 high = hh - 1;
119 }
120 }
121 #undef ELEM_SWAP
122
123 PointCloud::PointCloud()
124 {
125 float fmax = numeric_limits<float>::max();
126 float fmin = -fmax;
127 m_min = Vec3f(fmax, fmax, fmax);
128 m_max = Vec3f(fmin, fmin, fmin);
129 }
130
131 PointCloud::PointCloud(Point *points, unsigned int s)
132 {
133 float fmax = numeric_limits<float>::max();
134 float fmin = -fmax;
135 m_min = Vec3f(fmax, fmax, fmax);
136 m_max = Vec3f(fmin, fmin, fmin);
137 std::copy(points, points + s, std::back_inserter(*this));
138 }
139
140 void PointCloud::reset(size_t s)
141 {
142 resize(s);
143 float fmax = numeric_limits<float>::max();
144 float fmin = -fmax;
145 m_min = Vec3f(fmax, fmax, fmax);
146 m_max = Vec3f(fmin, fmin, fmin);
147 }
148
149 float *PointCloud::getBbox () const
150 {
151 float *bbox = new float[6];
152 m_min.getValue(bbox[0], bbox[2], bbox[4]);
153 m_max.getValue(bbox[1], bbox[3], bbox[5]);
154
155 return bbox;
156 }
157
158 void PointCloud::GetCurrentBBox(Vec3f *min, Vec3f *max) const
159 {
160 *min = m_min;
161 *max = m_max;
162 }
163
164 void PointCloud::Translate(const Vec3f &trans)
165 {
166 for(size_t i = 0; i < size(); ++i)
167 at(i).pos += trans;
168 m_min += trans;
169 m_max += trans;
170 }
171
172 void PointCloud::calcNormals ( float radius, unsigned int kNN, unsigned int maxTries )
173 {
174 // cerr << "Begin calcNormals " << endl << flush;
175
176 KdTree3Df kd;
177 kd.IndexedData(begin(), end());
178 kd.Build();
179
180 GfxTL::LimitedHeap< GfxTL::NN< float > > nn;
181 KdTree3Df::NearestNeighborsAuxData< value_type > nnAux;
182 //GfxTL::AssumeUniqueLimitedHeap< GfxTL::NN< float > > nn;
183 MiscLib::NoShrinkVector< float > weights;
184
185 vector<int> stats(91, 0);
186 #ifdef PCA_NORMALS
187 GfxTL::Plane< GfxTL::Vector3Df > plane;
188 #endif
189 for ( unsigned int i = 0; i < size(); i ++ )
190 {
191 //kd.PointsInBall(at(i), radius, &nn);
192 //if(nn.size() > kNN)
193 //{
194 // std::sort(nn.begin(), nn.end());
195 // nn.resize(kNN);
196 //}
197 kd.NearestNeighbors(at(i), kNN, &nn, &nnAux);
198 unsigned int num = (unsigned int)nn.size();
199
200 //if(i%1000 == 0)
201 // cerr << num << " ";
202
203 if ( num > kNN )
204 num = kNN;
205
206 at(i).normal = Vec3f(0,0,0);
207 if ( num < 8 ) {
208
209 continue;
210 }
211
212 #ifdef PCA_NORMALS
213 weights.resize(nn.size());
214 if(nn.front().sqrDist > 0)
215 {
216 float h = nn.front().sqrDist / 2;
217 for(unsigned int i = 0; i < nn.size(); ++i)
218 weights[i] = std::exp(-nn[i].sqrDist / h);
219 }
220 else
221 std::fill(weights.begin(), weights.end(), 1.f);
222 plane.Fit(GfxTL::IndexIterate(nn.begin(), begin()),
223 GfxTL::IndexIterate(nn.end(), begin()), weights.begin());
224 at(i).normal = Vec3f(plane.Normal().Data());
225 #endif
226
227 #ifdef LMS_NORMALS
228 float score, bestScore = -1.f;
229 for (unsigned int tries = 0; tries < maxTries; tries++)
230 {
231 //choose candidate
232 int i0, i1, i2;
233 i0 = rand() % num;
234 do
235 i1 = rand() % num;
236 while (i1 == i0);
237 do
238 i2 = rand() % num;
239 while (i2 == i0 || i2 == i1);
240
241 Plane plane;
242 if(!plane.Init(at(nn[i0]), at(nn[i1]), at(nn[i2])))
243 continue;
244
245 //evaluate metric
246 float *dist = new float[num];
247 for (unsigned int j = 0; j < num; j++)
248 {
249 dist[j] = plane.getDistance(at(nn[j]));
250 }
251 // sort(dist, dist+num);
252 // score = dist[num/2]; // evaluate median
253 score = quick_select(dist, num); // evaluate median
254 delete[] dist;
255
256 if (score < bestScore || bestScore < 0.f)
257 {
258 if ( tries > maxTries/2 ) {
259 // let us see how good the first half of candidates are...
260 int index = std::floor(180/M_PI*std::acos(std::min(1.f, fabs(plane.getNormal().dot(at(i).normal)))));
261 stats[index]++;
262 }
263 at(i).normal = plane.getNormal();
264 bestScore = score;
265 }
266
267 }
268
269 #endif
270
271 }
272
273 //cerr << "End calcNormals " << endl << flush;
274 //copy(stats.begin(), stats.end(), ostream_iterator<int>(cerr, " "));
275 }