Point Cloud Library (PCL)  1.7.2
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 
516  //Test of gradient: ret.calculateGradient ();
517 
518  delete [] tmpC;
519  return true;
520 }
This represents a bivariate polynomial and provides some functionality for it.
BivariatePolynomialT< real > bivariatePolynomialApproximation(std::vector< Eigen::Matrix< real, 3, 1 > > &samplePoints, unsigned int polynomial_degree, bool &error) const
Get the bivariate polynomial approximation for Z(X,Y) from the given sample points.
bool sqrtIsNearlyZero(real d) const
check if sqrt(fabs(d))
void solveQuarticEquation(real a, real b, real c, real d, real e, std::vector< real > &roots) const
Solves an equation of the form ax^4 + bx^3 + cx^2 +dx + e = 0 See http://en.wikipedia.org/wiki/Quartic_equation#Summary_of_Ferrari.27s_method.
void setZeroValue(real new_zero_value)
Set zero_value.
void setDegree(int new_degree)
Initialize members to default values.
real zero_value
Every value below this is considered to be zero.
bool isNearlyZero(real d) const
check if fabs(d)
void solveQuadraticEquation(real a, real b, real c, std::vector< real > &roots) const
Solves an equation of the form ax^2 + bx + c = 0 See http://en.wikipedia.org/wiki/Quadratic_equation...
void solveCubicEquation(real a, real b, real c, real d, std::vector< real > &roots) const
Solves an equation of the form ax^3 + bx^2 + cx + d = 0 See http://en.wikipedia.org/wiki/Cubic_equati...
static unsigned int getNoOfParametersFromDegree(int n)
How many parametes has a bivariate polynomial of the given degree.
void solveLinearEquation(real a, real b, std::vector< real > &roots) const
Solves an equation of the form ax + b = 0.