Debian Codegraph @

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

raw
1 #include "SpherePrimitiveShape.h"
2 #include "PrimitiveShapeVisitor.h"
3 #include "Bitmap.h"
4 #include <GfxTL/AABox.h>
5 #include <GfxTL/NullClass.h>
6 #include <iostream>
7 #include <fstream>
8 #include <sstream>
9 #include "IndexIterator.h"
10 #include <GfxTL/KdTree.h>
11 #include <GfxTL/BBoxDistanceKdTreeStrategy.h>
12 #include <GfxTL/CellRangeDataTreeStrategy.h>
13 #include <GfxTL/ImmediateTreeDataKernels.h>
14 #include <GfxTL/VectorKernel.h>
15 #include <GfxTL/NullTreeStrategy.h>
16 #include <GfxTL/IncrementalDistanceKdTreeStrategy.h>
17 #include <GfxTL/MaxIntervalSplittingKdTreeStrategy.h>
18 #include <GfxTL/CellBBoxBuildInformationKdTreeStrategy.h>
19 #include <GfxTL/BBoxBuildInformationTreeStrategy.h>
20 #include <GfxTL/BucketSizeSubdivisionTreeStrategy.h>
21 #include <GfxTL/L2Norm.h>
22 #include <GfxTL/NearestNeighbors.h>
23 #include <GfxTL/Mean.h>
24 #include <GfxTL/Covariance.h>
25 #include <GfxTL/IndexedIterator.h>
26 #include <GfxTL/Jacobi.h>
27 #include <MiscLib/Performance.h>
28 #include "PlanePrimitiveShape.h"
29 extern MiscLib::performance_t totalTime_sphereConnected;
30
31 SpherePrimitiveShape::SpherePrimitiveShape(const Sphere &s, float minRadius, float maxRadius)
32 : m_sphere(s)
33 , m_minRadius(minRadius)
34 , m_maxRadius(maxRadius)
35 , m_parametrization(m_sphere)
36 {}
37
38 SpherePrimitiveShape::SpherePrimitiveShape(const SpherePrimitiveShape &sps)
39 : BitmapPrimitiveShape(sps)
40 , m_sphere(sps.m_sphere)
41 , m_minRadius(sps.m_minRadius)
42 , m_maxRadius(sps.m_maxRadius)
43 , m_parametrization(sps.m_parametrization)
44 {
45 m_parametrization.Shape(m_sphere);
46 }
47
48 size_t SpherePrimitiveShape::Identifier() const
49 {
50 return 1;
51 }
52
53 bool SpherePrimitiveShape::Init(bool binary, std::istream *i)
54 {
55 // read the polygons but ignore them
56 GfxTL::AABox< GfxTL::Vector2Df > bboxUpper, bboxLower;
57 size_t upperuextent, uppervextent, loweruextent, lowervextent;
58 if(binary)
59 {
60 // read number of components
61 size_t size;
62 i->read((char *)&size, sizeof(size));
63 if(size)
64 {
65 // read upper bbox
66 i->read((char *)&bboxUpper, sizeof(bboxUpper));
67 // read upperuextent and uppervextent
68 i->read((char *)&upperuextent, sizeof(upperuextent));
69 i->read((char *)&uppervextent, sizeof(uppervextent));
70 for(size_t j = 0; j < size; ++j)
71 {
72 // read number of polys of component
73 size_t numPolys;
74 i->read((char *)&numPolys, sizeof(numPolys));
75 for(size_t k = 0; k < numPolys; ++k)
76 {
77 // read number of points in poly
78 size_t numPoints;
79 i->read((char *)&numPoints, sizeof(numPoints));
80 GfxTL::VectorXD< 2, size_t > pp;
81 for(size_t l = 0; l < numPoints; ++l)
82 i->read((char *)&pp, sizeof(pp));
83 }
84 }
85 }
86 // do the same for lower bitmap
87 // read number of components
88 i->read((char *)&size, sizeof(size));
89 if(size)
90 {
91 // read lower bbox
92 i->read((char *)&bboxLower, sizeof(bboxLower));
93 // read loweruextent and lowervextent
94 i->read((char *)&loweruextent, sizeof(loweruextent));
95 i->read((char *)&lowervextent, sizeof(lowervextent));
96 for(size_t j = 0; j < size; ++j)
97 {
98 // read number of polys of component
99 size_t numPolys;
100 i->read((char *)&numPolys, sizeof(numPolys));
101 for(size_t k = 0; k < numPolys; ++k)
102 {
103 // read number of points in poly
104 size_t numPoints;
105 i->read((char *)&numPoints, sizeof(numPoints));
106 GfxTL::VectorXD< 2, size_t > pp;
107 for(size_t l = 0; l < numPoints; ++l)
108 i->read((char *)&pp, sizeof(pp));
109 }
110 }
111 }
112 }
113 else
114 {
115 // read number of components
116 size_t size;
117 (*i) >> size;
118 if(size)
119 {
120 // read upper bbox
121 (*i) >> bboxUpper.Min()[0] >> bboxUpper.Max()[0]
122 >> bboxUpper.Min()[1] >> bboxUpper.Max()[1];
123 // read upperuextent and uppervextent
124 (*i) >> upperuextent >> uppervextent;
125 for(size_t j = 0; j < size; ++j)
126 {
127 // read number of polys of component
128 size_t numPolys;
129 (*i) >> numPolys;
130 for(size_t k = 0; k < numPolys; ++k)
131 {
132 // read number of points in poly
133 size_t numPoints;
134 (*i) >> numPoints;
135 GfxTL::VectorXD< 2, size_t > pp;
136 for(size_t l = 0; l < numPoints; ++l)
137 (*i) >> pp[0] >> pp[1];
138 }
139 }
140 }
141 // read number of components
142 (*i) >> size;
143 if(size)
144 {
145 // read lower bbox
146 (*i) >> bboxLower.Min()[0] >> bboxLower.Max()[0]
147 >> bboxLower.Min()[1] >> bboxLower.Max()[1];
148 // read loweruextent and lowervextent
149 (*i) >> loweruextent >> lowervextent;
150 for(size_t j = 0; j < size; ++j)
151 {
152 // read number of polys of component
153 size_t numPolys;
154 (*i) >> numPolys;
155 for(size_t k = 0; k < numPolys; ++k)
156 {
157 // read number of points in poly
158 size_t numPoints;
159 (*i) >> numPoints;
160 GfxTL::VectorXD< 2, size_t > pp;
161 for(size_t l = 0; l < numPoints; ++l)
162 (*i) >> pp[0] >> pp[1];
163 }
164 }
165 }
166 }
167 return true;
168 }
169
170 PrimitiveShape *SpherePrimitiveShape::Clone() const
171 {
172 return new SpherePrimitiveShape(*this);
173 }
174
175 float SpherePrimitiveShape::Distance(const Vec3f &p) const
176 {
177 return m_sphere.Distance(p);
178 }
179
180 float SpherePrimitiveShape::SignedDistance(const Vec3f &p) const
181 {
182 return m_sphere.SignedDistance(p);
183 }
184
185 float SpherePrimitiveShape::NormalDeviation(const Vec3f &p,
186 const Vec3f &n) const
187 {
188 Vec3f normal;
189 m_sphere.Normal(p, &normal);
190 return n.dot(normal);
191 }
192
193 void SpherePrimitiveShape::DistanceAndNormalDeviation(
194 const Vec3f &p, const Vec3f &n, std::pair< float, float > *dn) const
195 {
196 Vec3f normal;
197 dn->first = m_sphere.DistanceAndNormal(p, &normal);
198 dn->second = n.dot(normal);
199 }
200
201 void SpherePrimitiveShape::Project(const Vec3f &p, Vec3f *pp) const
202 {
203 m_sphere.Project(p, pp);
204 }
205
206 void SpherePrimitiveShape::Normal(const Vec3f &p, Vec3f *n) const
207 {
208 m_sphere.Normal(p, n);
209 }
210
211 unsigned int SpherePrimitiveShape::ConfidenceTests(unsigned int numTests,
212 float epsilon, float normalThresh, float rms, const PointCloud &pc,
213 const MiscLib::Vector< size_t > &indices) const
214 {
215 return BasePrimitiveShape::ConfidenceTests< Sphere >(numTests, epsilon,
216 normalThresh, rms, pc, indices);
217 }
218
219 void SpherePrimitiveShape::Description(std::string *s) const
220 {
221 *s = "Sphere";
222 }
223
224 bool SpherePrimitiveShape::Fit(const PointCloud &pc, float epsilon,
225 float normalThresh, MiscLib::Vector< size_t >::const_iterator begin,
226 MiscLib::Vector< size_t >::const_iterator end)
227 {
228 // do LS-fitting
229 Sphere fit = m_sphere;
230 if(fit.LeastSquaresFit(pc, begin, end))
231 {
232 m_sphere = fit;
233 m_parametrization.Shape(m_sphere);
234 return true;
235 }
236 return false;
237 }
238
239 PrimitiveShape *SpherePrimitiveShape::LSFit(const PointCloud &pc, float epsilon,
240 float normalThresh, MiscLib::Vector< size_t >::const_iterator begin,
241 MiscLib::Vector< size_t >::const_iterator end,
242 std::pair< size_t, float > *score) const
243 {
244 // do LS-fitting
245 Sphere fit = m_sphere;
246 if(fit.LeastSquaresFit(pc, begin, end))
247 {
248 score->first = -1;
249 return new SpherePrimitiveShape(fit);
250 }
251 score->first = 0;
252 return NULL;
253 }
254
255 void SpherePrimitiveShape::Serialize(std::ostream *o, bool binary) const
256 {
257 if(binary)
258 {
259 const char id = 1;
260 (*o) << id;
261 }
262 else
263 (*o) << "1" << " ";
264 m_sphere.Serialize(binary, o);
265 m_parametrization.Serialize(o, binary);
266 if(!binary)
267 *o << std::endl;
268 }
269
270 void SpherePrimitiveShape::Deserialize(std::istream *i, bool binary)
271 {
272 m_sphere.Init(binary, i);
273 m_parametrization.Shape(m_sphere);
274 m_parametrization.Deserialize(i, binary);
275 }
276
277 size_t SpherePrimitiveShape::SerializedSize() const
278 {
279 return m_sphere.SerializedSize()
280 + m_parametrization.SerializedSize() + 1;
281 }
282
283 LevMarFunc< float > *SpherePrimitiveShape::SignedDistanceFunc() const
284 {
285 return new SphereLevMarFunc(m_sphere);
286 }
287
288 void SpherePrimitiveShape::Transform(float scale, const Vec3f &translate)
289 {
290 m_sphere.Transform(scale, translate);
291 }
292
293 void SpherePrimitiveShape::Visit(PrimitiveShapeVisitor *visitor) const
294 {
295 visitor->Visit(*this);
296 }
297
298 void SpherePrimitiveShape::SuggestSimplifications(const PointCloud &pc,
299 MiscLib::Vector< size_t >::const_iterator begin,
300 MiscLib::Vector< size_t >::const_iterator end, float distThresh,
301 MiscLib::Vector< MiscLib::RefCountPtr< PrimitiveShape > > *suggestions) const
302 {
303 // sample the bounding box in parameter space at 25 locations
304 // these points are used to estimate the other shapes
305 // if the shapes succeed the suggestion is returned
306 MiscLib::Vector< Vec3f > samples;
307 samples.resize(2 * 25);
308 size_t c = samples.size() / 2;
309 float uStep = (m_extBbox.Max()[0] - m_extBbox.Min()[0]) / 4;
310 float vStep = (m_extBbox.Max()[1] - m_extBbox.Min()[1]) / 4;
311 float u = m_extBbox.Min()[0];
312 for(unsigned int i = 0; i < 5; ++i, u += uStep)
313 {
314 float v = m_extBbox.Min()[1];
315 for(unsigned int j = 0; j < 5; ++j, v += vStep)
316 m_parametrization.InSpace(u, v,
317 &samples[i * 5 + j], &samples[i * 5 + j + c]);
318 }
319
320 float d = 0;
321 float bestSum = 0;
322
323 for (size_t i = 0; i < c; ++i)
324 {
325 d = m_sphere.Distance(samples[i]);
326 bestSum += d;
327 }
328
329 Plane plane;
330 if(plane.LeastSquaresFit(samples.begin(), samples.begin() + c))
331 {
332 bool failed = false;
333 float sum = 0;
334 for (size_t i = 0; i < c; ++i)
335 {
336 d = plane.Distance(samples[i]);
337 sum += d;
338 if (d > distThresh)
339 {
340 failed = true;
341 break;
342 }
343 }
344 if (!failed)
345 {
346 if (sum < bestSum)
347 {
348 bestSum = sum;
349 suggestions->push_back(new PlanePrimitiveShape(plane));
350 suggestions->back()->Release();
351 }
352 }
353 }
354
355 /*// we suggest a plane if the radius is large enough so that the error
356 // along the two directions is less than distThresh
357 float ulength = 0, vlength = 0;
358 if(m_hasBitmap.first)
359 {
360 // has points on the upper side
361 ulength = m_extBboxUpper.Max()[0] - m_extBboxUpper.Min()[0];
362 vlength = m_extBboxUpper.Max()[1] - m_extBboxUpper.Min()[1];
363 }
364 if(m_hasBitmap.second)
365 {
366 ulength += m_extBboxLower.Max()[0] - m_extBboxLower.Min()[0];
367 vlength += m_extBboxLower.Max()[1] - m_extBboxLower.Min()[1];
368 }
369 float arcLength = M_PI * std::max(ulength, vlength);
370 float radiusDiff = (m_sphere.Radius() - std::sin(arcLength / 2)
371 * m_sphere.Radius()) / 2;
372 if(radiusDiff < distThresh)
373 {
374 Vec3f pos, normal;
375 SphereAsSquaresParametrization ssp(m_sphere, m_parametrizationNormal);
376 if(m_hasBitmap.first && m_hasBitmap.second)
377 {
378 GfxTL::Vector3Df center, eigenValues;
379 GfxTL::Mean(GfxTL::IndexIterate(begin, pc.begin()),
380 GfxTL::IndexIterate(end, pc.begin()), &center);
381 GfxTL::MatrixXX< 3, 3, float > cov, eigenVectors;
382 GfxTL::CovarianceMatrix(center, GfxTL::IndexIterate(begin, pc.begin()),
383 GfxTL::IndexIterate(end, pc.begin()), &cov);
384 GfxTL::Jacobi(cov, &eigenValues, &eigenVectors);
385 GfxTL::EigSortDesc(&eigenValues, &eigenVectors);
386 GfxTL::Vector3Df n = GfxTL::Vector3Df(eigenVectors[2]);
387 Plane plane(Vec3f(center.Data()), Vec3f(n.Data()));
388 suggestions->push_back(new PlanePrimitiveShape(plane));
389 suggestions->back()->Release();
390 }
391 else if(m_hasBitmap.first)
392 {
393 GfxTL::Vector2Df center;
394 m_extBboxUpper.Center(&center);
395 ssp.InSpace(std::make_pair(center[0], center[1]), false, &pos, &normal);
396 // offset position
397 pos -= radiusDiff * normal;
398 Plane plane(pos, normal);
399 suggestions->push_back(new PlanePrimitiveShape(plane));
400 suggestions->back()->Release();
401 }
402 else if(m_hasBitmap.second)
403 {
404 GfxTL::Vector2Df center;
405 m_extBboxUpper.Center(&center);
406 ssp.InSpace(std::make_pair(center[0], center[1]), true, &pos, &normal);
407 // offset position
408 pos -= radiusDiff * normal;
409 Plane plane(pos, normal);
410 suggestions->push_back(new PlanePrimitiveShape(plane));
411 suggestions->back()->Release();
412 }
413 }*/
414 }
415
416 void SpherePrimitiveShape::OptimizeParametrization(const PointCloud &pc,
417 MiscLib::Vector< size_t >::const_iterator begin,
418 MiscLib::Vector< size_t >::const_iterator end, float epsilon)
419 {
420 m_parametrization.Optimize(GfxTL::IndexIterate(begin, pc.begin()),
421 GfxTL::IndexIterate(end, pc.begin()), epsilon);
422 }
423
424 void SpherePrimitiveShape::OptimizeParametrization(const PointCloud &pc,
425 size_t begin, size_t end, float epsilon)
426 {
427 m_parametrization.Optimize(GfxTL::IndexIterate(IndexIterator(begin), pc.begin()),
428 GfxTL::IndexIterate(IndexIterator(end), pc.begin()), epsilon);
429 }
430
431 bool SpherePrimitiveShape::Similar(float tolerance,
432 const SpherePrimitiveShape &shape) const
433 {
434 return m_sphere.Radius() <= (1.f + tolerance) * shape.m_sphere.Radius()
435 && (1.f + tolerance) * m_sphere.Radius() >= shape.m_sphere.Radius();
436 }
437
438 void SpherePrimitiveShape::Parameters(const Vec3f &p,
439 std::pair< float, float > *param) const
440 {
441 m_parametrization.Parameters(p, param);
442 }
443
444 void SpherePrimitiveShape::Parameters(
445 GfxTL::IndexedIterator< MiscLib::Vector< size_t >::iterator,
446 PointCloud::const_iterator > begin,
447 GfxTL::IndexedIterator< MiscLib::Vector< size_t >::iterator,
448 PointCloud::const_iterator > end,
449 MiscLib::Vector< std::pair< float, float > > *bmpParams) const
450 {
451 ParametersImpl(begin, end, bmpParams);
452 }
453
454 void SpherePrimitiveShape::Parameters(
455 GfxTL::IndexedIterator< IndexIterator,
456 PointCloud::const_iterator > begin,
457 GfxTL::IndexedIterator< IndexIterator,
458 PointCloud::const_iterator > end,
459 MiscLib::Vector< std::pair< float, float > > *bmpParams) const
460 {
461 ParametersImpl(begin, end, bmpParams);
462 }
463
464 bool SpherePrimitiveShape::InSpace(float u, float v, Vec3f *p, Vec3f *n) const
465 {
466 return m_parametrization.InSpace(u, v, p, n);
467 }
468
469 void SpherePrimitiveShape::BitmapExtent(float epsilon,
470 GfxTL::AABox< GfxTL::Vector2Df > *bbox,
471 MiscLib::Vector< std::pair< float, float > > *params,
472 size_t *uextent, size_t *vextent)
473 {
474 *uextent = static_cast<size_t>(std::ceil((bbox->Max()[0] - bbox->Min()[0]) / epsilon));
475 *vextent = static_cast<size_t>(std::ceil((bbox->Max()[1] - bbox->Min()[1]) / epsilon));
476 }
477
478 void SpherePrimitiveShape::InBitmap(const std::pair< float, float > &param,
479 float epsilon, const GfxTL::AABox< GfxTL::Vector2Df > &bbox,
480 size_t uextent, size_t vextent,
481 std::pair< int, int > *inBmp) const
482 {
483 inBmp->first = static_cast<int>(std::floor((param.first - bbox.Min()[0]) / epsilon));
484 inBmp->second = static_cast<int>(std::floor((param.second - bbox.Min()[1]) / epsilon));
485 }
486
487 void SpherePrimitiveShape::WrapBitmap(
488 const GfxTL::AABox< GfxTL::Vector2Df > &bbox,
489 float epsilon, bool *uwrap, bool *vwrap) const
490 {
491 m_parametrization.WrapBitmap(bbox, epsilon, uwrap, vwrap);
492 }
493
494 void SpherePrimitiveShape::WrapComponents(const GfxTL::AABox< GfxTL::Vector2Df > &bbox,
495 float epsilon, size_t uextent, size_t vextent,
496 MiscLib::Vector< int > *componentImg,
497 MiscLib::Vector< std::pair< int, size_t > > *labels) const
498 {
499 m_parametrization.WrapComponents(bbox, epsilon, uextent, vextent,
500 componentImg, labels);
501 }
502
503 bool SpherePrimitiveShape::InSpace(size_t u, size_t v, float epsilon,
504 const GfxTL::AABox< GfxTL::Vector2Df > &bbox, size_t uextent,
505 size_t vextent, Vec3f *p, Vec3f *n) const
506 {
507 return m_parametrization.InSpace((u + .5f) * epsilon + bbox.Min()[0],
508 (v + .5f) * epsilon + bbox.Min()[1], p, n);
509 }