Debian Codegraph @

plugins/core/Standard/qRANSAC_SD/extern/RANSAC_SD/Cone.h

raw
1 #ifndef CONE_HEADER
2 #define CONE_HEADER
3 #ifdef DOPARALLEL
4 #include <omp.h>
5 #endif
6 #include "basic.h"
7 #include "PointCloud.h"
8 #include <GfxTL/HyperplaneCoordinateSystem.h>
9 #include <stdexcept>
10 #include <ostream>
11 #include <istream>
12 #include <stdio.h>
13 #if !defined(_WIN32) && !defined(WIN32)
14 #include <unistd.h>
15 #endif
16 #include <MiscLib/NoShrinkVector.h>
17 #include "LevMarLSWeight.h"
18 #include "LevMarFitting.h"
19
20 #ifndef DLL_LINKAGE
21 #define DLL_LINKAGE
22 #endif
23
24 // This implements a one sided cone!
25 class DLL_LINKAGE Cone
26 {
27 public:
28 struct ConeInfo
29 {
30 Vec3f center;
31 Vec3f axisDirection;
32 float angle;
33 Vec3f minPoint;
34 Vec3f maxPoint;
35 float height;
36 float minHeight;
37 float maxHeight;
38 float minRadius;
39 float maxRadius;
40 };
41 struct ParallelPlanesError
42 : public std::runtime_error
43 {
44 ParallelPlanesError()
45 : std::runtime_error("Parallel planes in cone construction")
46 {}
47 };
48 enum { RequiredSamples = 3 };
49 Cone();
50 Cone(const Vec3f &center, const Vec3f &axisDir, float angle);
51 Cone(const Vec3f &p1, const Vec3f &p2, const Vec3f &p3,
52 const Vec3f &n1, const Vec3f &n2, const Vec3f &n3);
53 bool Init(const MiscLib::Vector< Vec3f > &samples);
54 bool InitAverage(const MiscLib::Vector< Vec3f > &samples);
55 bool Init(const Vec3f &center, const Vec3f &axisDir, float angle);
56 bool Init(const Vec3f &p1, const Vec3f &p2, const Vec3f &p3,
57 const Vec3f &n1, const Vec3f &n2, const Vec3f &n3);
58 bool Init(bool binary, std::istream *i);
59 void Init(FILE *i);
60 void Init(float* array);
61 inline float Distance(const Vec3f &p) const;
62 inline void Normal(const Vec3f &p, Vec3f *n) const;
63 inline float DistanceAndNormal(const Vec3f &p, Vec3f *n) const;
64 inline float SignedDistance(const Vec3f &p) const;
65 inline float SignedDistanceAndNormal(const Vec3f &p, Vec3f *n) const;
66 void Project(const Vec3f &p, Vec3f *pp) const;
67 // Paramterizes into (length, angle)
68 void Parameters(const Vec3f &p,
69 std::pair< float, float > *param) const;
70 inline float Height(const Vec3f &p) const;
71 inline float Angle() const;
72 inline const Vec3f &Center() const;
73 inline const Vec3f &AxisDirection() const;
74 Vec3f &AxisDirection() { return m_axisDir; }
75 inline const Vec3f AngularDirection() const;
76 //void AngularDirection(const Vec3f &angular);
77 void RotateAngularDirection(float radians);
78 inline float RadiusAtLength(float length) const;
79 bool LeastSquaresFit(const PointCloud &pc,
80 MiscLib::Vector< size_t >::const_iterator begin,
81 MiscLib::Vector< size_t >::const_iterator end);
82 template< class IteratorT >
83 bool LeastSquaresFit(IteratorT begin, IteratorT end);
84 bool Fit(const PointCloud &pc,
85 MiscLib::Vector< size_t >::const_iterator begin,
86 MiscLib::Vector< size_t >::const_iterator end)
87 { return LeastSquaresFit(pc, begin, end); }
88 static bool Interpolate(const MiscLib::Vector< Cone > &cones,
89 const MiscLib::Vector< float > &weights, Cone *ic);
90 void Serialize(bool binary, std::ostream *o) const;
91 static size_t SerializedSize();
92 void Serialize(FILE *o) const;
93 void Serialize(float* array) const;
94 static size_t SerializedFloatSize();
95 void Transform(float scale, const Vec3f &translate);
96 void Transform(const GfxTL::MatrixXX< 3, 3, float > &rot,
97 const GfxTL::Vector3Df &trans);
98 inline unsigned int Intersect(const Vec3f &p, const Vec3f &r,
99 float lambda[2], Vec3f interPts[2]) const;
100 ConeInfo GetInfo(const PointCloud& pc, size_t startingIndx, size_t endingIndx) const;
101 ConeInfo GetInfo(const MiscLib::Vector< Vec3f >& samples) const;
102 private:
103 template< class WeightT >
104 class LevMarCone
105 : public WeightT
106 {
107 public:
108 enum { NumParams = 7 };
109 typedef float ScalarType;
110
111 template< class IteratorT >
112 ScalarType Chi(const ScalarType *params, IteratorT begin, IteratorT end,
113 ScalarType *values, ScalarType *temp) const
114 {
115 ScalarType chi = 0;
116 ScalarType cosPhi = std::cos(params[6]);
117 ScalarType sinPhi = std::sin(params[6]);
118 size_t size = end - begin;
119 #ifdef DOPARALLEL
120 #pragma omp parallel for schedule(static) reduction(+:chi)
121 #endif
122 for(int idx = 0; idx < size; ++idx)
123 {
124 Vec3f s;
125 for(unsigned int j = 0; j < 3; ++j)
126 s[j] = begin[idx][j] - params[j];
127 ScalarType g = fabs(s[0] * params[3] + s[1] * params[4] + s[2] * params[5]);
128 ScalarType f = s.sqrLength() - (g * g);
129 if(f <= 0)
130 f = 0;
131 else
132 f = std::sqrt(f);
133 temp[idx] = f;
134 chi += (values[idx] = WeightT::Weigh(cosPhi * f - sinPhi * g))
135 * values[idx];
136 }
137 return chi;
138 }
139
140 template< class IteratorT >
141 void Derivatives(const ScalarType *params, IteratorT begin, IteratorT end,
142 const ScalarType *values, const ScalarType *temp, ScalarType *matrix) const
143 {
144 ScalarType sinPhi = -std::sin(params[6]);
145 ScalarType cosPhi = std::cos(params[6]);
146 size_t size = end - begin;
147 #ifdef DOPARALLEL
148 #pragma omp parallel for schedule(static)
149 #endif
150 for(int idx = 0; idx < size; ++idx)
151 {
152 Vec3f s;
153 for(unsigned int j = 0; j < 3; ++j)
154 s[j] = begin[idx][j] - params[j];
155 ScalarType g = fabs(s[0] * params[3] + s[1] * params[4] + s[2] * params[5]);
156 ScalarType ggradient[6];
157 for(unsigned int j = 0; j < 3; ++j)
158 ggradient[j] = -params[j + 3];
159 for(unsigned int j = 0; j < 3; ++j)
160 ggradient[j + 3] = s[j] - params[j + 3] * g;
161 ScalarType fgradient[6];
162 if(temp[idx] < 1.0e-6)
163 {
164 fgradient[0] = std::sqrt(1 - params[3] * params[3]);
165 fgradient[1] = std::sqrt(1 - params[4] * params[4]);
166 fgradient[2] = std::sqrt(1 - params[5] * params[5]);
167 }
168 else
169 {
170 fgradient[0] = (params[3] * g - s[0]) / temp[idx];
171 fgradient[1] = (params[4] * g - s[1]) / temp[idx];
172 fgradient[2] = (params[5] * g - s[2]) / temp[idx];
173 }
174 fgradient[3] = g * fgradient[0];
175 fgradient[4] = g * fgradient[1];
176 fgradient[5] = g * fgradient[2];
177 for(unsigned int j = 0; j < 6; ++j)
178 matrix[idx * NumParams + j] =
179 cosPhi * fgradient[j] + sinPhi * ggradient[j];
180 matrix[idx * NumParams + 6] = temp[idx] * sinPhi - g * cosPhi;
181 WeightT::template DerivWeigh< NumParams >(cosPhi * temp[idx] + sinPhi * g,
182 matrix + idx * NumParams);
183 }
184 }
185
186 void Normalize(float *params) const
187 {
188 // normalize direction
189 ScalarType l = std::sqrt(params[3] * params[3] + params[4] * params[4] +
190 params[5] * params[5]);
191 for(unsigned int i = 3; i < 6; ++i)
192 params[i] /= l;
193 // normalize angle
194 params[6] -= std::floor(params[6] / (2 * ScalarType(M_PI))) * (2 * ScalarType(M_PI)); // params[6] %= 2*M_PI
195 if(params[6] > M_PI)
196 {
197 params[6] -= std::floor(params[6] / ScalarType(M_PI)) * ScalarType(M_PI); // params[6] %= M_PI
198 for(unsigned int i = 3; i < 6; ++i)
199 params[i] *= -1;
200 }
201 if(params[6] > ScalarType(M_PI) / 2)
202 params[6] = ScalarType(M_PI) - params[6];
203 }
204 };
205
206 private:
207 Vec3f m_center; // this is the apex of the cone
208 Vec3f m_axisDir; // the axis points into the interior of the cone
209 float m_angle; // the opening angle
210 Vec3f m_normal;
211 Vec3f m_normalY; // precomputed normal part
212 float m_n2d[2];
213 GfxTL::HyperplaneCoordinateSystem< float, 3 > m_hcs;
214 float m_angularRotatedRadians;
215 };
216
217 inline float Cone::Distance(const Vec3f &p) const
218 {
219 // this is for one sided cone!
220 Vec3f s = p - m_center;
221 float g = s.dot(m_axisDir); // distance to plane orhogonal to
222 // axisdir through center
223 // distance to axis
224 float sqrS = s.sqrLength();
225 float f = sqrS - (g * g);
226 if(f <= 0)
227 f = 0;
228 else
229 f = std::sqrt(f);
230 float da = m_n2d[0] * f;
231 float db = m_n2d[1] * g;
232 if(g < 0 && da - db < 0) // is inside other side of cone -> disallow
233 return std::sqrt(sqrS);
234 return fabs(da + db);
235 }
236
237 inline void Cone::Normal(const Vec3f &p, Vec3f *n) const
238 {
239 Vec3f s = p - m_center;
240 Vec3f pln = s.cross(m_axisDir);
241 Vec3f plx = m_axisDir.cross(pln);
242 plx.normalize();
243 // we are not dealing with two-sided cone
244 *n = m_normal[0] * plx + m_normalY;
245 }
246
247 inline float Cone::DistanceAndNormal(const Vec3f &p, Vec3f *n) const
248 {
249 // this is for one-sided cone !!!
250 Vec3f s = p - m_center;
251 float g = s.dot(m_axisDir); // distance to plane orhogonal to
252 // axisdir through center
253 // distance to axis
254 float sqrS = s.sqrLength();
255 float f = sqrS - (g * g);
256 if(f <= 0)
257 f = 0;
258 else
259 f = std::sqrt(f);
260 float da = m_n2d[0] * f;
261 float db = m_n2d[1] * g;
262 float dist;
263 if(g < 0 && da - db < 0) // is inside other side of cone -> disallow
264 dist = std::sqrt(sqrS);
265 else
266 dist = fabs(da + db);
267 // need normal
268 Vec3f plx = s - g * m_axisDir;
269 plx.normalize();
270 *n = m_normal[0] * plx + m_normalY;
271 return dist;
272 }
273
274 inline float Cone::SignedDistance(const Vec3f &p) const
275 {
276 // this is for one sided cone!
277 Vec3f s = p - m_center;
278 float g = s.dot(m_axisDir); // distance to plane orhogonal to
279 // axisdir through center
280 // distance to axis
281 float sqrS = s.sqrLength();
282 float f = sqrS - (g * g);
283 if(f <= 0)
284 f = 0;
285 else
286 f = std::sqrt(f);
287 float da = m_n2d[0] * f;
288 float db = m_n2d[1] * g;
289 if(g < 0 && da - db < 0) // is inside other side of cone -> disallow
290 return std::sqrt(sqrS);
291 return da + db;
292 }
293
294 inline float Cone::SignedDistanceAndNormal(const Vec3f &p, Vec3f *n) const
295 {
296 // this is for one-sided cone !!!
297 Vec3f s = p - m_center;
298 float g = s.dot(m_axisDir); // distance to plane orhogonal to
299 // axisdir through center
300 // distance to axis
301 float sqrS = s.sqrLength();
302 float f = sqrS - (g * g);
303 if(f <= 0)
304 f = 0;
305 else
306 f = std::sqrt(f);
307 float da = m_n2d[0] * f;
308 float db = m_n2d[1] * g;
309 float dist;
310 if(g < 0 && da - db < 0) // is inside other side of cone -> disallow
311 dist = std::sqrt(sqrS);
312 else
313 dist = da + db;
314 // need normal
315 Vec3f plx = s - g * m_axisDir;
316 plx.normalize();
317 *n = m_normal[0] * plx + m_normalY;
318 return dist;
319 }
320
321 float Cone::Angle() const
322 {
323 return m_angle;
324 }
325
326 const Vec3f &Cone::Center() const
327 {
328 return m_center;
329 }
330
331 const Vec3f &Cone::AxisDirection() const
332 {
333 return m_axisDir;
334 }
335
336 const Vec3f Cone::AngularDirection() const
337 {
338 return Vec3f(m_hcs[0].Data());
339 }
340
341 float Cone::RadiusAtLength(float length) const
342 {
343 return std::sin(m_angle) * fabs(length);
344 }
345
346 float Cone::Height(const Vec3f &p) const
347 {
348 Vec3f s = p - m_center;
349 return m_axisDir.dot(s);
350 }
351
352 template< class IteratorT >
353 bool Cone::LeastSquaresFit(IteratorT begin, IteratorT end)
354 {
355 float param[7];
356 for(unsigned int i = 0; i < 3; ++i)
357 param[i] = m_center[i];
358 for(unsigned int i = 0; i < 3; ++i)
359 param[i + 3] = m_axisDir[i];
360 param[6] = m_angle;
361 LevMarCone< LevMarLSWeight > levMarCone;
362 if(!LevMar(begin, end, levMarCone, param))
363 return false;
364 if(param[6] < 1.0e-6 || param[6] > float(M_PI) / 2 - 1.0e-6)
365 return false;
366 for(unsigned int i = 0; i < 3; ++i)
367 m_center[i] = param[i];
368 for(unsigned int i = 0; i < 3; ++i)
369 m_axisDir[i] = param[i + 3];
370 m_angle = param[6];
371 m_normal = Vec3f(std::cos(-m_angle), std::sin(-m_angle), 0);
372 m_normalY = m_normal[1] * m_axisDir;
373 m_n2d[0] = std::cos(m_angle);
374 m_n2d[1] = -std::sin(m_angle);
375 m_hcs.FromNormal(m_axisDir);
376 m_angularRotatedRadians = 0;
377 // it could be that the axis has flipped during fitting
378 // we need to detect such a case
379 // for this we run over all points and compute the sum
380 // of their respective heights. If that sum is negative
381 // the axis needs to be flipped.
382 float heightSum = 0;
383 intptr_t size = end - begin;
384 //#ifndef _WIN64 // for some reason the Microsoft x64 compiler crashes at the next line
385 #ifdef DOPARALLEL
386 #pragma omp parallel for schedule(static) reduction(+:heightSum)
387 #endif
388 //#endif
389 for(intptr_t i = 0; i < size; ++i)
390 heightSum += Height(begin[i]);
391 if(heightSum < 0)
392 {
393 m_axisDir *= -1;
394 m_hcs.FromNormal(m_axisDir);
395 }
396 return true;
397 }
398
399 inline unsigned int Cone::Intersect(const Vec3f &p, const Vec3f &r,
400 float lambda[2], Vec3f interPts[2]) const
401 {
402 // Set up the quadratic Q(t) = c2*t^2 + 2*c1*t + c0 that corresponds to
403 // the cone. Let the vertex be V, the unit-length direction vector be A,
404 // and the angle measured from the cone axis to the cone wall be Theta,
405 // and define g = cos(Theta). A point X is on the cone wall whenever
406 // Dot(A,(X-V)/|X-V|) = g. Square this equation and factor to obtain
407 // (X-V)^T * (A*A^T - g^2*I) * (X-V) = 0
408 // where the superscript T denotes the transpose operator. This defines
409 // a double-sided cone. The line is L(t) = P + t*D, where P is the line
410 // origin and D is a unit-length direction vector. Substituting
411 // X = L(t) into the cone equation above leads to Q(t) = 0. Since we
412 // want only intersection points on the single-sided cone that lives in
413 // the half-space pointed to by A, any point L(t) generated by a root of
414 // Q(t) = 0 must be tested for Dot(A,L(t)-V) >= 0.
415 using namespace std;
416 float fAdD = m_axisDir.dot(r);
417 float tmp = cos(m_angle);
418 float fCosSqr = tmp*tmp;
419 Vec3f kE = p - m_center;
420 float fAdE = m_axisDir.dot(kE);
421 float fDdE = r.dot(kE);
422 float fEdE = kE.dot(kE);
423 float fC2 = fAdD*fAdD - fCosSqr;
424 float fC1 = fAdD*fAdE - fCosSqr*fDdE;
425 float fC0 = fAdE*fAdE - fCosSqr*fEdE;
426 float fdot;
427
428 // Solve the quadratic. Keep only those X for which Dot(A,X-V) >= 0.
429 unsigned int interCount = 0;
430 if (fabs(fC2) >= 1e-7)
431 {
432 // c2 != 0
433 float fDiscr = fC1*fC1 - fC0*fC2;
434 if (fDiscr < 0)
435 {
436 // Q(t) = 0 has no real-valued roots. The line does not
437 // intersect the double-sided cone.
438 return 0;
439 }
440 else if (fDiscr > 1e-7)
441 {
442 // Q(t) = 0 has two distinct real-valued roots. However, one or
443 // both of them might intersect the portion of the double-sided
444 // cone "behind" the vertex. We are interested only in those
445 // intersections "in front" of the vertex.
446 float fRoot = sqrt(fDiscr);
447 float fInvC2 = 1.0f/fC2;
448 interCount = 0;
449
450 float fT = (-fC1 - fRoot)*fInvC2;
451 if(fT > 0) // intersect only in positive direction of ray
452 {
453 interPts[interCount] = p + fT*r;
454 kE = interPts[interCount] - m_center;
455 fdot = kE.dot(m_axisDir);
456 if (fdot > 0)
457 {
458 lambda[interCount] = fT;
459 interCount++;
460 }
461 }
462
463 fT = (-fC1 + fRoot)*fInvC2;
464 if(fT > 0)
465 {
466 interPts[interCount] = p + fT*r;
467 kE = interPts[interCount] - m_center;
468 fdot = kE.dot(m_axisDir);
469 if (fdot > 0)
470 {
471 lambda[interCount] = fT;
472 interCount++;
473 }
474 }
475 }
476 else if(fC1 / fC2 < 0)
477 {
478 // one repeated real root (line is tangent to the cone)
479 interPts[0] = p - (fC1/fC2)*r;
480 lambda[0] = -(fC1 / fC2);
481 kE = interPts[0] - m_center;
482 if (kE.dot(m_axisDir) > 0)
483 interCount = 1;
484 else
485 interCount = 0;
486 }
487 }
488 else if (fabs(fC1) >= 1.0e-7f)
489 {
490 // c2 = 0, c1 != 0 (D is a direction vector on the cone boundary)
491 lambda[0] = -(0.5f * fC0/fC1);
492 if(lambda[0] < 0)
493 return 0;
494 interPts[0] = p + lambda[0] *r;
495 kE = interPts[0] - m_center;
496 fdot = kE.dot(m_axisDir);
497 if (fdot > 0)
498 interCount = 1;
499 else
500 interCount = 0;
501 }
502 else if (abs(fC0) >= 1e-7)
503 {
504 // c2 = c1 = 0, c0 != 0
505 interCount = 0;
506 }
507 else
508 {
509 // c2 = c1 = c0 = 0, cone contains ray V+t*D where V is cone vertex
510 // and D is the line direction.
511 interCount = 1;
512 lambda[0] = (m_center - p).dot(r);
513 interPts[0] = m_center;
514 }
515
516 return interCount;
517 }
518
519 #endif