Debian Codegraph @

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

raw
1 #include "Sphere.h"
2 #include <MiscLib/Performance.h>
3 #include <GfxTL/IndexedIterator.h>
4 #include <GfxTL/MathHelper.h>
5 #ifdef DOPARALLEL
6 #include <omp.h>
7 #endif
8
9 extern int dmat_solve ( int n, int rhs_num, double a[] );
10
11 void tetrahedron_circumsphere_3d ( double tetra[3*4], double *r, double pc[3] )
12
13 //******************************************************************************
14 //
15 // Purpose:
16 //
17 // TETRAHEDRON_CIRCUMSPHERE_3D computes the circumsphere of a tetrahedron in 3D.
18 //
19 // Discussion:
20 //
21 // The circumsphere, or circumscribed sphere, of a tetrahedron is the sphere that
22 // passes through the four vertices. The circumsphere is not necessarily
23 // the smallest sphere that contains the tetrahedron.
24 //
25 // Surprisingly, the diameter of the sphere can be found by solving
26 // a 3 by 3 linear system. This is because the vectors P2 - P1,
27 // P3 - P1 and P4 - P1 are secants of the sphere, and each forms a
28 // right triangle with the diameter through P1. Hence, the dot product of
29 // P2 - P1 with that diameter is equal to the square of the length
30 // of P2 - P1, and similarly for P3 - P1 and P4 - P1. This determines
31 // the diameter vector originating at P1, and hence the radius and
32 // center.
33 //
34 // Modified:
35 //
36 // 10 August 2005
37 //
38 // Author:
39 //
40 // John Burkardt
41 //
42 // Reference:
43 //
44 // Adrian Bowyer and John Woodwark,
45 // A Programmer's Geometry,
46 // Butterworths, 1983.
47 //
48 // Parameters:
49 //
50 // Input, double TETRA[3*4], the vertices of the tetrahedron.
51 //
52 // Output, double *R, PC[3], the coordinates of the center of the
53 // circumscribed sphere, and its radius. If the linear system is
54 // singular, then R = -1, PC[] = 0.
55 //
56 {
57 # define DIM_NUM 3
58 # define RHS_NUM 1
59
60 double a[DIM_NUM*(DIM_NUM+RHS_NUM)];
61 int info;
62 //
63 // Set up the linear system.
64 //
65 a[0+0*3] = tetra[0+1*3] - tetra[0+0*3];
66 a[0+1*3] = tetra[1+1*3] - tetra[1+0*3];
67 a[0+2*3] = tetra[2+1*3] - tetra[2+0*3];
68 a[0+3*3] = std::pow ( tetra[0+1*3] - tetra[0+0*3], 2 )
69 + std::pow ( tetra[1+1*3] - tetra[1+0*3], 2 )
70 + std::pow ( tetra[2+1*3] - tetra[2+0*3], 2 );
71
72 a[1+0*3] = tetra[0+2*3] - tetra[0+0*3];
73 a[1+1*3] = tetra[1+2*3] - tetra[1+0*3];
74 a[1+2*3] = tetra[2+2*3] - tetra[2+0*3];
75 a[1+3*3] = std::pow ( tetra[0+2*3] - tetra[0+0*3], 2 )
76 + std::pow ( tetra[1+2*3] - tetra[1+0*3], 2 )
77 + std::pow ( tetra[2+2*3] - tetra[2+0*3], 2 );
78
79 a[2+0*3] = tetra[0+3*3] - tetra[0+0*3];
80 a[2+1*3] = tetra[1+3*3] - tetra[1+0*3];
81 a[2+2*3] = tetra[2+3*3] - tetra[2+0*3];
82 a[2+3*3] = std::pow ( tetra[0+3*3] - tetra[0+0*3], 2 )
83 + std::pow ( tetra[1+3*3] - tetra[1+0*3], 2 )
84 + std::pow ( tetra[2+3*3] - tetra[2+0*3], 2 );
85 //
86 // Solve the linear system.
87 //
88 info = dmat_solve ( DIM_NUM, RHS_NUM, a );
89 //
90 // If the system was singular, return a consolation prize.
91 //
92 if ( info != 0 )
93 {
94 *r = -1.0;
95 for(size_t i = 0; i < DIM_NUM; ++i)
96 pc[i] = 0;
97 //dvec_zero ( DIM_NUM, pc );
98 return;
99 }
100 //
101 // Compute the radius and center.
102 //
103 *r = 0.5 * std::sqrt
104 ( a[0+3*3] * a[0+3*3]
105 + a[1+3*3] * a[1+3*3]
106 + a[2+3*3] * a[2+3*3] );
107
108 pc[0] = tetra[0+0*3] + 0.5 * a[0+3*3];
109 pc[1] = tetra[1+0*3] + 0.5 * a[1+3*3];
110 pc[2] = tetra[2+0*3] + 0.5 * a[2+3*3];
111
112 return;
113 # undef DIM_NUM
114 # undef RHS_NUM
115 }
116
117 bool Midpoint(const Vec3f &p1, const Vec3f &n1, const Vec3f &p2, const Vec3f &n2,
118 Vec3f *mid)
119 {
120 float d1343,d4321,d1321,d4343,d2121;
121 float numer,denom, mua, mub;
122
123 Vec3f p13 = p1 - p2;
124 // p43 = n2
125 // p21 = n1
126 d1343 = p13[0] * n2[0] + p13[1] * n2[1] + p13[2] * n2[2];
127 d4321 = n2[0] * n1[0] + n2[1] * n1[1] + n2[2] * n1[2];
128 d1321 = p13[0] * n1[0] + p13[1] * n1[1] + p13[2] * n1[2];
129 d4343 = n2[0] * n2[0] + n2[1] * n2[1] + n2[2] * n2[2];
130 d2121 = n1[0] * n1[0] + n1[1] * n1[1] + n1[2] * n1[2];
131
132 denom = d2121 * d4343 - d4321 * d4321;
133 if(fabs(denom) < 1.0e-6)
134 return false;
135 numer = d1343 * d4321 - d1321 * d4343;
136
137 mua = numer / denom;
138 mub = (d1343 + d4321 * (mua)) / d4343;
139
140 Vec3f pa, pb;
141 pa = p1 + mua * n1;
142 pb = p2 + mub * n2;
143 *mid = 0.5f * (pa + pb);
144 return true;
145 }
146
147 InvalidTetrahedonError::InvalidTetrahedonError()
148 : std::runtime_error("Invalid tetrahedon")
149 {}
150
151 Sphere::Sphere()
152 {}
153
154 Sphere::Sphere(const Vec3f &center, float radius)
155 : m_center(center)
156 , m_radius(radius)
157 {}
158
159 Sphere::Sphere(const Vec3f &p1, const Vec3f &p2, const Vec3f &p3,
160 const Vec3f &p4)
161 {
162 if(!Init(p1, p2, p3, p4))
163 throw InvalidTetrahedonError();
164 }
165
166 bool Sphere::Init(const MiscLib::Vector< Vec3f > &samples)
167 {
168 if(samples.size() < 4)
169 return false;
170 // get center
171 size_t c = samples.size() / 2;
172 m_center = Vec3f(0, 0, 0);
173 size_t midCount = 0;
174 for(size_t i = 0; i < c - 1; ++i)
175 for(size_t j = i + 1; j < c; ++j)
176 {
177 Vec3f mid;
178 if(!Midpoint(samples[i], samples[i + c], samples[j], samples[j + c], &mid))
179 continue;
180 m_center += mid;
181 ++midCount;
182 }
183 if(!midCount)
184 return false;
185 m_center /= static_cast<float>(midCount);
186 m_radius = 0;
187 for(size_t i = 0; i < c; ++i)
188 {
189 float d = (samples[i] - m_center).length();
190 m_radius += d;
191 }
192 m_radius /= c;
193 return true;
194 }
195
196 bool Sphere::Init(const Vec3f &p1, const Vec3f &p2, const Vec3f &p3,
197 const Vec3f &p4)
198 {
199 // convert to double array
200 double tetra[4 * 3];
201 for(size_t i = 0; i < 3; ++i)
202 tetra[0 * 3 + i] = p1[i];
203 for(size_t i = 0; i < 3; ++i)
204 tetra[1 * 3 + i] = p2[i];
205 for(size_t i = 0; i < 3; ++i)
206 tetra[2 * 3 + i] = p3[i];
207 for(size_t i = 0; i < 3; ++i)
208 tetra[3 * 3 + i] = p4[i];
209 double r, pc[3];
210 tetrahedron_circumsphere_3d(tetra, &r, pc);
211 if(r < 0)
212 return false;
213 m_radius = static_cast<float>(r);
214 m_center[0] = static_cast<float>(pc[0]);
215 m_center[1] = static_cast<float>(pc[1]);
216 m_center[2] = static_cast<float>(pc[2]);
217 return true;
218 }
219
220 bool Sphere::Init2(const Vec3f &p1, const Vec3f &p2, const Vec3f &n1,
221 const Vec3f &n2)
222 {
223 /*
224 Calculate the line segment PaPb that is the shortest route between
225 two lines P1P2 and P3P4. Calculate also the values of mua and mub where
226 Pa = P1 + mua (P2 - P1)
227 Pb = P3 + mub (P4 - P3)
228 Return FALSE if no solution exists.
229 */
230 float d1343,d4321,d1321,d4343,d2121;
231 float numer,denom, mua, mub;
232
233 Vec3f p13 = p1 - p2;
234 // p43 = n2
235 // p21 = n1
236 d1343 = p13[0] * n2[0] + p13[1] * n2[1] + p13[2] * n2[2];
237 d4321 = n2[0] * n1[0] + n2[1] * n1[1] + n2[2] * n1[2];
238 d1321 = p13[0] * n1[0] + p13[1] * n1[1] + p13[2] * n1[2];
239 d4343 = n2[0] * n2[0] + n2[1] * n2[1] + n2[2] * n2[2];
240 d2121 = n1[0] * n1[0] + n1[1] * n1[1] + n1[2] * n1[2];
241
242 denom = d2121 * d4343 - d4321 * d4321;
243 if(fabs(denom) < 1.0e-6)
244 return false;
245 numer = d1343 * d4321 - d1321 * d4343;
246
247 mua = numer / denom;
248 mub = (d1343 + d4321 * (mua)) / d4343;
249
250 Vec3f pa, pb;
251 pa = p1 + mua * n1;
252 pb = p2 + mub * n2;
253
254 // get the midpoint between pa and pb and make it the center
255 m_center = 0.5f * (pa + pb);
256 // make the radius the average of the two distances
257 float da = (p1 - m_center).length();
258 float db = (p2 - m_center).length();
259 m_radius = 0.5f * (da + db);
260 // do some plausability checks
261 // lets say the actual distance should not deviate by more than 10%
262 float dev = da / m_radius;
263 if(dev < 0.9f || dev > 1.1f)
264 return false;
265 dev = db / m_radius;
266 if(dev < 0.9f || dev > 1.1f)
267 return false;
268 // distance between pa and pb should not be greater than 10% of the radius
269 dev = (pa - pb).length() / m_radius;
270 if(dev > 0.1f)
271 return false;
272 return true;
273 }
274
275 bool Sphere::Init(bool binary, std::istream *i)
276 {
277 if(binary)
278 {
279 i->read((char *)&m_center, sizeof(m_center));
280 i->read((char *)&m_radius, sizeof(m_radius));
281 }
282 else
283 {
284 for(size_t j = 0; j < 3; ++j)
285 (*i) >> m_center[j];
286 (*i) >> m_radius;
287 }
288 return true;
289 }
290
291 void Sphere::Init(FILE *i)
292 {
293 size_t readrtn; //unused return warning suppresion
294 readrtn = fread(&m_center, sizeof(m_center), 1, i);
295 readrtn = fread(&m_radius, sizeof(m_radius), 1, i);
296 }
297
298 void Sphere::Init(float *array)
299 {
300 for(int i=0;i<3;i++){
301 m_center[i] = array[i];
302 }
303 m_radius=array[3];
304 }
305
306 void Sphere::Project(const Vec3f &p, Vec3f *pp) const
307 {
308 *pp = p - m_center;
309 float l = pp->length();
310 *pp *= m_radius / l;
311 *pp += m_center;
312 }
313
314 const Vec3f &Sphere::Center() const
315 {
316 return m_center;
317 }
318
319 float Sphere::Radius() const
320 {
321 return m_radius;
322 }
323
324 float SphereDistance(const float *param, const float *x)
325 {
326 float s = x[0] - param[0];
327 s *= s;
328 for(unsigned int i = 1; i < 3; ++i)
329 {
330 float ss = x[i] - param[i];
331 s += ss * ss;
332 }
333 return std::sqrt(s) - param[3];
334 }
335
336 void SphereDistanceDerivatives(const float *param, const float *x,
337 float *gradient)
338 {
339 float s[3];
340 s[0] = x[0] - param[0];
341 float sl = s[0] * s[0];
342 for(unsigned int i = 1; i < 3; ++i)
343 {
344 s[i] = x[i] - param[i];
345 sl += s[i] * s[i];
346 }
347 sl = std::sqrt(sl);
348 gradient[0] = -s[0] / sl;
349 gradient[1] = -s[1] / sl;
350 gradient[2] = -s[2] / sl;
351 gradient[3] = -1;
352 }
353
354 void NormalizeSphereParams(float *param)
355 {}
356
357 bool Sphere::LeastSquaresFit(const PointCloud &pc,
358 MiscLib::Vector< size_t >::const_iterator begin,
359 MiscLib::Vector< size_t >::const_iterator end)
360 {
361 bool retVal = LeastSquaresFit(GfxTL::IndexIterate(begin, pc.begin()),
362 GfxTL::IndexIterate(end, pc.begin()));
363 return retVal;
364 }
365
366 bool Sphere::Interpolate(const MiscLib::Vector< Sphere > &spheres,
367 const MiscLib::Vector< float > &weights, Sphere *is)
368 {
369 Vec3f center(0, 0, 0);
370 float radius = 0;
371 for(size_t i = 0; i < spheres.size(); ++i)
372 {
373 center += weights[i] * spheres[i].Center();
374 radius += weights[i] * spheres[i].Radius();
375 }
376 is->Center(center);
377 is->Radius(radius);
378 return true;
379 }
380
381 void Sphere::Serialize(bool binary, std::ostream *o) const
382 {
383 if(binary)
384 {
385 o->write((const char *)&m_center, sizeof(m_center));
386 o->write((const char *)&m_radius, sizeof(m_radius));
387 }
388 else
389 {
390 (*o) << m_center[0] << " " << m_center[1] << " " << m_center[2] << " "
391 << m_radius << " ";
392 }
393 }
394
395 size_t Sphere::SerializedSize()
396 {
397 return sizeof(Vec3f)
398 + sizeof(float);
399 }
400
401 size_t Sphere::SerializedFloatSize()
402 {
403 return 4;
404 }
405
406 void Sphere::Serialize(FILE *o) const
407 {
408 fwrite(&m_center, sizeof(m_center), 1, o);
409 fwrite(&m_radius, sizeof(m_radius), 1, o);
410 }
411
412 void Sphere::Serialize(float *array) const
413 {
414 for(int i=0;i<3;i++){
415 array[i] = m_center[i];
416 }
417 array[3] = m_radius;
418 }
419
420
421 void Sphere::Transform(float scale, const Vec3f &translate)
422 {
423 m_center *= scale;
424 m_center += translate;
425 m_radius *= scale;
426 }
427
428 SphereAsSquaresParametrization::SphereAsSquaresParametrization(const Sphere &sphere,
429 const Vec3f &planeNormal)
430 : m_sphere(sphere)
431 , m_planeNormal(planeNormal)
432 , m_hcs(GfxTL::Vector3Df(planeNormal))
433 {}
434
435 void SphereAsSquaresParametrization::Init(const Sphere &sphere,
436 const Vec3f &planeNormal)
437 {
438 m_sphere = sphere;
439 m_planeNormal = planeNormal;
440 m_hcs.FromNormal(planeNormal[0], planeNormal[1], planeNormal[2]);
441 }
442
443 float SphereAsSquaresParametrization::Parameters(const Vec3f &p,
444 std::pair< float, float > *param) const
445 {
446 // convert to hemisphere coordinates
447 Vec3f s = p - m_sphere.Center();
448 s.normalize();
449 Vec3f hs;
450 hs[0] = s.dot(m_hcs[0].Data());
451 hs[1] = s.dot(m_hcs[1].Data());
452 hs[2] = s.dot(m_planeNormal);
453 float ret = hs[2];
454 hs[2] = fabs(hs[2]);
455 std::pair< float, float > inDisk;
456 Hemisphere2Disk(hs, &inDisk);
457 Disk2Square(inDisk, param);
458 return ret;
459 }
460
461 bool SphereAsSquaresParametrization::InSpace(
462 const std::pair< float, float > &param, bool lower, Vec3f *p) const
463 {
464 if(param.first < -0.1 || param.first > 1.1
465 || param.second < -0.1 || param.second > 1.1)
466 return false;
467 std::pair< float, float > clampedParam;
468 clampedParam.first = GfxTL::Math< float >::Clamp(param.first, 0, 1);
469 clampedParam.second = GfxTL::Math< float >::Clamp(param.second, 0, 1);
470 std::pair< float, float > inDisk;
471 Square2Disk(clampedParam, &inDisk);
472 Vec3f s;
473 Disk2Hemisphere(inDisk, &s);
474 *p = Vec3f((s[0] * m_hcs[0] + s[1] * m_hcs[1] +
475 GfxTL::Vector3Df((lower? -1 : 1) * s[2] * m_planeNormal)).Data());
476 *p *= m_sphere.Radius();
477 *p += m_sphere.Center();
478 return true;
479 }
480
481 bool SphereAsSquaresParametrization::InSpace(
482 const std::pair< float, float > &param, bool lower, Vec3f *p,
483 Vec3f *n) const
484 {
485 if(param.first < -0.1 || param.first > 1.1
486 || param.second < -0.1 || param.second > 1.1)
487 return false;
488 std::pair< float, float > clampedParam;
489 clampedParam.first = GfxTL::Math< float >::Clamp(param.first, 0, 1);
490 clampedParam.second = GfxTL::Math< float >::Clamp(param.second, 0, 1);
491 std::pair< float, float > inDisk;
492 Square2Disk(clampedParam, &inDisk);
493 Vec3f s;
494 Disk2Hemisphere(inDisk, &s);
495 if(lower)
496 s[2] *= -1;
497 *n = Vec3f((s[0] * m_hcs[0] + s[1] * m_hcs[1] +
498 GfxTL::Vector3Df(s[2] * m_planeNormal)).Data());
499 *p = m_sphere.Radius() * (*n);
500 *p += m_sphere.Center();
501 return true;
502 }
503
504 void SphereAsSquaresParametrization::Transform(
505 const GfxTL::MatrixXX< 3, 3, float > &rot, const GfxTL::Vector3Df &trans)
506 {
507 m_sphere = Sphere(Vec3f((rot * GfxTL::Vector3Df(m_sphere.Center())
508 + trans).Data()), m_sphere.Radius());
509 m_planeNormal = Vec3f((rot * GfxTL::Vector3Df(m_planeNormal)).Data());
510 m_hcs[0] = rot * m_hcs[0];
511 m_hcs[1] = rot * m_hcs[1];
512 }
513
514 void SphereAsSquaresParametrization::HyperplaneCoordinateSystem( Vec3f* hcs0, Vec3f* hcs1, Vec3f* hcs2 ) const
515 {
516 hcs0->setValue( m_hcs[0] );
517 hcs1->setValue( m_hcs[1] );
518 hcs2->setValue( m_hcs[2] );
519 }
520
521
522 void SphereAsSquaresParametrization::Hemisphere2Disk(const Vec3f &p,
523 std::pair< float, float > *inDisk) const
524 {
525 inDisk->first = std::sqrt(1 - p[2]);
526 inDisk->second = std::atan2(p[1], p[0]);
527 }
528
529 void SphereAsSquaresParametrization::Disk2Square(
530 const std::pair< float, float > &inDisk,
531 std::pair< float, float > *inSquare) const
532 {
533 float r = inDisk.first;
534 float phi = inDisk.second;
535 float a, b;
536
537 if(phi < float(-M_PI / 4.0))
538 phi += float(2 * M_PI);
539
540 if(phi < float(M_PI / 4.0))
541 {
542 a = r;
543 b = phi * a / float(M_PI / 4.0);
544 }
545 else if(phi < float( 3 * M_PI / 4.0))
546 {
547 b = r;
548 a = -(phi - float(M_PI / 2.0))* b / float(M_PI / 4.0);
549 }
550 else if(phi < float(5 * M_PI / 4.0))
551 {
552 a = -r;
553 b = (phi - float(M_PI))*a / float(M_PI / 4.0);
554 }
555 else
556 {
557 b = -r;
558 a = -(phi - float(3 * M_PI / 2.0))* b /float(M_PI / 4.0);
559 }
560
561 inSquare->first = (a + float(1.0)) / float(2.0);
562 inSquare->second = (b + float(1.0)) / float(2.0);
563 }
564
565 void SphereAsSquaresParametrization::Square2Disk(
566 const std::pair< float, float > &inSquare,
567 std::pair< float, float > *inDisk) const
568 {
569 float phi, r;
570 float a = 2 * inSquare.first - 1;
571 float b = 2 * inSquare.second - 1;
572
573 if(a > -b)
574 {
575 if(a > b)
576 {
577 r = a;
578 phi = float(M_PI / 4.0) * (b / a);
579 }
580 else
581 {
582 r = b;
583 phi = float(M_PI / 4.0) * (2 - (a / b));
584 }
585 }
586 else
587 {
588 if(a < b)
589 {
590 r = -a;
591 phi = float(M_PI / 4.0) * (4 + (b / a));
592 }
593 else
594 {
595 r = -b;
596 if(b != 0)
597 phi = float(M_PI / 4.0) * (6 - (a / b));
598 else
599 phi = 0;
600 }
601 }
602
603 inDisk->first = r;
604 inDisk->second = phi;
605 }
606
607 void SphereAsSquaresParametrization::Disk2Hemisphere(
608 const std::pair< float, float > &inDisk, Vec3f *p) const
609 {
610 (*p)[0] = inDisk.first * std::sqrt(2 - inDisk.first * inDisk.first)
611 * std::cos(inDisk.second);
612 (*p)[1] = inDisk.first * std::sqrt(2 - inDisk.first * inDisk.first)
613 * std::sin(inDisk.second);
614 (*p)[2] = 1 - inDisk.first * inDisk.first;
615 }