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