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