Torus.cpp
Go to the documentation of this file.
1 #include "Torus.h"
2 
3 #include <iterator>
4 #include <limits>
5 
6 #include "LevMarFitting.h"
7 #include "LevMarLSWeight.h"
8 #include <GfxTL/IndexedIterator.h>
9 #include <GfxTL/VectorXD.h>
10 #include <MiscLib/Performance.h>
11 #ifdef DOPARALLEL
12 #include <omp.h>
13 #endif
14 
15 template <class InIteratorT, class OutIteratorT>
16 static void
17 SpinImage(const Vec3f& axisPos,
18  const Vec3f& axisDir,
19  InIteratorT begin,
20  InIteratorT end,
21  OutIteratorT out)
22 {
23  for (; begin != end; ++begin)
24  {
25  Vec3f s = *begin - axisPos;
26  GfxTL::Vector2Df spin;
27  spin[1] = s.dot(axisDir);
28  spin[0] = (s - spin[1] * axisDir).length();
29  *out = spin;
30  }
31 }
32 
33 template <class InIteratorT>
34 static bool
35 CircleFrom3Points(InIteratorT i, float* r, GfxTL::Vector2Df* center)
36 {
37  float a;
38  float b;
39  float bot;
40  float c;
41  float top1;
42  float top2;
43  a = std::sqrt(std::pow(i[1][0] - i[0][0], 2) + std::pow(i[1][1] - i[0][1], 2));
44  b = std::sqrt(std::pow(i[2][0] - i[1][0], 2) + std::pow(i[2][1] - i[1][1], 2));
45  c = std::sqrt(std::pow(i[0][0] - i[2][0], 2) + std::pow(i[0][1] - i[2][1], 2));
46 
47  bot = (a + b + c) * (-a + b + c) * (a - b + c) * (a + b - c);
48 
49  if (bot <= 0.f)
50  {
51  return false;
52  }
53 
54  *r = a * b * c / std::sqrt(bot);
55  // center.
56  top1 = (i[1][1] - i[0][1]) * c * c - (i[2][1] - i[0][1]) * a * a;
57  top2 = (i[1][0] - i[0][0]) * c * c - (i[2][0] - i[0][0]) * a * a;
58  bot = (i[1][1] - i[0][1]) * (i[2][0] - i[0][0]) - (i[2][1] - i[0][1]) * (i[1][0] - i[0][0]);
59 
60  (*center)[0] = i[0][0] + 0.5f * top1 / bot;
61  (*center)[1] = i[0][1] - 0.5f * top2 / bot;
62  return true;
63 }
64 
65 // this function estimates the torus from four samples
66 bool
67 Torus::Init(const MiscLib::Vector<Vec3f>& samples)
68 {
69  if (samples.size() < 8)
70  {
71  return false;
72  }
73 
74  // the algorithm used is as follows:
75  // 1. The axis of rotation is estimated
76  // 2. A spin image of the samples around the axis is constructed
77  // 3. A circle is fitted to the samples in the spin image
78 
79  // 1. Axis of rotation
80  // The formula used can be found in "Geometric least-squares fitting of
81  // spheres, cylinders, cones and tori" by Lukacs, Marshall and Martin 1997
82  // solve quadratic equation
83  size_t k = samples.size() >> 1;
84  MiscLib::Vector<GfxTL::Vector3Dd> dsamples;
85  dsamples.reserve(samples.size());
86  for (size_t i = 0; i < samples.size(); ++i)
87  dsamples.push_back(GfxTL::Vector3Dd(samples[i][0], samples[i][1], samples[i][2]));
88  GfxTL::Vector3Dd n0xn1 = dsamples[k] % dsamples[k + 1];
89  double a01 = n0xn1 * dsamples[k + 2];
90  double b01 = n0xn1 * dsamples[k + 3];
91  double a0 = ((dsamples[2] - dsamples[1]) % dsamples[k]) * dsamples[k + 2];
92  double a1 = ((dsamples[0] - dsamples[2]) % dsamples[k + 1]) * dsamples[k + 2];
93  double a = ((dsamples[0] - dsamples[2]) % (dsamples[1] - dsamples[0])) * dsamples[k + 2];
94  double b0 = ((dsamples[3] - dsamples[1]) % dsamples[k]) * dsamples[k + 3];
95  double b1 = ((dsamples[0] - dsamples[3]) % dsamples[k + 1]) * dsamples[k + 3];
96  double b = ((dsamples[0] - dsamples[3]) % (dsamples[1] - dsamples[0])) * dsamples[k + 3];
97  double cc = b01 / a01;
98  double ccc = b0 - a0 * cc;
99  double c = -(b1 - a1 * cc) / ccc;
100  double d = (-b + a * cc) / ccc;
101  double p = (a0 * c + a1 + a01 * d) / (2 * a01 * c);
102  double q = (a + a0 * d) / (a01 * c);
103  double rt = p * p - q;
104  if (rt < -1e-8)
105  {
106  return false;
107  }
108  if (rt < 0)
109  {
110  rt = 0;
111  }
112  double t1 = -p + std::sqrt(rt);
113  double t2 = -p - std::sqrt(rt);
114  double s1 = c * t1 + d;
115  double s2 = c * t2 + d;
116 
117  Vec3f pos1 = samples[0] + s1 * samples[k];
118  Vec3f normal1 = pos1 - (samples[1] + t1 * samples[k + 1]);
119  normal1.normalize();
120  Vec3f pos2 = samples[0] + s2 * samples[k];
121  Vec3f normal2 = pos2 - (samples[1] + t2 * samples[k + 1]);
122  normal2.normalize();
123 
124  // at this point there are two possible solutions for the axis
125  MiscLib::Vector<GfxTL::Vector2Df> spin1, spin2;
126  SpinImage(pos1, normal1, samples.begin(), samples.begin() + k, std::back_inserter(spin1));
127  SpinImage(pos2, normal2, samples.begin(), samples.begin() + k, std::back_inserter(spin2));
128  float minorRadius1, majorRadius1, minorRadius2, majorRadius2,
129  distSum1 = std::numeric_limits<float>::infinity(),
130  distSum2 = std::numeric_limits<float>::infinity(), tmp;
131  GfxTL::Vector2Df minorCenter1, minorCenter2;
132  if (CircleFrom3Points(spin1.begin(), &minorRadius1, &minorCenter1))
133  {
134  majorRadius1 = minorCenter1[0];
135  // compute the distance of the points to the torus
136  distSum1 = 0;
137  for (size_t i = 3; i < spin1.size(); ++i)
138  distSum1 += (tmp = ((spin1[i] - minorCenter1).Length() - minorRadius1)) * tmp;
139  }
140  if (CircleFrom3Points(spin2.begin(), &minorRadius2, &minorCenter2))
141  {
142  majorRadius2 = minorCenter2[0];
143  // compute the distance of the points to the torus
144  distSum2 = 0;
145  for (size_t i = 3; i < spin2.size(); ++i)
146  distSum2 += (tmp = ((spin2[i] - minorCenter2).Length() - minorRadius2)) * tmp;
147  }
148  if (distSum1 != std::numeric_limits<float>::infinity() && distSum1 < distSum2)
149  {
150  m_normal = normal1;
151  m_rminor = minorRadius1;
152  m_rmajor = majorRadius1;
153  m_center = pos1 + minorCenter1[1] * m_normal;
154  }
155  else if (distSum2 != std::numeric_limits<float>::infinity())
156  {
157  m_normal = normal2;
158  m_rminor = minorRadius2;
159  m_rmajor = majorRadius2;
160  m_center = pos2 + minorCenter2[1] * m_normal;
161  }
162  else
163  {
164  return false;
165  }
166  m_appleShaped = m_rmajor < m_rminor;
167  ComputeAppleParams();
168  return true;
169 }
170 
171 bool
172 Torus::InitAverage(const MiscLib::Vector<Vec3f>& samples)
173 {
174  if (samples.size() < 8)
175  {
176  return false;
177  }
178 
179  // the algorithm used is as follows:
180  // 1. The axis of rotation is estimated
181  // 2. A spin image of the samples around the axis is constructed
182  // 3. A circle is fitted to the samples in the spin image
183 
184  // 1. Axis of rotation
185  // The formula used can be found in "Geometric least-squares fitting of
186  // spheres, cylinders, cones and tori" by Lukacs, Marshall and Martin 1997
187  // solve quadratic equation
188  size_t k = samples.size() >> 1;
189  MiscLib::Vector<GfxTL::Vector3Dd> dsamples;
190  dsamples.reserve(samples.size());
191  for (size_t i = 0; i < samples.size(); ++i)
192  dsamples.push_back(GfxTL::Vector3Dd(samples[i][0], samples[i][1], samples[i][2]));
193  // run over all four tuples until two axes are succesfully
194  // established
195  Vec3f pos1, normal1, pos2, normal2;
196  for (size_t w = 0; w < k /* - 3*/; ++w)
197  {
198  for (size_t x = 0 /*w + 1*/; x < k /* - 2*/; ++x)
199  {
200  GfxTL::Vector3Dd n0xn1 = dsamples[k + w] % dsamples[k + x];
201  for (size_t y = 0 /*x + 1*/; y < k /* - 1*/; ++y)
202  {
203  for (size_t z = 0 /*y + 1*/; z < k; ++z)
204  {
205  double a01 = n0xn1 * dsamples[k + y];
206  double b01 = n0xn1 * dsamples[k + z];
207  if (GfxTL::Math<double>::Abs(a01) < 1e-6 ||
208  GfxTL::Math<double>::Abs(b01) < 1e-6)
209  {
210  continue;
211  }
212  double a0 = ((dsamples[y] - dsamples[x]) % dsamples[k + w]) * dsamples[k + y];
213  double a1 = ((dsamples[w] - dsamples[y]) % dsamples[k + x]) * dsamples[k + y];
214  double a = ((dsamples[w] - dsamples[y]) % (dsamples[x] - dsamples[w])) *
215  dsamples[k + y];
216  double b0 = ((dsamples[z] - dsamples[x]) % dsamples[k + w]) * dsamples[k + z];
217  double b1 = ((dsamples[w] - dsamples[z]) % dsamples[k + x]) * dsamples[k + z];
218  double b = ((dsamples[w] - dsamples[z]) % (dsamples[x] - dsamples[w])) *
219  dsamples[k + z];
220  double cc = b01 / a01;
221  double ccc = b0 - a0 * cc;
222  double c = -(b1 - a1 * cc) / ccc;
223  double d = (-b + a * cc) / ccc;
224  double p = (a0 * c + a1 + a01 * d) / (2 * a01 * c);
225  double q = (a + a0 * d) / (a01 * c);
226  double rt = p * p - q;
227  if (rt < -1e-8)
228  {
229  continue;
230  }
231  if (rt < 0)
232  {
233  rt = 0;
234  }
235  double t1 = -p + std::sqrt(rt);
236  double t2 = -p - std::sqrt(rt);
237  double s1 = c * t1 + d;
238  double s2 = c * t2 + d;
239  pos1 = samples[w] + s1 * samples[k + w];
240  normal1 = pos1 - (samples[x] + t1 * samples[k + x]);
241  normal1.normalize();
242  pos2 = samples[w] + s2 * samples[k + w];
243  normal2 = pos2 - (samples[x] + t2 * samples[k + x]);
244  normal2.normalize();
245  goto foundAxis;
246  }
247  }
248  }
249  }
250  return false;
251 
252 foundAxis:
253  // at this point there are two possible solutions for the axis
254  MiscLib::Vector<GfxTL::Vector2Df> spin1, spin2;
255  SpinImage(pos1, normal1, samples.begin(), samples.begin() + k, std::back_inserter(spin1));
256  SpinImage(pos2, normal2, samples.begin(), samples.begin() + k, std::back_inserter(spin2));
257  float minorRadius1, majorRadius1, minorRadius2, majorRadius2,
258  distSum1 = std::numeric_limits<float>::infinity(),
259  distSum2 = std::numeric_limits<float>::infinity(), tmp;
260  GfxTL::Vector2Df minorCenter1, minorCenter2;
261  if (CircleFrom3Points(spin1.begin(), &minorRadius1, &minorCenter1))
262  {
263  majorRadius1 = minorCenter1[0];
264  // compute the distance of the points to the torus
265  distSum1 = 0;
266  for (size_t i = 3; i < spin1.size(); ++i)
267  distSum1 += (tmp = ((spin1[i] - minorCenter1).Length() - minorRadius1)) * tmp;
268  }
269  if (CircleFrom3Points(spin2.begin(), &minorRadius2, &minorCenter2))
270  {
271  majorRadius2 = minorCenter2[0];
272  // compute the distance of the points to the torus
273  distSum2 = 0;
274  for (size_t i = 3; i < spin2.size(); ++i)
275  distSum2 += (tmp = ((spin2[i] - minorCenter2).Length() - minorRadius2)) * tmp;
276  }
277  if (distSum1 != std::numeric_limits<float>::infinity() && distSum1 < distSum2)
278  {
279  m_normal = normal1;
280  m_rminor = minorRadius1;
281  m_rmajor = majorRadius1;
282  m_center = pos1 + minorCenter1[1] * m_normal;
283  }
284  else if (distSum2 != std::numeric_limits<float>::infinity())
285  {
286  m_normal = normal2;
287  m_rminor = minorRadius2;
288  m_rmajor = majorRadius2;
289  m_center = pos2 + minorCenter2[1] * m_normal;
290  }
291  else
292  {
293  return false;
294  }
295  m_appleShaped = m_rmajor < m_rminor;
296  ComputeAppleParams();
297  return true;
298 }
299 
300 bool
301 Torus::Init(bool binary, std::istream* i)
302 {
303  if (binary)
304  {
305  i->read((char*)&m_normal, sizeof(m_center));
306  i->read((char*)&m_center, sizeof(m_center));
307  i->read((char*)&m_rminor, sizeof(m_rminor));
308  i->read((char*)&m_rmajor, sizeof(m_rmajor));
309  }
310  else
311  {
312  for (size_t j = 0; j < 3; ++j)
313  {
314  (*i) >> m_normal[j];
315  }
316  for (size_t j = 0; j < 3; ++j)
317  {
318  (*i) >> m_center[j];
319  }
320  (*i) >> m_rminor;
321  (*i) >> m_rmajor;
322  }
323  m_appleShaped = m_rmajor < m_rminor;
324  ComputeAppleParams();
325  return true;
326 }
327 
328 void
329 Torus::Init(FILE* i)
330 {
331  float rot; // dummy rotation placeholder
332  fread(&m_normal, sizeof(m_normal), 1, i);
333  fread(&m_center, sizeof(m_center), 1, i);
334  fread(&m_rminor, sizeof(m_rminor), 1, i);
335  fread(&m_rmajor, sizeof(m_rmajor), 1, i);
336  fread(&rot, sizeof(rot), 1, i);
337  m_appleShaped = m_rmajor < m_rminor;
338  ComputeAppleParams();
339 }
340 
341 void
342 Torus::Init(float* array)
343 {
344  for (int i = 0; i < 3; i++)
345  {
346  m_normal[i] = array[i];
347  m_center[i] = array[i + 3];
348  }
349  m_rminor = array[6];
350  m_rmajor = array[7];
351  m_appleShaped = m_rmajor < m_rminor;
352  ComputeAppleParams();
353 }
354 
355 void
356 Torus::Transform(float scale, const Vec3f& translate)
357 {
358  m_rmajor *= scale;
359  m_rminor *= scale;
360  m_center += translate;
361 }
362 
363 float
364 TorusDistance(const float* param, const float* x)
365 {
366  Vec3f s;
367  s[0] = x[0] - param[0];
368  s[1] = x[1] - param[1];
369  s[2] = x[2] - param[2];
370  float g = s[0] * param[3] + s[1] * param[4] + s[2] * param[5];
371  float f = param[5] * s[1] - param[4] * s[2];
372  f *= f;
373  float v = param[3] * s[2] - param[5] * s[0];
374  f += v * v;
375  v = param[4] * s[0] - param[3] * s[1];
376  f += v * v;
377  f = std::sqrt(f);
378  float tmp;
379  return std::sqrt(g * g + ((tmp = (f - param[6])) * tmp)) - param[7];
380 }
381 
382 void
383 TorusDistanceDerivatives(const float* param, const float* x, float* gradient)
384 {
385  Vec3f s;
386  s[0] = x[0] - param[0];
387  s[1] = x[1] - param[1];
388  s[2] = x[2] - param[2];
389  float g = s[0] * param[3] + s[1] * param[4] + s[2] * param[5];
390  float f = param[5] * s[1] - param[4] * s[2];
391  f *= f;
392  float v = param[3] * s[2] - param[5] * s[0];
393  f += v * v;
394  v = param[4] * s[0] - param[3] * s[1];
395  f += v * v;
396  f = std::sqrt(f);
397  float dg[6];
398  dg[0] = -param[3];
399  dg[1] = -param[4];
400  dg[2] = -param[5];
401  dg[3] = s[0] - param[3] * g;
402  dg[4] = s[1] - param[4] * g;
403  dg[5] = s[2] - param[5] * g;
404  float df[6];
405  df[0] = (param[3] * g - s[0]) / f;
406  df[1] = (param[4] * g - s[1]) / f;
407  df[2] = (param[5] * g - s[2]) / f;
408  df[3] = g * df[0];
409  df[4] = g * df[1];
410  df[5] = g * df[2];
411  float tmp;
412  float d = std::sqrt(g * g + ((tmp = (f - param[6])) * tmp)) - param[7];
413  float dr = d + param[7];
414  float fr = f - param[6];
415  for (unsigned int i = 0; i < 6; ++i)
416  {
417  gradient[i] = (g * dg[i] + fr * df[i]) / dr;
418  }
419  gradient[6] = -fr * dr;
420  gradient[7] = -1;
421 }
422 
423 void
424 NormalizeTorusParams(float* param)
425 {
426  float l = std::sqrt(param[3] * param[3] + param[4] * param[4] + param[5] * param[5]);
427  for (unsigned int i = 3; i < 6; ++i)
428  {
429  param[i] /= l;
430  }
431 }
432 
433 template <class WeightT>
434 class LevMarTorus : public WeightT
435 {
436 public:
437  enum
438  {
439  NumParams = 8
440  };
441 
442  typedef float ScalarType;
443 
444  template <class IteratorT>
445  ScalarType
446  Chi(const ScalarType* params,
447  IteratorT begin,
448  IteratorT end,
449  ScalarType* values,
450  ScalarType* temp) const
451  {
452  ScalarType chi = 0;
453  int size = end - begin;
454 #pragma omp parallel for schedule(static) reduction(+ : chi)
455  for (int idx = 0; idx < size; ++idx)
456  {
457  Vec3f s;
458  s[0] = begin[idx][0] - params[0];
459  s[1] = begin[idx][1] - params[1];
460  s[2] = begin[idx][2] - params[2];
461  ScalarType g = s[0] * params[3] + s[1] * params[4] + s[2] * params[5];
462  ScalarType f = params[5] * s[1] - params[4] * s[2];
463  f *= f;
464  ScalarType v = params[3] * s[2] - params[5] * s[0];
465  f += v * v;
466  v = params[4] * s[0] - params[3] * s[1];
467  f += v * v;
468  f = std::sqrt(f);
469  temp[idx] = f;
470  ScalarType tmp;
471  chi += (values[idx] = WeightT::Weigh(
472  std::sqrt(g * g + ((tmp = (f - params[6])) * tmp)) - params[7])) *
473  values[idx];
474  ;
475  }
476  return chi;
477  }
478 
479  template <class IteratorT>
480  void
481  Derivatives(const ScalarType* params,
482  IteratorT begin,
483  IteratorT end,
484  const ScalarType* values,
485  const ScalarType* temp,
486  ScalarType* matrix) const
487  {
488  int size = end - begin;
489 #pragma omp parallel for schedule(static)
490  for (int idx = 0; idx < size; ++idx)
491  {
492  Vec3f s;
493  s[0] = begin[idx][0] - params[0];
494  s[1] = begin[idx][1] - params[1];
495  s[2] = begin[idx][2] - params[2];
496  ScalarType g = s[0] * params[3] + s[1] * params[4] + s[2] * params[5];
497  ScalarType dg[6];
498  dg[0] = -params[3];
499  dg[1] = -params[4];
500  dg[2] = -params[5];
501  dg[3] = s[0] - params[3] * g;
502  dg[4] = s[1] - params[4] * g;
503  dg[5] = s[2] - params[5] * g;
504  ScalarType df[6];
505  df[0] = (params[3] * g - s[0]) / temp[idx];
506  df[1] = (params[4] * g - s[1]) / temp[idx];
507  df[2] = (params[5] * g - s[2]) / temp[idx];
508  df[3] = g * df[0];
509  df[4] = g * df[1];
510  df[5] = g * df[2];
511  ScalarType tmp;
512  ScalarType d = std::sqrt(g * g + ((tmp = (temp[idx] - params[6])) * tmp)) - params[7];
513  ScalarType dr = d + params[7];
514  ScalarType fr = temp[idx] - params[6];
515  for (unsigned int j = 0; j < 6; ++j)
516  {
517  matrix[idx * NumParams + j] = (g * dg[j] + fr * df[j]) / dr;
518  }
519  matrix[idx * NumParams + 6] = -fr * dr;
520  matrix[idx * NumParams + 7] = -1;
521  WeightT::template DerivWeigh<NumParams>(d, matrix + idx * NumParams);
522  }
523  }
524 
525  void
526  Normalize(float* params) const
527  {
528  ScalarType l =
529  std::sqrt(params[3] * params[3] + params[4] * params[4] + params[5] * params[5]);
530  for (unsigned int i = 3; i < 6; ++i)
531  {
532  params[i] /= l;
533  }
534  }
535 };
536 
537 bool
538 Torus::LeastSquaresFit(const PointCloud& pc,
539  MiscLib::Vector<size_t>::const_iterator begin,
540  MiscLib::Vector<size_t>::const_iterator end)
541 {
542  float param[8];
543  for (size_t i = 0; i < 3; ++i)
544  {
545  param[i] = m_center[i];
546  }
547  for (size_t i = 0; i < 3; ++i)
548  {
549  param[i + 3] = m_normal[i];
550  }
551  param[6] = m_rmajor;
552  param[7] = m_rminor;
553  LevMarTorus<LevMarLSWeight> levMarTorus;
554  if (!LevMar(GfxTL::IndexIterate(begin, pc.begin()),
555  GfxTL::IndexIterate(end, pc.begin()),
556  levMarTorus,
557  param))
558  {
559  return false;
560  }
561  for (size_t i = 0; i < 3; ++i)
562  {
563  m_center[i] = param[i];
564  }
565  for (size_t i = 0; i < 3; ++i)
566  {
567  m_normal[i] = param[i + 3];
568  }
569  m_rmajor = param[6];
570  m_rminor = param[7];
571  m_appleShaped = m_rmajor < m_rminor;
572  ComputeAppleParams();
573  return true;
574 }
575 
576 void
577 Torus::Serialize(bool binary, std::ostream* o) const
578 {
579  if (binary)
580  {
581  o->write((const char*)&m_normal, sizeof(m_normal));
582  o->write((const char*)&m_center, sizeof(m_center));
583  o->write((const char*)&m_rminor, sizeof(m_rminor));
584  o->write((const char*)&m_rmajor, sizeof(m_rmajor));
585  }
586  else
587  {
588  (*o) << m_normal[0] << " " << m_normal[1] << " " << m_normal[2] << " " << m_center[0] << " "
589  << m_center[1] << " " << m_center[2] << " " << m_rminor << " " << m_rmajor << " ";
590  }
591 }
592 
593 size_t
594 Torus::SerializedSize()
595 {
596  return sizeof(Vec3f) + sizeof(Vec3f) + sizeof(float) + sizeof(float);
597 }
598 
599 void
600 Torus::Serialize(FILE* o) const
601 {
602  fwrite(&m_normal, sizeof(m_normal), 1, o);
603  fwrite(&m_center, sizeof(m_center), 1, o);
604  fwrite(&m_rminor, sizeof(m_rminor), 1, o);
605  fwrite(&m_rmajor, sizeof(m_rmajor), 1, o);
606 }
607 
608 size_t
609 Torus::SerializedFloatSize()
610 {
611  return 8;
612 }
613 
614 void
615 Torus::Serialize(float* array) const
616 {
617  for (int i = 0; i < 3; i++)
618  {
619  array[i] = m_normal[i];
620  array[i + 3] = m_center[i];
621  }
622  array[6] = m_rminor;
623  array[7] = m_rmajor;
624 }
625 
626 void
627 Torus::ComputeAppleParams()
628 {
629  if (!m_appleShaped)
630  {
631  m_cutOffAngle = M_PI;
632  m_appleHeight = 0;
633  return;
634  }
635  m_cutOffAngle = std::acos((2 * m_rmajor - m_rminor) / m_rminor) + M_PI / 2.f;
636  m_appleHeight = std::sin(m_cutOffAngle) * m_rminor;
637 }
GfxTL::VectorXD
Definition: MatrixXX.h:24
LevMarTorus::NumParams
@ NumParams
Definition: Torus.cpp:439
GfxTL::Vec3f
VectorXD< 3, float > Vec3f
Definition: VectorXD.h:733
MiscLib::Vector::begin
T * begin()
Definition: Vector.h:472
Vec3f::normalize
float normalize()
Definition: basic.h:141
Vec3f
Definition: basic.h:17
MiscLib::Vector::push_back
void push_back(const T &v)
Definition: Vector.h:354
Performance.h
Torus.h
Torus::LeastSquaresFit
bool LeastSquaresFit(const PointCloud &pc, MiscLib::Vector< size_t >::const_iterator begin, MiscLib::Vector< size_t >::const_iterator end)
Definition: Torus.cpp:538
ProsthesisInterface.values
values
Definition: ProsthesisInterface.py:190
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:46
LevMarTorus::Derivatives
void Derivatives(const ScalarType *params, IteratorT begin, IteratorT end, const ScalarType *values, const ScalarType *temp, ScalarType *matrix) const
Definition: Torus.cpp:481
TorusDistance
float TorusDistance(const float *param, const float *x)
Definition: Torus.cpp:364
MiscLib::Vector
Definition: Vector.h:19
VectorXD.h
LevMarFitting.h
MiscLib::Vector::size
size_type size() const
Definition: Vector.h:215
Torus::Transform
void Transform(float scale, const Vec3f &translate)
Definition: Torus.cpp:356
armarx::ctrlutil::a
double a(double t, double a0, double j)
Definition: CtrlUtil.h:45
M_PI
#define M_PI
Definition: MathTools.h:17
LevMarTorus::ScalarType
float ScalarType
Definition: Torus.cpp:442
LevMarTorus
Definition: Torus.cpp:434
GfxTL::IndexIterate
IndexedIterator< IndexIteratorT, IteratorT > IndexIterate(IndexIteratorT idxIt, IteratorT it)
Definition: IndexedIterator.h:173
NormalizeTorusParams
void NormalizeTorusParams(float *param)
Definition: Torus.cpp:424
GfxTL::Math
Definition: MathHelper.h:11
Torus::Serialize
void Serialize(bool binary, std::ostream *o) const
Definition: Torus.cpp:577
q
#define q
TorusDistanceDerivatives
void TorusDistanceDerivatives(const float *param, const float *x, float *gradient)
Definition: Torus.cpp:383
IndexedIterator.h
PointCloud
Definition: PointCloud.h:85
GfxTL::sqrt
VectorXD< D, T > sqrt(const VectorXD< D, T > &a)
Definition: VectorXD.h:704
MiscLib::Vector::reserve
void reserve(size_type s)
Definition: Vector.h:189
armarx::ctrlutil::v
double v(double t, double v0, double a0, double j)
Definition: CtrlUtil.h:39
float
#define float
Definition: 16_Level.h:22
Torus::Init
bool Init(const MiscLib::Vector< Vec3f > &samples)
Definition: Torus.cpp:67
armarx::view_selection::skills::direction::state::center
state::Type center(state::Type previous)
Definition: LookDirection.cpp:233
Torus::InitAverage
bool InitAverage(const MiscLib::Vector< Vec3f > &samples)
Definition: Torus.cpp:172
LevMarLSWeight.h
Torus::SerializedSize
static size_t SerializedSize()
Definition: Torus.cpp:594
pc
Introduction Thank you for taking interest in our work and downloading this software This library implements the algorithm described in the paper R R R Klein Efficient RANSAC for Point Cloud Shape in Computer Graphics Blackwell June If you use this software you should cite the aforementioned paper in any resulting publication Please send comments or bug reports to Ruwen Roland BUT NOT LIMITED THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY OR CONSEQUENTIAL WHETHER IN STRICT OR EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE Example usage This section shows how to use the library to detect the shapes in a point cloud PointCloud pc
Definition: ReadMe.txt:68
LevMarTorus::Normalize
void Normalize(float *params) const
Definition: Torus.cpp:526
armarx::ctrlutil::s
double s(double t, double s0, double v0, double a0, double j)
Definition: CtrlUtil.h:33
LevMar
bool LevMar(IteratorT begin, IteratorT end, FuncT &func, typename FuncT::ScalarType *param)
Definition: LevMarFitting.h:152
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:27
LevMarTorus::Chi
ScalarType Chi(const ScalarType *params, IteratorT begin, IteratorT end, ScalarType *values, ScalarType *temp) const
Definition: Torus.cpp:446
Torus::SerializedFloatSize
static size_t SerializedFloatSize()
Definition: Torus.cpp:609