Debian Codegraph @

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

raw
1 #ifndef SPHERE_HEADER
2 #define SPHERE_HEADER
3 #include "basic.h"
4 #include <MiscLib/Vector.h>
5 #include <stdexcept>
6 #include <GfxTL/HyperplaneCoordinateSystem.h>
7 #include <utility>
8 #include "PointCloud.h"
9 #include <ostream>
10 #include <istream>
11 #include <stdio.h>
12 #include <utility>
13 #include <MiscLib/NoShrinkVector.h>
14 #include "LevMarLSWeight.h"
15 #include "LevMarFitting.h"
16
17 #ifndef DLL_LINKAGE
18 #define DLL_LINKAGE
19 #endif
20
21 struct DLL_LINKAGE InvalidTetrahedonError
22 : public std::runtime_error
23 {
24 InvalidTetrahedonError();
25 };
26
27 class DLL_LINKAGE Sphere
28 {
29 public:
30 enum { RequiredSamples = 2 };
31 Sphere();
32 Sphere(const Vec3f &center, float radius);
33 Sphere(const Vec3f &p1, const Vec3f &p2, const Vec3f &p3,
34 const Vec3f &p4);
35 bool Init(const MiscLib::Vector< Vec3f > &samples);
36 bool Init(const Vec3f &p1, const Vec3f &p2, const Vec3f &p3,
37 const Vec3f &p4);
38 bool Init2(const Vec3f &p1, const Vec3f &p2, const Vec3f &n1,
39 const Vec3f &n2);
40 bool Init(bool binary, std::istream *i);
41 void Init(FILE *i);
42 void Init(float *array);
43 inline float Distance(const Vec3f &p) const;
44 inline void Normal(const Vec3f &p, Vec3f *normal) const;
45 inline float DistanceAndNormal(const Vec3f &p, Vec3f *normal) const;
46 inline float SignedDistance(const Vec3f &p) const;
47 void Project(const Vec3f &p, Vec3f *pp) const;
48 const Vec3f &Center() const;
49 void Center(const Vec3f &center) { m_center = center; }
50 float Radius() const;
51 void Radius(float radius) { m_radius = radius; }
52 bool LeastSquaresFit(const PointCloud &pc,
53 MiscLib::Vector< size_t >::const_iterator begin,
54 MiscLib::Vector< size_t >::const_iterator end);
55 template< class IteratorT >
56 bool LeastSquaresFit(IteratorT begin, IteratorT end);
57 bool Fit(const PointCloud &pc,
58 MiscLib::Vector< size_t >::const_iterator begin,
59 MiscLib::Vector< size_t >::const_iterator end)
60 { return LeastSquaresFit(pc, begin, end); }
61 static bool Interpolate(const MiscLib::Vector< Sphere > &spheres,
62 const MiscLib::Vector< float > &weights, Sphere *is);
63 void Serialize(bool binary, std::ostream *o) const;
64 static size_t SerializedSize();
65 void Serialize(FILE *o) const;
66 void Serialize(float* array) const;
67 static size_t SerializedFloatSize();
68
69 void Transform(float scale, const Vec3f &translate);
70 inline unsigned int Intersect(const Vec3f &p, const Vec3f &r,
71 float *first, float *second) const;
72
73 private:
74 template< class WeightT >
75 class LevMarSimpleSphere
76 : public WeightT
77 {
78 public:
79 enum { NumParams = 4 };
80 typedef float ScalarType;
81
82 template< class IteratorT >
83 ScalarType Chi(const ScalarType *params, IteratorT begin, IteratorT end,
84 ScalarType *values, ScalarType *temp) const
85 {
86 ScalarType chi = 0;
87 size_t size = end - begin;
88 #ifdef DOPARALLEL
89 #pragma omp parallel for schedule(static) reduction(+:chi)
90 #endif
91 for(int idx = 0; idx < size; ++idx)
92 {
93 float s = begin[idx][0] - params[0];
94 s *= s;
95 for(unsigned int j = 1; j < 3; ++j)
96 {
97 float ss = begin[idx][j] - params[j];
98 s += ss * ss;
99 }
100 values[idx] = WeightT::Weigh(std::sqrt(s) - params[3]);
101 chi += values[idx] * values[idx];
102 }
103 return chi;
104 }
105
106 template< class IteratorT >
107 void Derivatives(const ScalarType *params, IteratorT begin, IteratorT end,
108 const ScalarType *values, const ScalarType *temp, ScalarType *matrix) const
109 {
110 int size = static_cast<int>(end - begin);
111 #ifdef DOPARALLEL
112 #pragma omp parallel for schedule(static)
113 #endif
114 for(int idx = 0; idx < size; ++idx)
115 {
116 float s[3];
117 s[0] = begin[idx][0] - params[0];
118 float sl = s[0] * s[0];
119 for(unsigned int i = 1; i < 3; ++i)
120 {
121 s[i] = begin[idx][i] - params[i];
122 sl += s[i] * s[i];
123 }
124 sl = std::sqrt(sl);
125 matrix[idx * NumParams + 0] = -s[0] / sl;
126 matrix[idx * NumParams + 1] = -s[1] / sl;
127 matrix[idx * NumParams + 2] = -s[2] / sl;
128 matrix[idx * NumParams + 3] = -1;
129 WeightT::template DerivWeigh< NumParams >(sl - params[3],
130 matrix + idx * NumParams);
131 }
132 }
133
134 void Normalize(ScalarType *) const
135 {}
136 };
137
138 template< class WeightT >
139 class LevMarSphere
140 : public WeightT
141 {
142 public:
143 enum { NumParams = 7 };
144 typedef float ScalarType;
145
146 // parametrization: params[0] - params[2] = normal
147 // params[3] - params[5] = point
148 // params[6] = 1 / radius
149
150 template< class IteratorT >
151 ScalarType Chi(const ScalarType *params, IteratorT begin, IteratorT end,
152 ScalarType *values, ScalarType *temp) const
153 {
154 ScalarType chi = 0;
155 ScalarType radius = 1 / params[6];
156 Vec3f center = -radius * Vec3f(params[0], params[1], params[2])
157 + Vec3f(params[3], params[4], params[5]);
158 int size = end - begin;
159 #ifdef DOPARALLEL
160 #pragma omp parallel for schedule(static) reduction(+:chi)
161 #endif
162 for(int idx = 0; idx < size; ++idx)
163 {
164 temp[idx] = (begin[idx] - center).length();
165 chi += (values[idx] = WeightT::Weigh(temp[idx] - radius))
166 * values[idx];
167 }
168 return chi;
169 }
170
171 template< class IteratorT >
172 void Derivatives(const ScalarType *params, IteratorT begin, IteratorT end,
173 const ScalarType *values, const ScalarType *temp, ScalarType *matrix) const
174 {
175 Vec3f normal(params[0], params[1], params[2]);
176 Vec3f point(params[3], params[4], params[5]);
177 int size = end - begin;
178 #ifdef DOPARALLEL
179 #pragma omp parallel for schedule(static)
180 #endif
181 for(int idx = 0; idx < size; ++idx)
182 {
183 ScalarType denominator = -1.f / temp[idx] * params[6];
184 matrix[idx * NumParams + 0] =
185 (matrix[idx * NumParams + 3] = (point[0] - normal[0] * params[6] - begin[idx][0]))
186 * denominator;
187 matrix[idx * NumParams + 1] =
188 (matrix[idx * NumParams + 4] = (point[1] - normal[1] * params[6] - begin[idx][1]))
189 * denominator;
190 matrix[idx * NumParams + 2] =
191 (matrix[idx * NumParams + 5] = (point[2] - normal[2] * params[6] - begin[idx][2]))
192 * denominator;
193 matrix[idx * NumParams + 3] /= temp[idx];
194 matrix[idx * NumParams + 4] /= temp[idx];
195 matrix[idx * NumParams + 5] /= temp[idx];
196 matrix[idx * NumParams + 6] = (normal[0] * matrix[idx * NumParams + 3]
197 + normal[1] * matrix[idx * NumParams + 4]
198 + normal[2] * matrix[idx * NumParams + 5] + 1) * params[6] * params[6];
199 WeightT::template DerivWeigh< NumParams >(temp[idx] - 1.f / params[6],
200 matrix + idx * NumParams);
201 }
202 }
203
204 void Normalize(ScalarType *params) const
205 {
206 ScalarType len = std::sqrt(params[0] * params[0]
207 + params[1] * params[1] + params[2] * params[2]);
208 params[0] /= len;
209 params[1] /= len;
210 params[2] /= len;
211 }
212 };
213
214 private:
215 Vec3f m_center;
216 float m_radius;
217 };
218
219 inline float Sphere::Distance(const Vec3f &p) const
220 {
221 return fabs((m_center - p).length() - m_radius);
222 }
223
224 inline void Sphere::Normal(const Vec3f &p, Vec3f *normal) const
225 {
226 *normal = p - m_center;
227 normal->normalize();
228 }
229
230 inline float Sphere::DistanceAndNormal(const Vec3f &p, Vec3f *normal) const
231 {
232 *normal = p - m_center;
233 float l = normal->length();
234 if(l > 0)
235 *normal /= l;
236 return fabs(l - m_radius);
237 }
238
239 inline float Sphere::SignedDistance(const Vec3f &p) const
240 {
241 return (m_center - p).length() - m_radius;
242 }
243
244 template< class IteratorT >
245 bool Sphere::LeastSquaresFit(IteratorT begin, IteratorT end)
246 {
247 LevMarSimpleSphere< LevMarLSWeight > levMarSphere;
248 float param[4];
249 for(size_t i = 0; i < 3; ++i)
250 param[i] = m_center[i];
251 param[3] = m_radius;
252 if(!LevMar(begin, end, levMarSphere, param))
253 return false;
254 for(size_t i = 0; i < 3; ++i)
255 m_center[i] = param[i];
256 m_radius = param[3];
257 return true;
258 }
259
260 inline unsigned int Sphere::Intersect(const Vec3f &p, const Vec3f &r,
261 float *first, float *second) const
262 {
263 using namespace std;
264 Vec3f kDiff = p - m_center;
265 float fA0 = kDiff.dot(kDiff) - m_radius*m_radius;
266 float fA1, fDiscr, fRoot;
267 if (fA0 <= 0)
268 {
269 // P is inside the sphere
270 fA1 = r.dot(kDiff);
271 fDiscr = fA1*fA1 - fA0;
272 fRoot = sqrt(fDiscr);
273 *first = -fA1 + fRoot;
274 return 1;
275 }
276 // else: P is outside the sphere
277 fA1 = r.dot(kDiff);
278 if (fA1 >= 0)
279 return 0;
280 fDiscr = fA1*fA1 - fA0;
281 if(fDiscr < 0)
282 return 0;
283 else if(fDiscr >= /* zero tolerance eps */ 1e-7f)
284 {
285 fRoot = sqrt(fDiscr);
286 *first = -fA1 - fRoot;
287 *second = -fA1 + fRoot;
288 return 2;
289 }
290 *first = -fA1;
291 return 1;
292 }
293
294 class DLL_LINKAGE SphereAsSquaresParametrization
295 {
296 public:
297 SphereAsSquaresParametrization() {}
298 SphereAsSquaresParametrization(const Sphere &sphere,
299 const Vec3f &planeNormal);
300 void Init(const Sphere &sphere, const Vec3f &planeNormal);
301
302 // returns < 0 if point is on lower hemisphere
303 float Parameters(const Vec3f &p,
304 std::pair< float, float > *param) const;
305 bool InSpace(const std::pair< float, float > &param, bool lower,
306 Vec3f *p) const;
307 bool InSpace(const std::pair< float, float > &param, bool lower,
308 Vec3f *p, Vec3f *n) const;
309 void Transform(const GfxTL::MatrixXX< 3, 3, float > &rot,
310 const GfxTL::Vector3Df &trans);
311 void HyperplaneCoordinateSystem( Vec3f* hcs0, Vec3f* hcs1, Vec3f* hcs2 ) const;
312
313 private:
314 void Hemisphere2Disk(const Vec3f &p,
315 std::pair< float, float > *inDisk) const;
316 void Disk2Square(const std::pair< float, float > &inDisk,
317 std::pair< float, float > *inSquare) const;
318 void Square2Disk(const std::pair< float, float > &inSquare,
319 std::pair< float, float > *inDisk) const;
320 void Disk2Hemisphere(const std::pair< float, float > &inDisk,
321 Vec3f *p) const;
322
323 private:
324 Sphere m_sphere;
325 Vec3f m_planeNormal;
326 GfxTL::HyperplaneCoordinateSystem< float, 3 > m_hcs;
327 };
328
329 class DLL_LINKAGE UpperSphereAsSquaresParametrization
330 : public SphereAsSquaresParametrization
331 {
332 public:
333 UpperSphereAsSquaresParametrization() {}
334 UpperSphereAsSquaresParametrization(const SphereAsSquaresParametrization &p)
335 : SphereAsSquaresParametrization(p) {}
336 bool InSpace(const std::pair< float, float > &param, Vec3f *p) const
337 { return SphereAsSquaresParametrization::InSpace(param, false, p); }
338 bool InSpace(const std::pair< float, float > &param, Vec3f *p, Vec3f *n) const
339 { return SphereAsSquaresParametrization::InSpace(param, false, p, n); }
340 bool InSpace(float u, float v, Vec3f *p) const
341 { return SphereAsSquaresParametrization::InSpace(std::make_pair(u, v), false, p); }
342 bool InSpace(float u, float v, Vec3f *p, Vec3f *n) const
343 { return SphereAsSquaresParametrization::InSpace(std::make_pair(u, v), false, p, n); }
344 };
345
346 class DLL_LINKAGE LowerSphereAsSquaresParametrization
347 : public SphereAsSquaresParametrization
348 {
349 public:
350 LowerSphereAsSquaresParametrization() {}
351 LowerSphereAsSquaresParametrization(const SphereAsSquaresParametrization &p)
352 : SphereAsSquaresParametrization(p) {}
353 bool InSpace(const std::pair< float, float > &param, Vec3f *p) const
354 { return SphereAsSquaresParametrization::InSpace(param, true, p); }
355 bool InSpace(const std::pair< float, float > &param, Vec3f *p, Vec3f *n) const
356 { return SphereAsSquaresParametrization::InSpace(param, true, p, n); }
357 bool InSpace(float u, float v, Vec3f *p) const
358 { return SphereAsSquaresParametrization::InSpace(std::make_pair(u, v), true, p); }
359 bool InSpace(float u, float v, Vec3f *p, Vec3f *n) const
360 { return SphereAsSquaresParametrization::InSpace(std::make_pair(u, v), true, p, n); }
361 };
362
363 #endif