Point Cloud Library (PCL)  1.7.0
polynomial_calculations.hpp
1 ////////////////////////////////////
2
3 template <typename real>
5 {
6 }
7
8 ////////////////////////////////////
9
10 template <typename real>
12 {
13 }
14
15 ////////////////////////////////////
16
17 template <typename real>
18 inline void
20 {
21  zero_value = new_zero_value;
23 }
24
25 ////////////////////////////////////
26
27 template <typename real>
28 inline void
29  pcl::PolynomialCalculationsT<real>::solveLinearEquation (real a, real b, std::vector<real>& roots) const
30 {
31  //cout << "Trying to solve "<<a<<"x + "<<b<<" = 0\n";
32
33  if (isNearlyZero (b))
34  {
35  roots.push_back (0.0);
36  }
37  if (!isNearlyZero (a/b))
38  {
39  roots.push_back (-b/a);
40  }
41
42 #if 0
43  cout << __PRETTY_FUNCTION__ << ": Found "<<roots.size ()<<" roots.\n";
44  for (unsigned int i=0; i<roots.size (); i++)
45  {
46  real x=roots[i];
47  real result = a*x + b;
48  if (!isNearlyZero (result))
49  {
50  cout << "Something went wrong during solving of polynomial "<<a<<"x + "<<b<<" = 0\n";
51  //roots.clear ();
52  }
53  cout << "Root "<<i<<" = "<<roots[i]<<". ("<<a<<"x^ + "<<b<<" = "<<result<<")\n";
54  }
55 #endif
56 }
57
58 ////////////////////////////////////
59
60 template <typename real>
61 inline void
62  pcl::PolynomialCalculationsT<real>::solveQuadraticEquation (real a, real b, real c, std::vector<real>& roots) const
63 {
64  //cout << "Trying to solve "<<a<<"x^2 + "<<b<<"x + "<<c<<" = 0\n";
65
66  if (isNearlyZero (a))
67  {
68  //cout << "Highest order element is 0 => Calling solveLineaqrEquation.\n";
69  solveLinearEquation (b, c, roots);
70  return;
71  }
72
73  if (isNearlyZero (c))
74  {
75  roots.push_back (0.0);
76  //cout << "Constant element is 0 => Adding root 0 and calling solveLinearEquation.\n";
77  std::vector<real> tmpRoots;
78  solveLinearEquation (a, b, tmpRoots);
79  for (unsigned int i=0; i<tmpRoots.size (); i++)
80  if (!isNearlyZero (tmpRoots[i]))
81  roots.push_back (tmpRoots[i]);
82  return;
83  }
84
85  real tmp = b*b - 4*a*c;
86  if (tmp>0)
87  {
88  tmp = sqrt (tmp);
89  real tmp2 = 1.0/ (2*a);
90  roots.push_back ( (-b + tmp)*tmp2);
91  roots.push_back ( (-b - tmp)*tmp2);
92  }
93  else if (sqrtIsNearlyZero (tmp))
94  {
95  roots.push_back (-b/ (2*a));
96  }
97
98 #if 0
99  cout << __PRETTY_FUNCTION__ << ": Found "<<roots.size ()<<" roots.\n";
100  for (unsigned int i=0; i<roots.size (); i++)
101  {
102  real x=roots[i], x2=x*x;
103  real result = a*x2 + b*x + c;
104  if (!isNearlyZero (result))
105  {
106  cout << "Something went wrong during solving of polynomial "<<a<<"x^2 + "<<b<<"x + "<<c<<" = 0\n";
107  //roots.clear ();
108  }
109  //cout << "Root "<<i<<" = "<<roots[i]<<". ("<<a<<"x^2 + "<<b<<"x + "<<c<<" = "<<result<<")\n";
110  }
111 #endif
112 }
113
114 ////////////////////////////////////
115
116 template<typename real>
117 inline void
118  pcl::PolynomialCalculationsT<real>::solveCubicEquation (real a, real b, real c, real d, std::vector<real>& roots) const
119 {
120  //cout << "Trying to solve "<<a<<"x^3 + "<<b<<"x^2 + "<<c<<"x + "<<d<<" = 0\n";
121
122  if (isNearlyZero (a))
123  {
124  //cout << "Highest order element is 0 => Calling solveQuadraticEquation.\n";
125  solveQuadraticEquation (b, c, d, roots);
126  return;
127  }
128
129  if (isNearlyZero (d))
130  {
131  roots.push_back (0.0);
132  //cout << "Constant element is 0 => Adding root 0 and calling solveQuadraticEquation.\n";
133  std::vector<real> tmpRoots;
134  solveQuadraticEquation (a, b, c, tmpRoots);
135  for (unsigned int i=0; i<tmpRoots.size (); i++)
136  if (!isNearlyZero (tmpRoots[i]))
137  roots.push_back (tmpRoots[i]);
138  return;
139  }
140
141  double a2 = a*a,
142  a3 = a2*a,
143  b2 = b*b,
144  b3 = b2*b,
145  alpha = ( (3.0*a*c-b2)/ (3.0*a2)),
146  beta = (2*b3/ (27.0*a3)) - ( (b*c)/ (3.0*a2)) + (d/a),
147  alpha2 = alpha*alpha,
148  alpha3 = alpha2*alpha,
149  beta2 = beta*beta;
150
151  // Value for resubstitution:
152  double resubValue = b/ (3*a);
153
154  //cout << "Trying to solve y^3 + "<<alpha<<"y + "<<beta<<"\n";
155
156  double discriminant = (alpha3/27.0) + 0.25*beta2;
157
158  //cout << "Discriminant is "<<discriminant<<"\n";
159
160  if (isNearlyZero (discriminant))
161  {
162  if (!isNearlyZero (alpha) || !isNearlyZero (beta))
163  {
164  roots.push_back ( (-3.0*beta)/ (2.0*alpha) - resubValue);
165  roots.push_back ( (3.0*beta)/alpha - resubValue);
166  }
167  else
168  {
169  roots.push_back (-resubValue);
170  }
171  }
172  else if (discriminant > 0)
173  {
174  double sqrtDiscriminant = sqrt (discriminant);
175  double d1 = -0.5*beta + sqrtDiscriminant,
176  d2 = -0.5*beta - sqrtDiscriminant;
177  if (d1 < 0)
178  d1 = -pow (-d1, 1.0/3.0);
179  else
180  d1 = pow (d1, 1.0/3.0);
181
182  if (d2 < 0)
183  d2 = -pow (-d2, 1.0/3.0);
184  else
185  d2 = pow (d2, 1.0/3.0);
186
187  //cout << PVAR (d1)<<", "<<PVAR (d2)<<"\n";
188  roots.push_back (d1 + d2 - resubValue);
189  }
190  else
191  {
192  double tmp1 = sqrt (- (4.0/3.0)*alpha),
193  tmp2 = acos (-sqrt (-27.0/alpha3)*0.5*beta)/3.0;
194  roots.push_back (tmp1*cos (tmp2) - resubValue);
195  roots.push_back (-tmp1*cos (tmp2 + M_PI/3.0) - resubValue);
196  roots.push_back (-tmp1*cos (tmp2 - M_PI/3.0) - resubValue);
197  }
198
199 #if 0
200  cout << __PRETTY_FUNCTION__ << ": Found "<<roots.size ()<<" roots.\n";
201  for (unsigned int i=0; i<roots.size (); i++)
202  {
203  real x=roots[i], x2=x*x, x3=x2*x;
204  real result = a*x3 + b*x2 + c*x + d;
205  if (fabs (result) > 1e-4)
206  {
207  cout << "Something went wrong:\n";
208  //roots.clear ();
209  }
210  cout << "Root "<<i<<" = "<<roots[i]<<". ("<<a<<"x^3 + "<<b<<"x^2 + "<<c<<"x + "<<d<<" = "<<result<<")\n";
211  }
212  cout << "\n\n";
213 #endif
214 }
215
216 ////////////////////////////////////
217
218 template<typename real>
219 inline void
220  pcl::PolynomialCalculationsT<real>::solveQuarticEquation (real a, real b, real c, real d, real e,
221  std::vector<real>& roots) const
222 {
223  //cout << "Trying to solve "<<a<<"x^4 + "<<b<<"x^3 + "<<c<<"x^2 + "<<d<<"x + "<<e<<" = 0\n";
224
225  if (isNearlyZero (a))
226  {
227  //cout << "Highest order element is 0 => Calling solveCubicEquation.\n";
228  solveCubicEquation (b, c, d, e, roots);
229  return;
230  }
231
232  if (isNearlyZero (e))
233  {
234  roots.push_back (0.0);
235  //cout << "Constant element is 0 => Adding root 0 and calling solveCubicEquation.\n";
236  std::vector<real> tmpRoots;
237  solveCubicEquation (a, b, c, d, tmpRoots);
238  for (unsigned int i=0; i<tmpRoots.size (); i++)
239  if (!isNearlyZero (tmpRoots[i]))
240  roots.push_back (tmpRoots[i]);
241  return;
242  }
243
244  double root1, root2, root3, root4,
245  a2 = a*a,
246  a3 = a2*a,
247  a4 = a2*a2,
248  b2 = b*b,
249  b3 = b2*b,
250  b4 = b2*b2,
251  alpha = ( (-3.0*b2)/ (8.0*a2)) + (c/a),
252  beta = (b3/ (8.0*a3)) - ( (b*c)/ (2.0*a2)) + (d/a),
253  gamma = ( (-3.0*b4)/ (256.0*a4)) + ( (c*b2)/ (16.0*a3)) - ( (b*d)/ (4.0*a2)) + (e/a),
254  alpha2 = alpha*alpha;
255
256  // Value for resubstitution:
257  double resubValue = b/ (4*a);
258
259  //cout << "Trying to solve y^4 + "<<alpha<<"y^2 + "<<beta<<"y + "<<gamma<<"\n";
260
261  if (isNearlyZero (beta))
262  { // y^4 + alpha*y^2 + gamma\n";
263  //cout << "Using beta=0 condition\n";
264  std::vector<real> tmpRoots;
265  solveQuadraticEquation (1.0, alpha, gamma, tmpRoots);
266  for (unsigned int i=0; i<tmpRoots.size (); i++)
267  {
268  double qudraticRoot = tmpRoots[i];
269  if (sqrtIsNearlyZero (qudraticRoot))
270  {
271  roots.push_back (-resubValue);
272  }
273  else if (qudraticRoot > 0.0)
274  {
275  root1 = sqrt (qudraticRoot);
276  roots.push_back (root1 - resubValue);
277  roots.push_back (-root1 - resubValue);
278  }
279  }
280  }
281  else
282  {
283  //cout << "beta != 0\n";
284  double alpha3 = alpha2*alpha,
285  beta2 = beta*beta,
286  p = (-alpha2/12.0)-gamma,
287  q = (-alpha3/108.0)+ ( (alpha*gamma)/3.0)- (beta2/8.0),
288  q2 = q*q,
289  p3 = p*p*p,
290  u = (0.5*q) + sqrt ( (0.25*q2)+ (p3/27.0));
291  if (u > 0.0)
292  u = pow (u, 1.0/3.0);
293  else if (isNearlyZero (u))
294  u = 0.0;
295  else
296  u = -pow (-u, 1.0/3.0);
297
298  double y = (-5.0/6.0)*alpha - u;
299  if (!isNearlyZero (u))
300  y += p/ (3.0*u);
301
302  double w = alpha + 2.0*y;
303
304  if (w > 0)
305  {
306  w = sqrt (w);
307  }
308  else if (isNearlyZero (w))
309  {
310  w = 0;
311  }
312  else
313  {
314  //cout << "Found no roots\n";
315  return;
316  }
317
318  double tmp1 = - (3.0*alpha + 2.0*y + 2.0* (beta/w)),
319  tmp2 = - (3.0*alpha + 2.0*y - 2.0* (beta/w));
320
321  if (tmp1 > 0)
322  {
323  tmp1 = sqrt (tmp1);
324  root1 = - (b/ (4.0*a)) + 0.5* (w+tmp1);
325  root2 = - (b/ (4.0*a)) + 0.5* (w-tmp1);
326  roots.push_back (root1);
327  roots.push_back (root2);
328  }
329  else if (isNearlyZero (tmp1))
330  {
331  root1 = - (b/ (4.0*a)) + 0.5*w;
332  roots.push_back (root1);
333  }
334
335  if (tmp2 > 0)
336  {
337  tmp2 = sqrt (tmp2);
338  root3 = - (b/ (4.0*a)) + 0.5* (-w+tmp2);
339  root4 = - (b/ (4.0*a)) + 0.5* (-w-tmp2);
340  roots.push_back (root3);
341  roots.push_back (root4);
342  }
343  else if (isNearlyZero (tmp2))
344  {
345  root3 = - (b/ (4.0*a)) - 0.5*w;
346  roots.push_back (root3);
347  }
348
349  //cout << "Test: " << alpha<<", "<<beta<<", "<<gamma<<", "<<p<<", "<<q<<", "<<u <<", "<<y<<", "<<w<<"\n";
350  }
351
352 #if 0
353  cout << __PRETTY_FUNCTION__ << ": Found "<<roots.size ()<<" roots.\n";
354  for (unsigned int i=0; i<roots.size (); i++)
355  {
356  real x=roots[i], x2=x*x, x3=x2*x, x4=x2*x2;
357  real result = a*x4 + b*x3 + c*x2 + d*x + e;
358  if (fabs (result) > 1e-4)
359  {
360  cout << "Something went wrong:\n";
361  //roots.clear ();
362  }
363  cout << "Root "<<i<<" = "<<roots[i]
364  << ". ("<<a<<"x^4 + "<<b<<"x^3 + "<<c<<"x^2 + "<<d<<"x + "<<e<<" = "<<result<<")\n";
365  }
366  cout << "\n\n";
367 #endif
368 }
369
370 ////////////////////////////////////
371
372 template<typename real>
375  std::vector<Eigen::Matrix<real, 3, 1> >& samplePoints, unsigned int polynomial_degree, bool& error) const
376 {
378  error = bivariatePolynomialApproximation (samplePoints, polynomial_degree, ret);
379  return ret;
380 }
381
382 ////////////////////////////////////
383
384 template<typename real>
385 inline bool
387  std::vector<Eigen::Matrix<real, 3, 1> >& samplePoints, unsigned int polynomial_degree,
389 {
390  //MEASURE_FUNCTION_TIME;
391  unsigned int parameters_size = BivariatePolynomialT<real>::getNoOfParametersFromDegree (polynomial_degree);
392  //cout << PVARN (parameters_size);
393
394  //cout << "Searching for the "<<parameters_size<<" parameters for the bivariate polynom of degree "
395  // << polynomial_degree<<" using "<<samplePoints.size ()<<" points.\n";
396
397  if (parameters_size > samplePoints.size ()) // Too many parameters for this number of equations (points)?
398  {
399  return false;
400  // Reduce degree of polynomial
401  //polynomial_degree = (unsigned int) (0.5f* (sqrtf (8*samplePoints.size ()+1) - 3));
402  //parameters_size = BivariatePolynomialT<real>::getNoOfParametersFromDegree (polynomial_degree);
403  //cout << "Not enough points, so degree of polynomial was decreased to "<<polynomial_degree
404  // << " ("<<samplePoints.size ()<<" points => "<<parameters_size<<" parameters)\n";
405  }
406
407  ret.setDegree (polynomial_degree);
408
409  //double coeffStuffStartTime=-get_time ();
410  Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> A (parameters_size, parameters_size);
411  A.setZero();
412  Eigen::Matrix<real, Eigen::Dynamic, 1> b (parameters_size);
413  b.setZero();
414  real currentX, currentY, currentZ;
415  real tmpX, tmpY;
416  real *tmpC = new real[parameters_size];
417  real* tmpCEndPtr = &tmpC[parameters_size-1];
418  for (typename std::vector<Eigen::Matrix<real, 3, 1> >::const_iterator it=samplePoints.begin ();
419  it!=samplePoints.end (); ++it)
420  {
421  currentX= (*it)[0]; currentY= (*it)[1]; currentZ= (*it)[2];
422  //cout << "current point: "<<currentX<<","<<currentY<<" => "<<currentZ<<"\n";
423  //unsigned int posInC = parameters_size-1;
424  real* tmpCPtr = tmpCEndPtr;
425  tmpX = 1.0;
426  for (unsigned int xDegree=0; xDegree<=polynomial_degree; ++xDegree)
427  {
428  tmpY = 1.0;
429  for (unsigned int yDegree=0; yDegree<=polynomial_degree-xDegree; ++yDegree)
430  {
431  * (tmpCPtr--) = tmpX*tmpY;
432  //cout << "x="<<currentX<<", y="<<currentY<<", Pos "<<posInC--<<": "<<tmpX<<"*"<<tmpY<<"="<<tmpC[posInC]<<"\n";
433  tmpY *= currentY;
434  }
435  tmpX *= currentX;
436  }
437
438  real* APtr = &A(0,0);
439  real* bPtr = &b[0];
440  real* tmpCPtr1=tmpC;
441  for (unsigned int i=0; i<parameters_size; ++i)
442  {
443  * (bPtr++) += currentZ * *tmpCPtr1;
444
445  real* tmpCPtr2=tmpC;
446  for (unsigned int j=0; j<parameters_size; ++j)
447  {
448  * (APtr++) += *tmpCPtr1 * * (tmpCPtr2++);
449  }
450
451  ++tmpCPtr1;
452  }
453  //A += DMatrix<real>::outProd (tmpC);
454  //b += currentZ * tmpC;
455  }
456  //cout << "Calculating matrix A and vector b (size "<<b.size ()<<") from "<<samplePoints.size ()<<" points took "
457  //<< (coeffStuffStartTime+get_time ())*1000<<"ms using constant memory.\n";
458  //cout << PVARC (A)<<PVARN (b);
459
460
461  //double coeffStuffStartTime=-get_time ();
462  //DMatrix<real> A (parameters_size, parameters_size);
463  //DVector<real> b (parameters_size);
464  //real currentX, currentY, currentZ;
465  //unsigned int posInC;
466  //real tmpX, tmpY;
467  //DVector<real> tmpC (parameters_size);
468  //for (typename std::vector<Eigen::Matrix<real, 3, 1> >::const_iterator it=samplePoints.begin ();
469  // it!=samplePoints.end (); ++it)
470  //{
471  //currentX= (*it)[0]; currentY= (*it)[1]; currentZ= (*it)[2];
472  ////cout << "x="<<currentX<<", y="<<currentY<<"\n";
473  //posInC = parameters_size-1;
474  //tmpX = 1.0;
475  //for (unsigned int xDegree=0; xDegree<=polynomial_degree; xDegree++)
476  //{
477  //tmpY = 1.0;
478  //for (unsigned int yDegree=0; yDegree<=polynomial_degree-xDegree; yDegree++)
479  //{
480  //tmpC[posInC] = tmpX*tmpY;
481  ////cout << "x="<<currentX<<", y="<<currentY<<", Pos "<<posInC<<": "<<tmpX<<"*"<<tmpY<<"="<<tmpC[posInC]<<"\n";
482  //tmpY *= currentY;
483  //posInC--;
484  //}
485  //tmpX *= currentX;
486  //}
487  //A += DMatrix<real>::outProd (tmpC);
488  //b += currentZ * tmpC;
489  //}
490  //cout << "Calculating matrix A and vector b (size "<<b.size ()<<") from "<<samplePoints.size ()<<" points took "
491  //<< (coeffStuffStartTime+get_time ())*1000<<"ms.\n";
492
493  Eigen::Matrix<real, Eigen::Dynamic, 1> parameters;
494  //double choleskyStartTime=-get_time ();
495  //parameters = A.choleskySolve (b);
496  //cout << "Cholesky took "<< (choleskyStartTime+get_time ())*1000<<"ms.\n";
497
498  //double invStartTime=-get_time ();
499  parameters = A.inverse () * b;
500  //cout << "Inverse took "<< (invStartTime+get_time ())*1000<<"ms.\n";
501
502  //cout << PVARC (A)<<PVARC (b)<<PVARN (parameters);
503
504  real inversionCheckResult = (A*parameters - b).norm ();
505  if (inversionCheckResult > 1e-5)
506  {
507  //cout << "Inversion result: "<< inversionCheckResult<<" for matrix "<<A<<"\n";
508  return false;
509  }
510
511  for (unsigned int i=0; i<parameters_size; i++)
512  ret.parameters[i] = parameters[i];
513
514  //cout << "Resulting polynomial is "<<ret<<"\n";
515