Debian Codegraph @

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

raw
1 #include "CylinderPrimitiveShape.h"
2 #include "ScoreComputer.h"
3 #include "PrimitiveShapeVisitor.h"
4 #include <limits>
5 #ifndef _USE_MATH_DEFINES
6 #define _USE_MATH_DEFINES
7 #endif
8 #include <cmath>
9 #include <GfxTL/NullClass.h>
10 #include <iostream>
11 #include <algorithm>
12 #include <MiscLib/Performance.h>
13 #include "TorusPrimitiveShape.h"
14 #include "ConePrimitiveShape.h"
15 #include "SpherePrimitiveShape.h"
16 #include "PlanePrimitiveShape.h"
17 extern MiscLib::performance_t totalTime_cylinderConnected;
18
19 CylinderPrimitiveShape::CylinderPrimitiveShape()
20 : m_clip(false)
21 , m_minPhi(0)
22 , m_maxPhi(0)
23 , m_minRadius(-std::numeric_limits<float>::infinity())
24 , m_maxRadius(std::numeric_limits<float>::infinity())
25 , m_maxLength(std::numeric_limits<float>::infinity())
26 {}
27
28 CylinderPrimitiveShape::CylinderPrimitiveShape(const Cylinder &cylinder, float minRadius, float maxRadius, float maxLength)
29 : m_cylinder(cylinder)
30 , m_clip(false)
31 , m_minPhi(0)
32 , m_maxPhi(0)
33 , m_minRadius(minRadius)
34 , m_maxRadius(maxRadius)
35 , m_maxLength(maxLength)
36 {}
37
38 size_t CylinderPrimitiveShape::Identifier() const
39 {
40 return 2;
41 }
42
43 PrimitiveShape *CylinderPrimitiveShape::Clone() const
44 {
45 return new CylinderPrimitiveShape(*this);
46 }
47
48 bool CylinderPrimitiveShape::Init(const Vec3f &pointA, const Vec3f &pointB,
49 const Vec3f &normalA, const Vec3f &normalB)
50 {
51 return m_cylinder.Init(pointA, pointB, normalA, normalB);
52 }
53
54 float CylinderPrimitiveShape::Distance(const Vec3f &p) const
55 {
56 return m_cylinder.Distance(p);
57 }
58
59 float CylinderPrimitiveShape::SignedDistance(const Vec3f &p) const
60 {
61 return m_cylinder.SignedDistance(p);
62 }
63
64 float CylinderPrimitiveShape::NormalDeviation(const Vec3f &p,
65 const Vec3f &n) const
66 {
67 Vec3f normal;
68 m_cylinder.Normal(p, &normal);
69 return n.dot(normal);
70 }
71
72 void CylinderPrimitiveShape::DistanceAndNormalDeviation(
73 const Vec3f &p, const Vec3f &n, std::pair< float, float > *dn) const
74 {
75 Vec3f normal;
76 dn->first = m_cylinder.DistanceAndNormal(p, &normal);
77 dn->second = n.dot(normal);
78 }
79
80 void CylinderPrimitiveShape::Project(const Vec3f &p, Vec3f *pp) const
81 {
82 m_cylinder.Project(p, pp);
83 }
84
85 void CylinderPrimitiveShape::Normal(const Vec3f &p, Vec3f *n) const
86 {
87 m_cylinder.Normal(p, n);
88 }
89
90 unsigned int CylinderPrimitiveShape::ConfidenceTests(unsigned int numTests,
91 float epsilon, float normalThresh, float rms, const PointCloud &pc,
92 const MiscLib::Vector< size_t > &indices) const
93 {
94 return BasePrimitiveShape::ConfidenceTests< Cylinder >(numTests, epsilon,
95 normalThresh, rms, pc, indices);
96 }
97
98 void CylinderPrimitiveShape::Description(std::string *s) const
99 {
100 *s = "Cylinder";
101 }
102
103 bool CylinderPrimitiveShape::Fit(const PointCloud &pc, float epsilon,
104 float normalThresh, MiscLib::Vector< size_t >::const_iterator begin,
105 MiscLib::Vector< size_t >::const_iterator end)
106
107 {
108 Cylinder fit = m_cylinder;
109 if(fit.LeastSquaresFit(pc, begin, end))
110 {
111 m_cylinder = fit;
112 return true;
113 }
114 return false;
115 }
116
117 PrimitiveShape *CylinderPrimitiveShape::LSFit(const PointCloud &pc,
118 float epsilon, float normalThresh,
119 MiscLib::Vector< size_t >::const_iterator begin,
120 MiscLib::Vector< size_t >::const_iterator end,
121 std::pair< size_t, float > *score) const
122 {
123 Cylinder fit = m_cylinder;
124 if(fit.LeastSquaresFit(pc, begin, end))
125 {
126 score->first = -1;
127 return new CylinderPrimitiveShape(fit, m_maxRadius, m_maxLength);
128 }
129 score->first = 0;
130 return NULL;
131 }
132
133 LevMarFunc< float > *CylinderPrimitiveShape::SignedDistanceFunc() const
134 {
135 return new CylinderLevMarFunc(m_cylinder);
136 }
137
138 void CylinderPrimitiveShape::Serialize(std::ostream *o, bool binary) const
139 {
140 if(binary)
141 {
142 const char id = 2;
143 (*o) << id;
144 }
145 else
146 (*o) << "2" << " ";
147 m_cylinder.Serialize(binary, o);
148 if(!binary)
149 *o << std::endl;
150 }
151
152 size_t CylinderPrimitiveShape::SerializedSize() const
153 {
154 return m_cylinder.SerializedSize() + 1;
155 }
156
157 void CylinderPrimitiveShape::Transform(float scale, const Vec3f &translate)
158 {
159 m_cylinder.Transform(scale, translate);
160 }
161
162 void CylinderPrimitiveShape::Transform(
163 const GfxTL::MatrixXX< 3, 3, float > &rot, const GfxTL::Vector3Df &trans)
164 {
165 m_cylinder.Transform(rot, trans);
166 }
167
168 void CylinderPrimitiveShape::Visit(PrimitiveShapeVisitor *visitor) const
169 {
170 visitor->Visit(*this);
171 }
172
173 void CylinderPrimitiveShape::SuggestSimplifications(const PointCloud &pc,
174 MiscLib::Vector< size_t >::const_iterator begin,
175 MiscLib::Vector< size_t >::const_iterator end, float distThresh,
176 MiscLib::Vector< MiscLib::RefCountPtr< PrimitiveShape > > *suggestions) const
177 {
178 // sample the bounding box in parameter space at 25 locations
179 // these points are used to estimate the other shapes
180 // if the shapes succeed the suggestion is returned
181 MiscLib::Vector< Vec3f > samples(2 * 25);
182 float uStep = (m_extBbox.Max()[0] - m_extBbox.Min()[0]) / 4;
183 float vStep = (m_extBbox.Max()[1] - m_extBbox.Min()[1]) / 4;
184 float u = m_extBbox.Min()[0];
185 for(unsigned int i = 0; i < 5; ++i, u += uStep)
186 {
187 float v = m_extBbox.Min()[1];
188 for(unsigned int j = 0; j < 5; ++j, v += vStep)
189 InSpace(u, v * m_cylinder.Radius(), &samples[i * 5 + j],
190 &samples[i * 5 + j + 25]);
191 }
192 size_t c = samples.size() / 2;
193 float d = 0;
194 float bestSum = 0;
195
196 for (size_t i = 0; i < c; ++i)
197 {
198 d = m_cylinder.Distance(samples[i]);
199 bestSum += d;
200 }
201
202 // now check all the shape types
203 Sphere sphere;
204 if(sphere.Init(samples))
205 {
206 sphere.LeastSquaresFit(samples.begin(), samples.begin() + c);
207 bool failed = false;
208 float sum = 0;
209 for (size_t i = 0; i < c; ++i)
210 {
211 d = sphere.Distance(samples[i]);
212 sum += d;
213 if (d > distThresh)
214 {
215 failed = true;
216 break;
217 }
218 }
219 if (!failed)
220 {
221 if (sum < bestSum)
222 {
223 bestSum = sum;
224 suggestions->push_back(new SpherePrimitiveShape(sphere));
225 suggestions->back()->Release();
226 }
227 }
228 }
229 Plane plane;
230 if(plane.LeastSquaresFit(samples.begin(), samples.begin() + c))
231 {
232 bool failed = false;
233 float sum = 0;
234 for(size_t i = 0; i < c; ++i)
235 {
236 d = plane.Distance(samples[i]);
237 sum += d;
238 if (d > distThresh)
239 {
240 failed = true;
241 break;
242 }
243 }
244 if (!failed)
245 {
246 if (sum < bestSum)
247 {
248 bestSum = sum;
249 suggestions->push_back(new PlanePrimitiveShape(plane));
250 suggestions->back()->Release();
251 }
252 }
253 }
254 /*// We suggest a sphere if a curvature of radius along the height
255 // does not introduce too large an error
256 float length = m_extBbox.Max()[0] - m_extBbox.Min()[0];
257 float meanLength = (m_extBbox.Max()[0] + m_extBbox.Min()[0]) / 2;
258 float radiusDiff = (std::sqrt(m_cylinder.Radius() * m_cylinder.Radius()
259 + length * length / 4) - m_cylinder.Radius()) / 2;
260 float radialExtent = m_extBbox.Max()[1] - m_extBbox.Min()[1];
261 if(radiusDiff < distThresh)
262 {
263 // the center of the sphere is given as the point on the axis
264 // with the height of the mean length
265 Vec3f center = meanLength * m_cylinder.AxisDirection()
266 + m_cylinder.AxisPosition();
267 Sphere sphere(center, m_cylinder.Radius() + radiusDiff);
268 suggestions->push_back(new SpherePrimitiveShape(sphere));
269 suggestions->back()->Release();
270 }
271
272 // We suggest a plane if the mean radius causes only a small error
273 // for this we need the angular extent in the curved direction of the cone
274 radiusDiff = (m_cylinder.Radius() - std::cos(radialExtent / 2)
275 * m_cylinder.Radius()) / 2;
276 if(radiusDiff < distThresh)
277 {
278 GfxTL::Vector2Df bboxCenter;
279 m_extBbox.Center(&bboxCenter);
280 Vec3f pos, normal;
281 InSpace(bboxCenter[0], bboxCenter[1] * m_cylinder.Radius(),
282 &pos, &normal);
283 // offset position
284 pos -= radiusDiff * normal;
285 Plane plane(pos, normal);
286 suggestions->push_back(new PlanePrimitiveShape(plane));
287 suggestions->back()->Release();
288 }*/
289 }
290
291 bool CylinderPrimitiveShape::Similar(float tolerance,
292 const CylinderPrimitiveShape &shape) const
293 {
294 return m_cylinder.Radius() <= (1.f + tolerance) * shape.m_cylinder.Radius()
295 && (1.f + tolerance) * m_cylinder.Radius() >= shape.m_cylinder.Radius();
296 }
297
298 float CylinderPrimitiveShape::Height() const
299 {
300 return std::abs(m_extBbox.Max()[0] - m_extBbox.Min()[0]);
301 }
302
303 float CylinderPrimitiveShape::MinHeight() const
304 {
305 return m_extBbox.Min()[0];
306 }
307
308 float CylinderPrimitiveShape::MaxHeight() const
309 {
310 return m_extBbox.Max()[0];
311 }
312
313 void CylinderPrimitiveShape::Parameters(const Vec3f &p,
314 std::pair< float, float > *param) const
315 {
316 m_cylinder.Parameters(p, param);
317 // convert angle to arc length
318 param->second *= m_cylinder.Radius();
319 }
320
321 void CylinderPrimitiveShape::Parameters(
322 GfxTL::IndexedIterator< MiscLib::Vector< size_t >::iterator,
323 PointCloud::const_iterator > begin,
324 GfxTL::IndexedIterator< MiscLib::Vector< size_t >::iterator,
325 PointCloud::const_iterator > end,
326 MiscLib::Vector< std::pair< float, float > > *bmpParams) const
327 {
328 ParametersImpl(begin, end, bmpParams);
329 }
330
331 void CylinderPrimitiveShape::Parameters(
332 GfxTL::IndexedIterator< IndexIterator,
333 PointCloud::const_iterator > begin,
334 GfxTL::IndexedIterator< IndexIterator,
335 PointCloud::const_iterator > end,
336 MiscLib::Vector< std::pair< float, float > > *bmpParams) const
337 {
338 ParametersImpl(begin, end, bmpParams);
339 }
340
341 void CylinderPrimitiveShape::BitmapExtent(float epsilon,
342 GfxTL::AABox< GfxTL::Vector2Df > *bbox,
343 MiscLib::Vector< std::pair< float, float > > *params,
344 size_t *uextent, size_t *vextent)
345 {
346 *uextent = size_t(std::ceil(
347 (bbox->Max()[0] - bbox->Min()[0]) / epsilon));
348 *vextent = size_t(std::ceil((bbox->Max()[1] - bbox->Min()[1]) / epsilon));
349 if((*vextent) * (*uextent) > 1e6)
350 {
351 // try to reparameterize
352 if(bbox->Min()[1] > epsilon && bbox->Max()[1] < 2 * M_PI * m_cylinder.Radius() - epsilon)
353 return; // there is no wrapping -> we can't do anything
354 MiscLib::Vector< float > angularParams(params->size());
355 for(size_t i = 0; i < params->size(); ++i)
356 angularParams[i] = (*params)[i].second;
357 std::sort(angularParams.begin(), angularParams.end());
358 // try to find a large gap
359 float maxGap = 0;
360 float lower = 0.0f, upper = 0.0f;
361 for(size_t i = 1; i < angularParams.size(); ++i)
362 {
363 float gap = angularParams[i] - angularParams[i - 1];
364 if(gap > maxGap)
365 {
366 maxGap = gap;
367 lower = angularParams[i - 1];
368 upper = angularParams[i];
369 }
370 }
371 if(maxGap > epsilon)
372 {
373 // reparameterize with new angular cut
374 float newCut = (lower + upper) / 2.f;
375 m_cylinder.RotateAngularDirection(newCut / m_cylinder.Radius());
376 bbox->Min()[1] = std::numeric_limits< float >::infinity();
377 bbox->Max()[1] = -std::numeric_limits< float >::infinity();
378 for(size_t i = 0; i < params->size(); ++i)
379 {
380 (*params)[i].second -= newCut;
381 if((*params)[i].second < 0)
382 (*params)[i].second = static_cast<float>(2 * M_PI * m_cylinder.Radius() + (*params)[i].second);
383 if((*params)[i].second < bbox->Min()[1])
384 bbox->Min()[1] = (*params)[i].second;
385 if((*params)[i].second > bbox->Max()[1])
386 bbox->Max()[1] = (*params)[i].second;
387 }
388 *vextent = size_t(std::ceil((bbox->Max()[1] - bbox->Min()[1]) / epsilon));
389 }
390 }
391 }
392
393 void CylinderPrimitiveShape::InBitmap(const std::pair< float, float > &param,
394 float epsilon, const GfxTL::AABox< GfxTL::Vector2Df > &bbox,
395 size_t uextent, size_t vextent, std::pair< int, int > *inBmp) const
396 {
397 // convert the parameters to bitmap coordinates
398 inBmp->first = static_cast<int>(std::floor((param.first - bbox.Min()[0]) / epsilon));
399 inBmp->second = static_cast<int>(std::floor((param.second - bbox.Min()[1]) / epsilon));
400 }
401 void CylinderPrimitiveShape::WrapBitmap(
402 const GfxTL::AABox< GfxTL::Vector2Df > &bbox, float epsilon, bool *uwrap,
403 bool *vwrap) const
404 {
405 *uwrap = false;
406 if(static_cast<size_t>(bbox.Max()[1]) - bbox.Min()[1]
407 >= 2 * M_PI * m_cylinder.Radius() - static_cast<double>(epsilon) * 2)
408 *vwrap = true; // wrap around angular component
409 else
410 *vwrap = false;
411 }
412
413 void CylinderPrimitiveShape::PreWrapBitmap(const GfxTL::AABox< GfxTL::Vector2Df > &bbox,
414 float epsilon, size_t uextent, size_t vextent, MiscLib::Vector< char > *bmp) const
415 {
416 // wraps the bitmpap around the v-axis
417 // note: we do not check, if the cylinder is really wrapped around !
418 // Use WrapBitmap for this check
419 for (int i=0; i < uextent; i++)
420 {
421 char t = (*bmp)[i];
422 bmp->push_back(t);
423 }
424 }
425
426 void CylinderPrimitiveShape::SetExtent(
427 const GfxTL::AABox< GfxTL::Vector2Df > &extBbox,
428 const MiscLib::Vector< int > &componentsImg, size_t uextent,
429 size_t vextent, float epsilon, int label)
430 {
431 if(extBbox.Min()[1] * m_cylinder.Radius() <= epsilon
432 && static_cast<size_t>(extBbox.Max()[1]) * m_cylinder.Radius()
433 >= 2 * M_PI * m_cylinder.Radius() - epsilon)
434 {
435 // component has been cut along angular direction
436 // run from both sides to find both ends
437 size_t row = 0, j = 0;
438 for(; j < vextent; ++j)
439 {
440 bool found = false;
441 for(size_t i = 0; i < uextent; ++i)
442 {
443 if(componentsImg[row + i] == label)
444 {
445 found = true;
446 break;
447 }
448 }
449 if(!found)
450 break;
451 row += uextent;
452 }
453 size_t maxj = j;
454 if(maxj == vextent) // cylinder is complete
455 {
456 m_clip = false;
457 return;
458 }
459 row = (vextent - 1) * uextent, j = 0;
460 for(; j < vextent; ++j)
461 {
462 bool found = false;
463 for(size_t i = 0; i < uextent; ++i)
464 {
465 if(componentsImg[row + i] == label)
466 {
467 found = true;
468 break;
469 }
470 }
471 if(!found)
472 break;
473 row -= uextent;
474 }
475 size_t minj = j;
476 // convert min and max to angular parameters
477 m_minPhi = minj * epsilon / m_cylinder.Radius() + extBbox.Min()[1];
478 m_maxPhi = maxj * epsilon / m_cylinder.Radius() + extBbox.Min()[1];
479 }
480 else
481 {
482 m_minPhi = extBbox.Min()[1];
483 m_maxPhi = extBbox.Max()[1];
484 }
485 m_clip = true;
486 }
487
488 bool CylinderPrimitiveShape::InSpace(float u, float v, Vec3f *p, Vec3f *n) const
489 {
490 GfxTL::Quaternion< float > q;
491 q.RotationRad(v / m_cylinder.Radius(), m_cylinder.AxisDirection()[0],
492 m_cylinder.AxisDirection()[1], m_cylinder.AxisDirection()[2]);
493 Vec3f vvec;
494 q.Rotate(m_cylinder.AngularDirection(), &vvec);
495 *p = u * m_cylinder.AxisDirection() + m_cylinder.Radius() * vvec
496 + m_cylinder.AxisPosition();
497 *n = vvec;
498 return true;
499 }
500
501 bool CylinderPrimitiveShape::InSpace(size_t u, size_t v, float epsilon,
502 const GfxTL::AABox< GfxTL::Vector2Df > &bbox, size_t uextent,
503 size_t vextent, Vec3f *p, Vec3f *n) const
504 {
505 GfxTL::Quaternion< float > q;
506 q.RotationRad((bbox.Min()[1] + epsilon * (v + .5f)) / m_cylinder.Radius(),
507 m_cylinder.AxisDirection()[0],
508 m_cylinder.AxisDirection()[1],
509 m_cylinder.AxisDirection()[2]);
510 Vec3f vvec;
511 q.Rotate(m_cylinder.AngularDirection(), &vvec);
512 *p = (bbox.Min()[0] + epsilon * (u + .5f)) * m_cylinder.AxisDirection() +
513 m_cylinder.Radius() * vvec + m_cylinder.AxisPosition();
514 *n = vvec;
515 return true;
516 }