Point Cloud Library (PCL)  1.9.1-dev
polynomial_calculations.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010, Willow Garage, Inc.
6  * Copyright (c) 2012-, Open Perception, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of the copyright holder(s) nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  */
38 #ifndef PCL_POLYNOMIAL_CALCULATIONS_HPP_
39 #define PCL_POLYNOMIAL_CALCULATIONS_HPP_
40 
41 ////////////////////////////////////
42 
43 template <typename real>
45 {
46 }
47 
48 ////////////////////////////////////
49 
50 template <typename real>
52 {
53 }
54 
55 ////////////////////////////////////
56 
57 template <typename real>
58 inline void
60 {
61  zero_value = new_zero_value;
63 }
64 
65 ////////////////////////////////////
66 
67 template <typename real>
68 inline void
69  pcl::PolynomialCalculationsT<real>::solveLinearEquation (real a, real b, std::vector<real>& roots) const
70 {
71  //cout << "Trying to solve "<<a<<"x + "<<b<<" = 0\n";
72 
73  if (isNearlyZero (b))
74  {
75  roots.push_back (0.0);
76  }
77  if (!isNearlyZero (a/b))
78  {
79  roots.push_back (-b/a);
80  }
81 
82 #if 0
83  cout << __PRETTY_FUNCTION__ << ": Found "<<roots.size ()<<" roots.\n";
84  for (unsigned int i=0; i<roots.size (); i++)
85  {
86  real x=roots[i];
87  real result = a*x + b;
88  if (!isNearlyZero (result))
89  {
90  cout << "Something went wrong during solving of polynomial "<<a<<"x + "<<b<<" = 0\n";
91  //roots.clear ();
92  }
93  cout << "Root "<<i<<" = "<<roots[i]<<". ("<<a<<"x^ + "<<b<<" = "<<result<<")\n";
94  }
95 #endif
96 }
97 
98 ////////////////////////////////////
99 
100 template <typename real>
101 inline void
102  pcl::PolynomialCalculationsT<real>::solveQuadraticEquation (real a, real b, real c, std::vector<real>& roots) const
103 {
104  //cout << "Trying to solve "<<a<<"x^2 + "<<b<<"x + "<<c<<" = 0\n";
105 
106  if (isNearlyZero (a))
107  {
108  //cout << "Highest order element is 0 => Calling solveLineaqrEquation.\n";
109  solveLinearEquation (b, c, roots);
110  return;
111  }
112 
113  if (isNearlyZero (c))
114  {
115  roots.push_back (0.0);
116  //cout << "Constant element is 0 => Adding root 0 and calling solveLinearEquation.\n";
117  std::vector<real> tmpRoots;
118  solveLinearEquation (a, b, tmpRoots);
119  for (unsigned int i=0; i<tmpRoots.size (); i++)
120  if (!isNearlyZero (tmpRoots[i]))
121  roots.push_back (tmpRoots[i]);
122  return;
123  }
124 
125  real tmp = b*b - 4*a*c;
126  if (tmp>0)
127  {
128  tmp = sqrt (tmp);
129  real tmp2 = 1.0/ (2*a);
130  roots.push_back ( (-b + tmp)*tmp2);
131  roots.push_back ( (-b - tmp)*tmp2);
132  }
133  else if (sqrtIsNearlyZero (tmp))
134  {
135  roots.push_back (-b/ (2*a));
136  }
137 
138 #if 0
139  cout << __PRETTY_FUNCTION__ << ": Found "<<roots.size ()<<" roots.\n";
140  for (unsigned int i=0; i<roots.size (); i++)
141  {
142  real x=roots[i], x2=x*x;
143  real result = a*x2 + b*x + c;
144  if (!isNearlyZero (result))
145  {
146  cout << "Something went wrong during solving of polynomial "<<a<<"x^2 + "<<b<<"x + "<<c<<" = 0\n";
147  //roots.clear ();
148  }
149  //cout << "Root "<<i<<" = "<<roots[i]<<". ("<<a<<"x^2 + "<<b<<"x + "<<c<<" = "<<result<<")\n";
150  }
151 #endif
152 }
153 
154 ////////////////////////////////////
155 
156 template<typename real>
157 inline void
158  pcl::PolynomialCalculationsT<real>::solveCubicEquation (real a, real b, real c, real d, std::vector<real>& roots) const
159 {
160  //cout << "Trying to solve "<<a<<"x^3 + "<<b<<"x^2 + "<<c<<"x + "<<d<<" = 0\n";
161 
162  if (isNearlyZero (a))
163  {
164  //cout << "Highest order element is 0 => Calling solveQuadraticEquation.\n";
165  solveQuadraticEquation (b, c, d, roots);
166  return;
167  }
168 
169  if (isNearlyZero (d))
170  {
171  roots.push_back (0.0);
172  //cout << "Constant element is 0 => Adding root 0 and calling solveQuadraticEquation.\n";
173  std::vector<real> tmpRoots;
174  solveQuadraticEquation (a, b, c, tmpRoots);
175  for (unsigned int i=0; i<tmpRoots.size (); i++)
176  if (!isNearlyZero (tmpRoots[i]))
177  roots.push_back (tmpRoots[i]);
178  return;
179  }
180 
181  double a2 = a*a,
182  a3 = a2*a,
183  b2 = b*b,
184  b3 = b2*b,
185  alpha = ( (3.0*a*c-b2)/ (3.0*a2)),
186  beta = (2*b3/ (27.0*a3)) - ( (b*c)/ (3.0*a2)) + (d/a),
187  alpha2 = alpha*alpha,
188  alpha3 = alpha2*alpha,
189  beta2 = beta*beta;
190 
191  // Value for resubstitution:
192  double resubValue = b/ (3*a);
193 
194  //cout << "Trying to solve y^3 + "<<alpha<<"y + "<<beta<<"\n";
195 
196  double discriminant = (alpha3/27.0) + 0.25*beta2;
197 
198  //cout << "Discriminant is "<<discriminant<<"\n";
199 
200  if (isNearlyZero (discriminant))
201  {
202  if (!isNearlyZero (alpha) || !isNearlyZero (beta))
203  {
204  roots.push_back ( (-3.0*beta)/ (2.0*alpha) - resubValue);
205  roots.push_back ( (3.0*beta)/alpha - resubValue);
206  }
207  else
208  {
209  roots.push_back (-resubValue);
210  }
211  }
212  else if (discriminant > 0)
213  {
214  double sqrtDiscriminant = sqrt (discriminant);
215  double d1 = -0.5*beta + sqrtDiscriminant,
216  d2 = -0.5*beta - sqrtDiscriminant;
217  if (d1 < 0)
218  d1 = -pow (-d1, 1.0/3.0);
219  else
220  d1 = pow (d1, 1.0/3.0);
221 
222  if (d2 < 0)
223  d2 = -pow (-d2, 1.0/3.0);
224  else
225  d2 = pow (d2, 1.0/3.0);
226 
227  //cout << PVAR (d1)<<", "<<PVAR (d2)<<"\n";
228  roots.push_back (d1 + d2 - resubValue);
229  }
230  else
231  {
232  double tmp1 = sqrt (- (4.0/3.0)*alpha),
233  tmp2 = std::acos (-sqrt (-27.0/alpha3)*0.5*beta)/3.0;
234  roots.push_back (tmp1*cos (tmp2) - resubValue);
235  roots.push_back (-tmp1*cos (tmp2 + M_PI/3.0) - resubValue);
236  roots.push_back (-tmp1*cos (tmp2 - M_PI/3.0) - resubValue);
237  }
238 
239 #if 0
240  cout << __PRETTY_FUNCTION__ << ": Found "<<roots.size ()<<" roots.\n";
241  for (unsigned int i=0; i<roots.size (); i++)
242  {
243  real x=roots[i], x2=x*x, x3=x2*x;
244  real result = a*x3 + b*x2 + c*x + d;
245  if (fabs (result) > 1e-4)
246  {
247  cout << "Something went wrong:\n";
248  //roots.clear ();
249  }
250  cout << "Root "<<i<<" = "<<roots[i]<<". ("<<a<<"x^3 + "<<b<<"x^2 + "<<c<<"x + "<<d<<" = "<<result<<")\n";
251  }
252  cout << "\n\n";
253 #endif
254 }
255 
256 ////////////////////////////////////
257 
258 template<typename real>
259 inline void
260  pcl::PolynomialCalculationsT<real>::solveQuarticEquation (real a, real b, real c, real d, real e,
261  std::vector<real>& roots) const
262 {
263  //cout << "Trying to solve "<<a<<"x^4 + "<<b<<"x^3 + "<<c<<"x^2 + "<<d<<"x + "<<e<<" = 0\n";
264 
265  if (isNearlyZero (a))
266  {
267  //cout << "Highest order element is 0 => Calling solveCubicEquation.\n";
268  solveCubicEquation (b, c, d, e, roots);
269  return;
270  }
271 
272  if (isNearlyZero (e))
273  {
274  roots.push_back (0.0);
275  //cout << "Constant element is 0 => Adding root 0 and calling solveCubicEquation.\n";
276  std::vector<real> tmpRoots;
277  solveCubicEquation (a, b, c, d, tmpRoots);
278  for (unsigned int i=0; i<tmpRoots.size (); i++)
279  if (!isNearlyZero (tmpRoots[i]))
280  roots.push_back (tmpRoots[i]);
281  return;
282  }
283 
284  double root1, root2, root3, root4,
285  a2 = a*a,
286  a3 = a2*a,
287  a4 = a2*a2,
288  b2 = b*b,
289  b3 = b2*b,
290  b4 = b2*b2,
291  alpha = ( (-3.0*b2)/ (8.0*a2)) + (c/a),
292  beta = (b3/ (8.0*a3)) - ( (b*c)/ (2.0*a2)) + (d/a),
293  gamma = ( (-3.0*b4)/ (256.0*a4)) + ( (c*b2)/ (16.0*a3)) - ( (b*d)/ (4.0*a2)) + (e/a),
294  alpha2 = alpha*alpha;
295 
296  // Value for resubstitution:
297  double resubValue = b/ (4*a);
298 
299  //cout << "Trying to solve y^4 + "<<alpha<<"y^2 + "<<beta<<"y + "<<gamma<<"\n";
300 
301  if (isNearlyZero (beta))
302  { // y^4 + alpha*y^2 + gamma\n";
303  //cout << "Using beta=0 condition\n";
304  std::vector<real> tmpRoots;
305  solveQuadraticEquation (1.0, alpha, gamma, tmpRoots);
306  for (unsigned int i=0; i<tmpRoots.size (); i++)
307  {
308  double qudraticRoot = tmpRoots[i];
309  if (sqrtIsNearlyZero (qudraticRoot))
310  {
311  roots.push_back (-resubValue);
312  }
313  else if (qudraticRoot > 0.0)
314  {
315  root1 = sqrt (qudraticRoot);
316  roots.push_back (root1 - resubValue);
317  roots.push_back (-root1 - resubValue);
318  }
319  }
320  }
321  else
322  {
323  //cout << "beta != 0\n";
324  double alpha3 = alpha2*alpha,
325  beta2 = beta*beta,
326  p = (-alpha2/12.0)-gamma,
327  q = (-alpha3/108.0)+ ( (alpha*gamma)/3.0)- (beta2/8.0),
328  q2 = q*q,
329  p3 = p*p*p,
330  u = (0.5*q) + sqrt ( (0.25*q2)+ (p3/27.0));
331  if (u > 0.0)
332  u = pow (u, 1.0/3.0);
333  else if (isNearlyZero (u))
334  u = 0.0;
335  else
336  u = -pow (-u, 1.0/3.0);
337 
338  double y = (-5.0/6.0)*alpha - u;
339  if (!isNearlyZero (u))
340  y += p/ (3.0*u);
341 
342  double w = alpha + 2.0*y;
343 
344  if (w > 0)
345  {
346  w = sqrt (w);
347  }
348  else if (isNearlyZero (w))
349  {
350  w = 0;
351  }
352  else
353  {
354  //cout << "Found no roots\n";
355  return;
356  }
357 
358  double tmp1 = - (3.0*alpha + 2.0*y + 2.0* (beta/w)),
359  tmp2 = - (3.0*alpha + 2.0*y - 2.0* (beta/w));
360 
361  if (tmp1 > 0)
362  {
363  tmp1 = sqrt (tmp1);
364  root1 = - (b/ (4.0*a)) + 0.5* (w+tmp1);
365  root2 = - (b/ (4.0*a)) + 0.5* (w-tmp1);
366  roots.push_back (root1);
367  roots.push_back (root2);
368  }
369  else if (isNearlyZero (tmp1))
370  {
371  root1 = - (b/ (4.0*a)) + 0.5*w;
372  roots.push_back (root1);
373  }
374 
375  if (tmp2 > 0)
376  {
377  tmp2 = sqrt (tmp2);
378  root3 = - (b/ (4.0*a)) + 0.5* (-w+tmp2);
379  root4 = - (b/ (4.0*a)) + 0.5* (-w-tmp2);
380  roots.push_back (root3);
381  roots.push_back (root4);
382  }
383  else if (isNearlyZero (tmp2))
384  {
385  root3 = - (b/ (4.0*a)) - 0.5*w;
386  roots.push_back (root3);
387  }
388 
389  //cout << "Test: " << alpha<<", "<<beta<<", "<<gamma<<", "<<p<<", "<<q<<", "<<u <<", "<<y<<", "<<w<<"\n";
390  }
391 
392 #if 0
393  cout << __PRETTY_FUNCTION__ << ": Found "<<roots.size ()<<" roots.\n";
394  for (unsigned int i=0; i<roots.size (); i++)
395  {
396  real x=roots[i], x2=x*x, x3=x2*x, x4=x2*x2;
397  real result = a*x4 + b*x3 + c*x2 + d*x + e;
398  if (fabs (result) > 1e-4)
399  {
400  cout << "Something went wrong:\n";
401  //roots.clear ();
402  }
403  cout << "Root "<<i<<" = "<<roots[i]
404  << ". ("<<a<<"x^4 + "<<b<<"x^3 + "<<c<<"x^2 + "<<d<<"x + "<<e<<" = "<<result<<")\n";
405  }
406  cout << "\n\n";
407 #endif
408 }
409 
410 ////////////////////////////////////
411 
412 template<typename real>
415  std::vector<Eigen::Matrix<real, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<real, 3, 1> > >& samplePoints, unsigned int polynomial_degree, bool& error) const
416 {
418  error = bivariatePolynomialApproximation (samplePoints, polynomial_degree, ret);
419  return ret;
420 }
421 
422 ////////////////////////////////////
423 
424 template<typename real>
425 inline bool
427  std::vector<Eigen::Matrix<real, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<real, 3, 1> > >& samplePoints, unsigned int polynomial_degree,
429 {
430  //MEASURE_FUNCTION_TIME;
431  unsigned int parameters_size = BivariatePolynomialT<real>::getNoOfParametersFromDegree (polynomial_degree);
432  //cout << PVARN (parameters_size);
433 
434  //cout << "Searching for the "<<parameters_size<<" parameters for the bivariate polynom of degree "
435  // << polynomial_degree<<" using "<<samplePoints.size ()<<" points.\n";
436 
437  if (parameters_size > samplePoints.size ()) // Too many parameters for this number of equations (points)?
438  {
439  return false;
440  // Reduce degree of polynomial
441  //polynomial_degree = (unsigned int) (0.5f* (std::sqrt (8*samplePoints.size ()+1) - 3));
442  //parameters_size = BivariatePolynomialT<real>::getNoOfParametersFromDegree (polynomial_degree);
443  //cout << "Not enough points, so degree of polynomial was decreased to "<<polynomial_degree
444  // << " ("<<samplePoints.size ()<<" points => "<<parameters_size<<" parameters)\n";
445  }
446 
447  ret.setDegree (polynomial_degree);
448 
449  //double coeffStuffStartTime=-get_time ();
450  Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> A (parameters_size, parameters_size);
451  A.setZero();
452  Eigen::Matrix<real, Eigen::Dynamic, 1> b (parameters_size);
453  b.setZero();
454  real currentX, currentY, currentZ;
455  real tmpX, tmpY;
456  real *tmpC = new real[parameters_size];
457  real* tmpCEndPtr = &tmpC[parameters_size-1];
458  for (typename std::vector<Eigen::Matrix<real, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<real, 3, 1> > >::const_iterator it=samplePoints.begin ();
459  it!=samplePoints.end (); ++it)
460  {
461  currentX= (*it)[0]; currentY= (*it)[1]; currentZ= (*it)[2];
462  //cout << "current point: "<<currentX<<","<<currentY<<" => "<<currentZ<<"\n";
463  //unsigned int posInC = parameters_size-1;
464  real* tmpCPtr = tmpCEndPtr;
465  tmpX = 1.0;
466  for (unsigned int xDegree=0; xDegree<=polynomial_degree; ++xDegree)
467  {
468  tmpY = 1.0;
469  for (unsigned int yDegree=0; yDegree<=polynomial_degree-xDegree; ++yDegree)
470  {
471  * (tmpCPtr--) = tmpX*tmpY;
472  //cout << "x="<<currentX<<", y="<<currentY<<", Pos "<<posInC--<<": "<<tmpX<<"*"<<tmpY<<"="<<tmpC[posInC]<<"\n";
473  tmpY *= currentY;
474  }
475  tmpX *= currentX;
476  }
477 
478  real* APtr = &A(0,0);
479  real* bPtr = &b[0];
480  real* tmpCPtr1=tmpC;
481  for (unsigned int i=0; i<parameters_size; ++i)
482  {
483  * (bPtr++) += currentZ * *tmpCPtr1;
484 
485  real* tmpCPtr2=tmpC;
486  for (unsigned int j=0; j<parameters_size; ++j)
487  {
488  * (APtr++) += *tmpCPtr1 * * (tmpCPtr2++);
489  }
490 
491  ++tmpCPtr1;
492  }
493  //A += DMatrix<real>::outProd (tmpC);
494  //b += currentZ * tmpC;
495  }
496  //cout << "Calculating matrix A and vector b (size "<<b.size ()<<") from "<<samplePoints.size ()<<" points took "
497  //<< (coeffStuffStartTime+get_time ())*1000<<"ms using constant memory.\n";
498  //cout << PVARC (A)<<PVARN (b);
499 
500 
501  //double coeffStuffStartTime=-get_time ();
502  //DMatrix<real> A (parameters_size, parameters_size);
503  //DVector<real> b (parameters_size);
504  //real currentX, currentY, currentZ;
505  //unsigned int posInC;
506  //real tmpX, tmpY;
507  //DVector<real> tmpC (parameters_size);
508  //for (typename std::vector<Eigen::Matrix<real, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<real, 3, 1> > >::const_iterator it=samplePoints.begin ();
509  // it!=samplePoints.end (); ++it)
510  //{
511  //currentX= (*it)[0]; currentY= (*it)[1]; currentZ= (*it)[2];
512  ////cout << "x="<<currentX<<", y="<<currentY<<"\n";
513  //posInC = parameters_size-1;
514  //tmpX = 1.0;
515  //for (unsigned int xDegree=0; xDegree<=polynomial_degree; xDegree++)
516  //{
517  //tmpY = 1.0;
518  //for (unsigned int yDegree=0; yDegree<=polynomial_degree-xDegree; yDegree++)
519  //{
520  //tmpC[posInC] = tmpX*tmpY;
521  ////cout << "x="<<currentX<<", y="<<currentY<<", Pos "<<posInC<<": "<<tmpX<<"*"<<tmpY<<"="<<tmpC[posInC]<<"\n";
522  //tmpY *= currentY;
523  //posInC--;
524  //}
525  //tmpX *= currentX;
526  //}
527  //A += DMatrix<real>::outProd (tmpC);
528  //b += currentZ * tmpC;
529  //}
530  //cout << "Calculating matrix A and vector b (size "<<b.size ()<<") from "<<samplePoints.size ()<<" points took "
531  //<< (coeffStuffStartTime+get_time ())*1000<<"ms.\n";
532 
533  Eigen::Matrix<real, Eigen::Dynamic, 1> parameters;
534  //double choleskyStartTime=-get_time ();
535  //parameters = A.choleskySolve (b);
536  //cout << "Cholesky took "<< (choleskyStartTime+get_time ())*1000<<"ms.\n";
537 
538  //double invStartTime=-get_time ();
539  parameters = A.inverse () * b;
540  //cout << "Inverse took "<< (invStartTime+get_time ())*1000<<"ms.\n";
541 
542  //cout << PVARC (A)<<PVARC (b)<<PVARN (parameters);
543 
544  real inversionCheckResult = (A*parameters - b).norm ();
545  if (inversionCheckResult > 1e-5)
546  {
547  //cout << "Inversion result: "<< inversionCheckResult<<" for matrix "<<A<<"\n";
548  return false;
549  }
550 
551  for (unsigned int i=0; i<parameters_size; i++)
552  ret.parameters[i] = parameters[i];
553 
554  //cout << "Resulting polynomial is "<<ret<<"\n";
555 
556  //Test of gradient: ret.calculateGradient ();
557 
558  delete [] tmpC;
559  return true;
560 }
561 
562 #endif // PCL_POLYNOMIAL_CALCULATIONS_HPP_
563 
This represents a bivariate polynomial and provides some functionality for it.
bool sqrtIsNearlyZero(real d) const
check if sqrt(fabs(d))<zeroValue
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)<zeroValue
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 parameters has a bivariate polynomial of the given degree.
BivariatePolynomialT< real > bivariatePolynomialApproximation(std::vector< Eigen::Matrix< real, 3, 1 >, Eigen::aligned_allocator< 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.
void solveLinearEquation(real a, real b, std::vector< real > &roots) const
Solves an equation of the form ax + b = 0.