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