Cone.cpp
Go to the documentation of this file.
1 #include "Cone.h"
2 #include "Plane.h"
3 #include "LevMarFitting.h"
4 #include <MiscLib/Performance.h>
5 #include <GfxTL/IndexedIterator.h>
6 #include "LevMarLSWeight.h"
7 #include <GfxTL/Mean.h>
8 #ifdef DOPARALLEL
9 #include <omp.h>
10 #endif
11 
12 extern int dmat_solve(int n, int rhs_num, double a[]);
13 
14 Cone::Cone()
15  : m_angularRotatedRadians(0)
16 {}
17 
18 Cone::Cone(const Vec3f& center, const Vec3f& axisDir, float angle)
19  : m_angularRotatedRadians(0)
20 {
21  m_center = center;
22  m_axisDir = axisDir;
23  m_angle = angle;
24  m_normal = Vec3f(std::cos(-m_angle), std::sin(-m_angle), 0);
25  m_normalY = m_normal[1] * m_axisDir;
26  m_n2d[0] = std::cos(m_angle);
27  m_n2d[1] = -std::sin(m_angle);
28  m_hcs.FromNormal(m_axisDir);
29  m_angularRotatedRadians = 0;
30 }
31 
32 Cone::Cone(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3,
33  const Vec3f& n1, const Vec3f& n2, const Vec3f& n3)
34  : m_angularRotatedRadians(0)
35 {
36  if (!Init(p1, p2, p3, n1, n2, n3))
37  {
38  throw ParallelPlanesError();
39  }
40 }
41 
42 bool Cone::Init(const MiscLib::Vector< Vec3f >& samples)
43 {
44  if (samples.size() < 6)
45  {
46  return false;
47  }
48  size_t c = samples.size() >> 1;
49  return Init(samples[0], samples[1], samples[2],
50  samples[c], samples[c + 1], samples[c + 2]);
51 }
52 
53 class LevMarPlaneDistance
54 {
55 public:
56  enum { NumParams = 3 };
57  typedef float ScalarType;
58 
59  template< class IteratorT >
60  ScalarType Chi(const ScalarType* params, IteratorT begin, IteratorT end,
61  ScalarType* values, ScalarType* temp) const
62  {
63  ScalarType chi = 0;
64  intptr_t size = end - begin;
65  #pragma omp parallel for schedule(static) reduction(+:chi)
66  for (intptr_t i = 0; i < size; ++i)
67  {
68  values[i] = 0;
69  for (unsigned int j = 0; j < 3; ++j)
70  {
71  values[i] += begin[i][j] * params[j];
72  }
73  values[i] -= begin[i][3];
74  chi += values[i] * values[i];
75  }
76  return chi;
77  }
78 
79  template< class IteratorT >
80  void Derivatives(const ScalarType* params, IteratorT begin, IteratorT end,
81  ScalarType* values, ScalarType* temp, ScalarType* matrix) const
82  {
83  ScalarType chi = 0;
84  intptr_t size = end - begin;
85  #pragma omp parallel for schedule(static)
86  for (intptr_t i = 0; i < size; ++i)
87  {
88  for (unsigned int j = 0; j < 3; ++j)
89  {
90  matrix[i * NumParams + j] = begin[i][j];
91  }
92  }
93  }
94 
95  void Normalize(ScalarType*) const {}
96 };
97 
98 bool Cone::InitAverage(const MiscLib::Vector< Vec3f >& samples)
99 {
100  // setup all the planes
101  size_t c = samples.size() / 2;
102  MiscLib::Vector< GfxTL::Vector4Df > planes(c);
103  #pragma omp parallel for schedule(static)
104  for (intptr_t i = 0; i < c; ++i)
105  {
106  for (unsigned int j = 0; j < 3; ++j)
107  {
108  planes[i][j] = samples[i][j];
109  }
110  planes[i][3] = samples[i].dot(samples[i + c]);
111  }
112  // compute center by intersecting the three planes given by (p1, n1)
113  // (p2, n2) and (p3, n3)
114  // set up linear system
115  double a[4 * 3];
116  double d1 = samples[0].dot(samples[c + 0]);
117  double d2 = samples[1].dot(samples[c + 1]);
118  double d3 = samples[2].dot(samples[c + 2]);
119  // column major
120  a[0 + 0 * 3] = samples[c + 0][0];
121  a[1 + 0 * 3] = samples[c + 1][0];
122  a[2 + 0 * 3] = samples[c + 2][0];
123  a[0 + 1 * 3] = samples[c + 0][1];
124  a[1 + 1 * 3] = samples[c + 1][1];
125  a[2 + 1 * 3] = samples[c + 2][1];
126  a[0 + 2 * 3] = samples[c + 0][2];
127  a[1 + 2 * 3] = samples[c + 1][2];
128  a[2 + 2 * 3] = samples[c + 2][2];
129  a[0 + 3 * 3] = d1;
130  a[1 + 3 * 3] = d2;
131  a[2 + 3 * 3] = d3;
132  if (dmat_solve(3, 1, a))
133  {
134  return false;
135  }
136  m_center[0] = a[0 + 3 * 3];
137  m_center[1] = a[1 + 3 * 3];
138  m_center[2] = a[2 + 3 * 3];
139 
140  LevMarPlaneDistance planeDistance;
141  LevMar(planes.begin(), planes.end(), planeDistance,
142  (float*)m_center);
143 
144  MiscLib::Vector< GfxTL::Vector3Df > spoints(c);
145  #pragma omp parallel for schedule(static)
146  for (intptr_t i = 0; i < c; ++i)
147  {
148  spoints[i] = GfxTL::Vector3Df(samples[i] - m_center);
149  spoints[i].Normalize();
150  }
151  GfxTL::Vector3Df axisDir;
152  GfxTL::MeanOfNormals(spoints.begin(), spoints.end(), &axisDir);
153  m_axisDir = GfxTL::Vector3Df(axisDir);
154 
155  // make sure axis points in good direction
156  // the axis is defined to point into the interior of the cone
157  float heightSum = 0;
158  #pragma omp parallel for schedule(static) reduction(+:heightSum)
159  for (intptr_t i = 0; i < c; ++i)
160  {
161  heightSum += Height(samples[i]);
162  }
163  if (heightSum < 0)
164  {
165  m_axisDir *= -1;
166  }
167 
168  float angleReduction = 0;
169  #pragma omp parallel for schedule(static) reduction(+:angleReduction)
170  for (intptr_t i = 0; i < c; ++i)
171  {
172  float angle = m_axisDir.dot(samples[i + c]);
173  if (angle < -1) // clamp angle to [-1, 1]
174  {
175  angle = -1;
176  }
177  else if (angle > 1)
178  {
179  angle = 1;
180  }
181  if (angle < 0)
182  // m_angle = omega + 90
183  {
184  angle = std::acos(angle) - float(M_PI) / 2;
185  }
186  else
187  // m_angle = 90 - omega
188  {
189  angle = float(M_PI) / 2 - std::acos(angle);
190  }
191  angleReduction += angle;
192  }
193  angleReduction /= c;
194  m_angle = angleReduction;
195  if (m_angle < 1e-6 || m_angle > float(M_PI) / 2 - 1e-6)
196  {
197  return false;
198  }
199  //if(m_angle > 1.3962634015954636615389526147909) // 80 degrees
200  if (m_angle > 1.4835298641951801403851371532153f) // 85 degrees
201  {
202  return false;
203  }
204  m_normal = Vec3f(std::cos(-m_angle), std::sin(-m_angle), 0);
205  m_normalY = m_normal[1] * m_axisDir;
206  m_n2d[0] = std::cos(m_angle);
207  m_n2d[1] = -std::sin(m_angle);
208  m_hcs.FromNormal(m_axisDir);
209  m_angularRotatedRadians = 0;
210  return true;
211 }
212 
213 bool Cone::Init(const Vec3f& center, const Vec3f& axisDir, float angle)
214 {
215  if (angle > 1.4835298641951801403851371532153) // do not allow flat cones
216  {
217  return false;
218  }
219  m_center = center;
220  m_axisDir = axisDir;
221  m_angle = angle;
222  m_normal = Vec3f(std::cos(-m_angle), std::sin(-m_angle), 0);
223  m_normalY = m_normal[1] * m_axisDir;
224  m_n2d[0] = std::cos(m_angle);
225  m_n2d[1] = -std::sin(m_angle);
226  m_hcs.FromNormal(m_axisDir);
227  m_angularRotatedRadians = 0;
228  return true;
229 }
230 
231 bool Cone::Init(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3,
232  const Vec3f& n1, const Vec3f& n2, const Vec3f& n3)
233 {
234  //float ncheck = std::max(n2.dot(n3), std::max(n1.dot(n2), n1.dot(n3)));
235  //if(ncheck > 0.999)
236  // return false;
237  // compute center by intersecting the three planes given by (p1, n1)
238  // (p2, n2) and (p3, n3)
239  // set up linear system
240  double a[4 * 3];
241  double d1 = p1.dot(n1);
242  double d2 = p2.dot(n2);
243  double d3 = p3.dot(n3);
244  // column major
245  a[0 + 0 * 3] = n1[0];
246  a[1 + 0 * 3] = n2[0];
247  a[2 + 0 * 3] = n3[0];
248  a[0 + 1 * 3] = n1[1];
249  a[1 + 1 * 3] = n2[1];
250  a[2 + 1 * 3] = n3[1];
251  a[0 + 2 * 3] = n1[2];
252  a[1 + 2 * 3] = n2[2];
253  a[2 + 2 * 3] = n3[2];
254  a[0 + 3 * 3] = d1;
255  a[1 + 3 * 3] = d2;
256  a[2 + 3 * 3] = d3;
257  if (dmat_solve(3, 1, a))
258  {
259  return false;
260  }
261  m_center[0] = a[0 + 3 * 3];
262  m_center[1] = a[1 + 3 * 3];
263  m_center[2] = a[2 + 3 * 3];
264 
265  // compute axisDir
266  Vec3f s1 = p1 - m_center;
267  Vec3f s2 = p2 - m_center;
268  Vec3f s3 = p3 - m_center;
269  s1.normalize();
270  s2.normalize();
271  s3.normalize();
272  Plane pl(s1 + m_center, s2 + m_center, s3 + m_center);
273  m_axisDir = pl.getNormal();
274  // make sure axis points in direction of s1
275  // this defines the side of the cone!!!
276  if (m_axisDir.dot(s1) < 0)
277  {
278  m_axisDir *= -1;
279  }
280  m_angle = 0;
281  float angle = m_axisDir.dot(n1);
282  if (angle < -1) // clamp angle to [-1, 1]
283  {
284  angle = -1;
285  }
286  else if (angle > 1)
287  {
288  angle = 1;
289  }
290  if (angle < 0)
291  // m_angle = omega + 90
292  {
293  angle = std::acos(angle) - float(M_PI) / 2;
294  }
295  else
296  // m_angle = 90 - omega
297  {
298  angle = float(M_PI) / 2 - std::acos(angle);
299  }
300  m_angle += angle;
301  angle = m_axisDir.dot(n2);
302  if (angle < -1) // clamp angle to [-1, 1]
303  {
304  angle = -1;
305  }
306  else if (angle > 1)
307  {
308  angle = 1;
309  }
310  if (angle < 0)
311  // m_angle = omega + 90
312  {
313  angle = std::acos(angle) - float(M_PI) / 2;
314  }
315  else
316  // m_angle = 90 - omega
317  {
318  angle = float(M_PI) / 2 - std::acos(angle);
319  }
320  m_angle += angle;
321  angle = m_axisDir.dot(n3);
322  if (angle < -1) // clamp angle to [-1, 1]
323  {
324  angle = -1;
325  }
326  else if (angle > 1)
327  {
328  angle = 1;
329  }
330  if (angle < 0)
331  // m_angle = omega + 90
332  {
333  angle = std::acos(angle) - float(M_PI) / 2;
334  }
335  else
336  // m_angle = 90 - omega
337  {
338  angle = float(M_PI) / 2 - std::acos(angle);
339  }
340  m_angle += angle;
341  m_angle /= 3;
342  if (m_angle < 1e-6 || m_angle > float(M_PI) / 2 - 1e-6)
343  {
344  return false;
345  }
346  //if(m_angle > 1.3962634015954636615389526147909) // 80 degrees
347  if (m_angle > 1.4835298641951801403851371532153f) // 85 degrees
348  {
349  return false;
350  }
351  m_normal = Vec3f(std::cos(-m_angle), std::sin(-m_angle), 0);
352  m_normalY = m_normal[1] * m_axisDir;
353  m_n2d[0] = std::cos(m_angle);
354  m_n2d[1] = -std::sin(m_angle);
355  m_hcs.FromNormal(m_axisDir);
356  m_angularRotatedRadians = 0;
357  return true;
358 }
359 
360 bool Cone::Init(bool binary, std::istream* i)
361 {
362  float rotate = 0;
363  if (binary)
364  {
365  i->read((char*)&m_center, sizeof(m_center));
366  i->read((char*)&m_axisDir, sizeof(m_axisDir));
367  i->read((char*)&m_angle, sizeof(m_angle));
368  i->read((char*)&rotate,
369  sizeof(rotate));
370  }
371  else
372  {
373  for (size_t j = 0; j < 3; ++j)
374  {
375  (*i) >> m_center[j];
376  }
377  for (size_t j = 0; j < 3; ++j)
378  {
379  (*i) >> m_axisDir[j];
380  }
381  (*i) >> m_angle;
382  (*i) >> rotate;
383  }
384  m_normal = Vec3f(std::cos(-m_angle), std::sin(-m_angle), 0);
385  m_normalY = m_normal[1] * m_axisDir;
386  m_n2d[0] = std::cos(m_angle);
387  m_n2d[1] = -std::sin(m_angle);
388  m_hcs.FromNormal(m_axisDir);
389  m_angularRotatedRadians = 0;
390  RotateAngularDirection(rotate);
391  return true;
392 }
393 
394 void Cone::Init(float* array)
395 {
396  float rotate = 0;
397  for (int i = 0; i < 3; i++)
398  {
399  m_center[i] = array[i];
400  m_axisDir[i] = array[3 + i];
401  }
402  m_angle = array[6];
403  rotate = array[7];
404 
405  m_normal = Vec3f(std::cos(-m_angle), std::sin(-m_angle), 0);
406  m_normalY = m_normal[1] * m_axisDir;
407  m_n2d[0] = std::cos(m_angle);
408  m_n2d[1] = -std::sin(m_angle);
409  m_hcs.FromNormal(m_axisDir);
410  m_angularRotatedRadians = 0;
411  RotateAngularDirection(rotate);
412 }
413 
414 
415 void Cone::Init(FILE* i)
416 {
417  float rotate = 0;
418  fread(&m_center, sizeof(m_center), 1, i);
419  fread(&m_axisDir, sizeof(m_axisDir), 1, i);
420  fread(&m_angle, sizeof(m_angle), 1, i);
421  fread(&rotate, sizeof(rotate), 1, i);
422  m_normal = Vec3f(std::cos(-m_angle), std::sin(-m_angle), 0);
423  m_normalY = m_normal[1] * m_axisDir;
424  m_n2d[0] = std::cos(m_angle);
425  m_n2d[1] = -std::sin(m_angle);
426  m_hcs.FromNormal(m_axisDir);
427  m_angularRotatedRadians = 0;
428  RotateAngularDirection(rotate);
429 }
430 
431 void Cone::Project(const Vec3f& p, Vec3f* pp) const
432 {
433  // this is for one-sided cone !!!
434  Vec3f s = p - m_center;
435  float g = s.dot(m_axisDir); // distance to plane orhogonal to
436  // axisdir through center
437  // distance to axis
438  float sqrS = s.sqrLength();
439  float f = sqrS - (g * g);
440  if (f <= 0)
441  {
442  f = 0;
443  }
444  else
445  {
446  f = std::sqrt(f);
447  }
448  float da = m_n2d[0] * f;
449  float db = m_n2d[1] * g;
450  if (g < 0 && da - db < 0) // is inside other side of cone -> disallow
451  {
452  *pp = m_center;
453  return;
454  }
455  float dist = -(da + db);
456  // need normal
457  Vec3f plx = s - g * m_axisDir;
458  plx.normalize();
459  Vec3f n = m_normal[0] * plx + m_normalY;
460  *pp = p + dist * n;
461 }
462 
463 void Cone::Parameters(const Vec3f& p, std::pair< float, float >* param) const
464 {
465  // parametrize
466  Vec3f s = p - m_center;
467  float height = m_axisDir.dot(s);
468  float planex = s.dot(m_hcs[0].Data());
469  float planey = s.dot(m_hcs[1].Data());
470  float l = planex * planex + planey * planey;
471  if (l > 0)
472  {
473  planex /= l;
474  planey /= l;
475  }
476  float angle = std::atan2(planey, planex);
477  if (angle < 0)
478  {
479  angle += float(2 * M_PI);
480  }
481  /*Vec3f axisDiff = s - height * m_axisDir;
482  axisDiff.normalize();
483  float angle = m_angular.dot(axisDiff);
484  if(angle < -1) // clamp angle to [-1, 1]
485  angle = -1;
486  else if(angle > 1)
487  angle = 1;
488  if(m_angular.cross(axisDiff).dot(m_axisDir) < 0)
489  angle = std::acos(-angle) + M_PI;
490  else
491  angle = std::acos(angle);
492  // angle ok*/
493  // get length from height
494  //float length = height / std::cos(m_angle);
495  //param->first = length;
496  // this should be more precise than a division with std::cos:
497  // this is for two sided cone!
498  // distance to axis
499  float sqrS = s.sqrLength();
500  float f = sqrS - (height * height);
501  if (f <= 0)
502  {
503  f = 0;
504  }
505  else
506  {
507  f = std::sqrt(f);
508  }
509  float sdist = abs(m_n2d[0] * f + ((height < 0) ? -1 : 1) * m_n2d[1] * height);
510  float length = std::sqrt(sqrS + sdist * sdist);
511  param->first = /*(height < 0)? -length :*/ length;
512  param->second = angle;
513  /*// get normal for p
514  Vec3f pln = s.cross(m_axisDir);
515  Vec3f plx = m_axisDir.cross(pln);
516  Vec3f n;
517  if(plx.normalize() < 1e-6)
518  {
519  *param = std::make_pair(0.0f, angle);
520  return height;
521  }
522  if(height < 0)
523  n = m_normal[0] * plx - m_normalY;
524  else
525  n = m_normal[0] * plx + m_normalY;
526  Vec3f l = n.cross(pln);
527  l.normalize();
528  // make sure l points in direction of axis
529  if(m_axisDir.dot(l) < 0)
530  l *= -1;
531  // project p on line m_center + lambda * l
532  // get lambda
533  float lambda = s.dot(l);
534  // make sure l points in direction of axis
535  if(m_axisDir.dot(l) < 0)
536  {
537  if(lambda > 0)
538  {
539  *param = std::make_pair(s.length(), angle);
540  return height;
541  }
542  }
543  else if(lambda < 0)
544  {
545  *param = std::make_pair(s.length(), angle);
546  return height;
547  }
548  *param = std::make_pair(*abs(lambda), angle);*/
549 }
550 
551 void Cone::RotateAngularDirection(float radians)
552 {
553  GfxTL::Quaternion< float > q;
554  q.RotationRad(radians, m_axisDir[0], m_axisDir[1], m_axisDir[2]);
555  Vec3f vvec;
556  q.Rotate(AngularDirection(), &vvec);
557  m_hcs[0] = GfxTL::Vector3Df(vvec);
558  m_hcs[1] = GfxTL::Vector3Df(m_axisDir.cross(Vec3f(m_hcs[0].Data())));
559  m_angularRotatedRadians += radians;
560 }
561 
562 //void Cone::AngularDirection(const Vec3f &angular)
563 //{
564 // m_hcs[0] = GfxTL::Vector3Df(angular);
565 // m_hcs[1] = GfxTL::Vector3Df(m_axisDir.cross(Vec3f(m_hcs[0].Data())));
566 //}
567 
568 float ConeDistance(const float* param, const float* x)
569 {
570  Vec3f s;
571  for (unsigned int i = 0; i < 3; ++i)
572  {
573  s[i] = x[i] - param[i];
574  }
575  float g = abs(s[0] * param[3] + s[1] * param[4] + s[2] * param[5]);
576  float f = s.sqrLength() - (g * g);
577  if (f <= 0)
578  {
579  f = 0;
580  }
581  else
582  {
583  f = std::sqrt(f);
584  }
585  return std::cos(param[6]) * f - std::sin(param[6]) * g;
586  //- param[7];
587 }
588 
589 void ConeDistanceDerivatives(const float* param, const float* x,
590  float* gradient)
591 {
592  Vec3f s;
593  for (unsigned int i = 0; i < 3; ++i)
594  {
595  s[i] = x[i] - param[i];
596  }
597  float g = abs(s[0] * param[3] + s[1] * param[4] + s[2] * param[5]);
598  float f = s.sqrLength() - (g * g);
599  if (f <= 0)
600  {
601  f = 0;
602  }
603  else
604  {
605  f = std::sqrt(f);
606  }
607  float ggradient[6];
608  for (unsigned int i = 0; i < 3; ++i)
609  {
610  ggradient[i] = -param[i + 3];
611  }
612  for (unsigned int i = 0; i < 3; ++i)
613  {
614  ggradient[i + 3] = s[i] - param[i + 3] * g;
615  }
616  float fgradient[6];
617  if (f < 1e-6)
618  {
619  fgradient[0] = std::sqrt(1 - param[3] * param[3]);
620  fgradient[1] = std::sqrt(1 - param[4] * param[4]);
621  fgradient[2] = std::sqrt(1 - param[5] * param[5]);
622  }
623  else
624  {
625  fgradient[0] = (param[3] * g - s[0]) / f;
626  fgradient[1] = (param[4] * g - s[1]) / f;
627  fgradient[2] = (param[5] * g - s[2]) / f;
628  }
629  fgradient[3] = g * fgradient[0];
630  fgradient[4] = g * fgradient[1];
631  fgradient[5] = g * fgradient[2];
632  float sinPhi = -std::sin(param[6]);
633  float cosPhi = std::cos(param[6]);
634  for (unsigned int i = 0; i < 6; ++i)
635  {
636  gradient[i] = cosPhi * fgradient[i] + sinPhi * ggradient[i];
637  }
638  gradient[6] = f * sinPhi - g * cosPhi;
639  //gradient[7] = -1;
640 }
641 
642 void NormalizeConeParams(float* param)
643 {
644  // normalize direction
645  float l = std::sqrt(param[3] * param[3] + param[4] * param[4] +
646  param[5] * param[5]);
647  for (unsigned int i = 3; i < 6; ++i)
648  {
649  param[i] /= l;
650  }
651  // normalize angle
652  param[6] -= std::floor(param[6] / (2 * float(M_PI))) * (2 * float(M_PI)); // param[6] %= 2*M_PI
653  if (param[6] > M_PI)
654  {
655  param[6] -= std::floor(param[6] / float(M_PI)) * float(M_PI); // param[6] %= M_PI
656  for (unsigned int i = 3; i < 6; ++i)
657  {
658  param[i] *= -1;
659  }
660  }
661  if (param[6] > float(M_PI) / 2)
662  {
663  param[6] = float(M_PI) - param[6];
664  }
665 }
666 
667 bool Cone::LeastSquaresFit(const PointCloud& pc,
668  MiscLib::Vector< size_t >::const_iterator begin,
669  MiscLib::Vector< size_t >::const_iterator end)
670 {
671  bool retVal = LeastSquaresFit(GfxTL::IndexIterate(begin, pc.begin()),
672  GfxTL::IndexIterate(end, pc.begin()));
673  return retVal;
674 }
675 
676 bool Cone::Interpolate(const MiscLib::Vector< Cone >& cones,
677  const MiscLib::Vector< float >& weights, Cone* ic)
678 {
679  Vec3f center(0, 0, 0);
680  Vec3f axisDir(0, 0, 0);
681  float omega = 0;
682  for (size_t i = 0; i < cones.size(); ++i)
683  {
684  center += weights[i] * cones[i].Center();
685  axisDir += weights[i] * cones[i].AxisDirection();
686  omega += weights[i] * cones[i].Angle();
687  }
688  axisDir.normalize();
689  return ic->Init(center, axisDir, omega);
690 }
691 
692 void Cone::Serialize(bool binary, std::ostream* o) const
693 {
694  if (binary)
695  {
696  o->write((const char*)&m_center, sizeof(m_center));
697  o->write((const char*)&m_axisDir, sizeof(m_axisDir));
698  o->write((const char*)&m_angle, sizeof(m_angle));
699  o->write((const char*)&m_angularRotatedRadians,
700  sizeof(m_angularRotatedRadians));
701  }
702  else
703  {
704  (*o) << m_center[0] << " " << m_center[1] << " " << m_center[2] << " "
705  << m_axisDir[0] << " " << m_axisDir[1] << " " << m_axisDir[2] << " "
706  << m_angle << " " << m_angularRotatedRadians << " ";
707  }
708 }
709 
710 size_t Cone::SerializedSize()
711 {
712  return sizeof(Vec3f)
713  + sizeof(Vec3f)
714  + sizeof(float)
715  + sizeof(float);
716 }
717 
718 void Cone::Serialize(FILE* o) const
719 {
720  fwrite(&m_center, sizeof(m_center), 1, o);
721  fwrite(&m_axisDir, sizeof(m_axisDir), 1, o);
722  fwrite(&m_angle, sizeof(m_angle), 1, o);
723  fwrite(&m_angularRotatedRadians, sizeof(m_angularRotatedRadians), 1, o);
724 }
725 
726 size_t Cone::SerializedFloatSize()
727 {
728  return 8;
729 }
730 
731 void Cone::Serialize(float* array) const
732 {
733  for (int i = 0; i < 3; i++)
734  {
735  array[i] = m_center[i];
736  array[i + 3] = m_axisDir[i];
737  }
738  array[6] = m_angle;
739  array[7] = m_angularRotatedRadians;
740 }
741 
742 
743 void Cone::Transform(float scale, const Vec3f& translate)
744 {
745  m_center *= scale;
746  m_center += translate;
747 }
748 
749 void Cone::Transform(const GfxTL::MatrixXX< 3, 3, float >& rot,
750  const GfxTL::Vector3Df& trans)
751 {
752  m_center = Vec3f((rot * GfxTL::Vector3Df(m_center) + trans).Data());
753  m_axisDir = Vec3f((rot * GfxTL::Vector3Df(m_axisDir)).Data());
754  m_hcs[0] = rot * m_hcs[0];
755  m_hcs[1] = rot * m_hcs[1];
756  m_normalY = m_normal[1] * m_axisDir;
757 }
GfxTL::sqrt
VectorXD< D, T > sqrt(const VectorXD< D, T > &a)
Definition: VectorXD.h:662
GfxTL::VectorXD
Definition: MatrixXX.h:21
cyberglove_with_calib_22dof.ic
ic
Definition: cyberglove_with_calib_22dof.py:22
MiscLib::Vector::begin
T * begin()
Definition: Vector.h:460
Mean.h
Vec3f::normalize
float normalize()
Definition: basic.h:125
Vec3f
Definition: basic.h:16
Performance.h
Vec3f::cross
Vec3f cross(const Vec3f &v) const
Definition: basic.h:107
Cone::Interpolate
static bool Interpolate(const MiscLib::Vector< Cone > &cones, const MiscLib::Vector< float > &weights, Cone *ic)
Definition: Cone.cpp:676
Cone::Project
void Project(const Vec3f &p, Vec3f *pp) const
Definition: Cone.cpp:431
Cone::RotateAngularDirection
void RotateAngularDirection(float radians)
Definition: Cone.cpp:551
ProsthesisInterface.values
values
Definition: ProsthesisInterface.py:190
dmat_solve
int dmat_solve(int n, int rhs_num, double a[])
Definition: solve.cpp:7
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
GfxTL::Quaternion
Definition: VectorXD.h:707
LevMarPlaneDistance::ScalarType
float ScalarType
Definition: Cone.cpp:57
GfxTL::MatrixXX
Definition: MatrixXX.h:25
MiscLib::Vector
Definition: Vector.h:19
Plane.h
LevMarFitting.h
LevMarPlaneDistance::Chi
ScalarType Chi(const ScalarType *params, IteratorT begin, IteratorT end, ScalarType *values, ScalarType *temp) const
Definition: Cone.cpp:60
Cone::ParallelPlanesError
Definition: Cone.h:25
GfxTL::Vector3Df
VectorXD< 3, float > Vector3Df
Definition: VectorXD.h:676
ConeDistanceDerivatives
void ConeDistanceDerivatives(const float *param, const float *x, float *gradient)
Definition: Cone.cpp:589
MiscLib::Vector::size
size_type size() const
Definition: Vector.h:212
armarx::ctrlutil::a
double a(double t, double a0, double j)
Definition: CtrlUtil.h:45
ConeDistance
float ConeDistance(const float *param, const float *x)
Definition: Cone.cpp:568
armarx::abs
std::vector< T > abs(const std::vector< T > &v)
Definition: VectorHelpers.h:253
M_PI
#define M_PI
Definition: MathTools.h:17
Cone
Definition: Cone.h:22
Cone.h
Cone::Init
bool Init(const MiscLib::Vector< Vec3f > &samples)
Definition: Cone.cpp:42
GfxTL::IndexIterate
IndexedIterator< IndexIteratorT, IteratorT > IndexIterate(IndexIteratorT idxIt, IteratorT it)
Definition: IndexedIterator.h:154
Cone::Parameters
void Parameters(const Vec3f &p, std::pair< float, float > *param) const
Definition: Cone.cpp:463
MiscLib::Vector::end
T * end()
Definition: Vector.h:470
Cone::Serialize
void Serialize(bool binary, std::ostream *o) const
Definition: Cone.cpp:692
Cone::SerializedFloatSize
static size_t SerializedFloatSize()
Definition: Cone.cpp:726
Cone::Transform
void Transform(float scale, const Vec3f &translate)
Definition: Cone.cpp:743
Cone::InitAverage
bool InitAverage(const MiscLib::Vector< Vec3f > &samples)
Definition: Cone.cpp:98
q
#define q
IndexedIterator.h
PointCloud
Definition: PointCloud.h:69
Cone::SerializedSize
static size_t SerializedSize()
Definition: Cone.cpp:710
Vec3f::dot
float dot(const Vec3f &v) const
Definition: basic.h:92
float
#define float
Definition: 16_Level.h:22
Cone::Cone
Cone()
Definition: Cone.cpp:14
Plane::getNormal
const Vec3f & getNormal() const
Definition: Plane.h:51
LevMarPlaneDistance::NumParams
@ NumParams
Definition: Cone.cpp:56
LevMarLSWeight.h
Plane
Definition: Plane.h:16
angle
double angle(const Point &a, const Point &b, const Point &c)
Definition: point.hpp:100
GfxTL::Vec3f
VectorXD< 3, float > Vec3f
Definition: VectorXD.h:691
NormalizeConeParams
void NormalizeConeParams(float *param)
Definition: Cone.cpp:642
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
LevMarPlaneDistance::Normalize
void Normalize(ScalarType *) const
Definition: Cone.cpp:95
Cone::AngularDirection
const Vec3f AngularDirection() const
Definition: Cone.h:366
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
LevMarPlaneDistance::Derivatives
void Derivatives(const ScalarType *params, IteratorT begin, IteratorT end, ScalarType *values, ScalarType *temp, ScalarType *matrix) const
Definition: Cone.cpp:80
LevMarPlaneDistance
Definition: Cone.cpp:53
GfxTL::MeanOfNormals
bool MeanOfNormals(NormalsItT begin, NormalsItT end, WeightsItT weights, MeanT *mean)
Definition: Mean.h:60
Cone::Height
float Height(const Vec3f &p) const
Definition: Cone.h:376
Cone::LeastSquaresFit
bool LeastSquaresFit(const PointCloud &pc, MiscLib::Vector< size_t >::const_iterator begin, MiscLib::Vector< size_t >::const_iterator end)
Definition: Cone.cpp:667