Point Cloud Library (PCL)  1.10.1
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 a2 = a*a,
271  a3 = a2*a,
272  a4 = a2*a2,
273  b2 = b*b,
274  b3 = b2*b,
275  b4 = b2*b2,
276  alpha = ( (-3.0*b2)/ (8.0*a2)) + (c/a),
277  beta = (b3/ (8.0*a3)) - ( (b*c)/ (2.0*a2)) + (d/a),
278  gamma = ( (-3.0*b4)/ (256.0*a4)) + ( (c*b2)/ (16.0*a3)) - ( (b*d)/ (4.0*a2)) + (e/a),
279  alpha2 = alpha*alpha;
280 
281  // Value for resubstitution:
282  double resubValue = b/ (4*a);
283 
284  //std::cout << "Trying to solve y^4 + "<<alpha<<"y^2 + "<<beta<<"y + "<<gamma<<"\n";
285 
286  if (isNearlyZero (beta))
287  { // y^4 + alpha*y^2 + gamma\n";
288  //std::cout << "Using beta=0 condition\n";
289  std::vector<real> tmpRoots;
290  solveQuadraticEquation (1.0, alpha, gamma, tmpRoots);
291  for (const auto& quadraticRoot: tmpRoots)
292  {
293  if (sqrtIsNearlyZero (quadraticRoot))
294  {
295  roots.push_back (-resubValue);
296  }
297  else if (quadraticRoot > 0.0)
298  {
299  double root1 = sqrt (quadraticRoot);
300  roots.push_back (root1 - resubValue);
301  roots.push_back (-root1 - resubValue);
302  }
303  }
304  }
305  else
306  {
307  //std::cout << "beta != 0\n";
308  double alpha3 = alpha2*alpha,
309  beta2 = beta*beta,
310  p = (-alpha2/12.0)-gamma,
311  q = (-alpha3/108.0)+ ( (alpha*gamma)/3.0)- (beta2/8.0),
312  q2 = q*q,
313  p3 = p*p*p,
314  u = (0.5*q) + sqrt ( (0.25*q2)+ (p3/27.0));
315  if (u > 0.0)
316  u = pow (u, 1.0/3.0);
317  else if (isNearlyZero (u))
318  u = 0.0;
319  else
320  u = -pow (-u, 1.0/3.0);
321 
322  double y = (-5.0/6.0)*alpha - u;
323  if (!isNearlyZero (u))
324  y += p/ (3.0*u);
325 
326  double w = alpha + 2.0*y;
327 
328  if (w > 0)
329  {
330  w = sqrt (w);
331  }
332  else if (isNearlyZero (w))
333  {
334  w = 0;
335  }
336  else
337  {
338  //std::cout << "Found no roots\n";
339  return;
340  }
341 
342  double tmp1 = - (3.0*alpha + 2.0*y + 2.0* (beta/w)),
343  tmp2 = - (3.0*alpha + 2.0*y - 2.0* (beta/w));
344 
345  if (tmp1 > 0)
346  {
347  tmp1 = sqrt (tmp1);
348  double root1 = - (b/ (4.0*a)) + 0.5* (w+tmp1);
349  double root2 = - (b/ (4.0*a)) + 0.5* (w-tmp1);
350  roots.push_back (root1);
351  roots.push_back (root2);
352  }
353  else if (isNearlyZero (tmp1))
354  {
355  double root1 = - (b/ (4.0*a)) + 0.5*w;
356  roots.push_back (root1);
357  }
358 
359  if (tmp2 > 0)
360  {
361  tmp2 = sqrt (tmp2);
362  double root3 = - (b/ (4.0*a)) + 0.5* (-w+tmp2);
363  double root4 = - (b/ (4.0*a)) + 0.5* (-w-tmp2);
364  roots.push_back (root3);
365  roots.push_back (root4);
366  }
367  else if (isNearlyZero (tmp2))
368  {
369  double root3 = - (b/ (4.0*a)) - 0.5*w;
370  roots.push_back (root3);
371  }
372 
373  //std::cout << "Test: " << alpha<<", "<<beta<<", "<<gamma<<", "<<p<<", "<<q<<", "<<u <<", "<<y<<", "<<w<<"\n";
374  }
375 
376 #if 0
377  std::cout << __PRETTY_FUNCTION__ << ": Found "<<roots.size ()<<" roots.\n";
378  for (unsigned int i=0; i<roots.size (); i++)
379  {
380  real x=roots[i], x2=x*x, x3=x2*x, x4=x2*x2;
381  real result = a*x4 + b*x3 + c*x2 + d*x + e;
382  if (std::abs (result) > 1e-4)
383  {
384  std::cout << "Something went wrong:\n";
385  //roots.clear ();
386  }
387  std::cout << "Root "<<i<<" = "<<roots[i]
388  << ". ("<<a<<"x^4 + "<<b<<"x^3 + "<<c<<"x^2 + "<<d<<"x + "<<e<<" = "<<result<<")\n";
389  }
390  std::cout << "\n\n";
391 #endif
392 }
393 
394 ////////////////////////////////////
395 
396 template<typename real>
399  std::vector<Eigen::Matrix<real, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<real, 3, 1> > >& samplePoints, unsigned int polynomial_degree, bool& error) const
400 {
402  error = bivariatePolynomialApproximation (samplePoints, polynomial_degree, ret);
403  return ret;
404 }
405 
406 ////////////////////////////////////
407 
408 template<typename real>
409 inline bool
411  std::vector<Eigen::Matrix<real, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<real, 3, 1> > >& samplePoints, unsigned int polynomial_degree,
413 {
414  const auto parameters_size = BivariatePolynomialT<real>::getNoOfParametersFromDegree (polynomial_degree);
415 
416  // Too many parameters for this number of equations (points)?
417  if (parameters_size > samplePoints.size ())
418  {
419  return false;
420  // Reduce degree of polynomial
421  //polynomial_degree = (unsigned int) (0.5f* (std::sqrt (8*samplePoints.size ()+1) - 3));
422  //parameters_size = BivariatePolynomialT<real>::getNoOfParametersFromDegree (polynomial_degree);
423  //std::cout << "Not enough points, so degree of polynomial was decreased to "<<polynomial_degree
424  // << " ("<<samplePoints.size ()<<" points => "<<parameters_size<<" parameters)\n";
425  }
426 
427  ret.setDegree (polynomial_degree);
428 
429  // A is a symmetric matrix
430  Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> A (parameters_size, parameters_size);
431  A.setZero();
432  Eigen::Matrix<real, Eigen::Dynamic, 1> b (parameters_size);
433  b.setZero();
434 
435  {
436  std::vector<real> C (parameters_size);
437  for (const auto& point: samplePoints)
438  {
439  real currentX = point[0], currentY = point[1], currentZ = point[2];
440 
441  {
442  auto CRevPtr = C.rbegin ();
443  real tmpX = 1.0;
444  for (unsigned int xDegree=0; xDegree<=polynomial_degree; ++xDegree)
445  {
446  real tmpY = 1.0;
447  for (unsigned int yDegree=0; yDegree<=polynomial_degree-xDegree; ++yDegree, ++CRevPtr)
448  {
449  *CRevPtr = tmpX*tmpY;
450  tmpY *= currentY;
451  }
452  tmpX *= currentX;
453  }
454  }
455 
456  for (std::size_t i=0; i<parameters_size; ++i)
457  {
458  b[i] += currentZ * C[i];
459  // fill the upper right triangular matrix
460  for (std::size_t j=i; j<parameters_size; ++j)
461  {
462  A (i, j) += C[i] * C[j];
463  }
464  }
465  //A += DMatrix<real>::outProd (C);
466  //b += currentZ * C;
467  }
468  }
469 
470  // The Eigen only solution is slow for small matrices. Maybe file a bug
471  // A.traingularView<Eigen::StrictlyLower> = A.transpose();
472  // copy upper-right elements to lower-left
473  for (std::size_t i = 0; i < parameters_size; ++i)
474  {
475  for (std::size_t j = 0; j < i; ++j)
476  {
477  A (i, j) = A (j, i);
478  }
479  }
480 
481  Eigen::Matrix<real, Eigen::Dynamic, 1> parameters;
482  //double choleskyStartTime=-get_time ();
483  //parameters = A.choleskySolve (b);
484  //std::cout << "Cholesky took "<< (choleskyStartTime+get_time ())*1000<<"ms.\n";
485 
486  //double invStartTime=-get_time ();
487  parameters = A.inverse () * b;
488  //std::cout << "Inverse took "<< (invStartTime+get_time ())*1000<<"ms.\n";
489 
490  //std::cout << PVARC (A)<<PVARC (b)<<PVARN (parameters);
491 
492  real inversionCheckResult = (A*parameters - b).norm ();
493  if (inversionCheckResult > 1e-5)
494  {
495  //std::cout << "Inversion result: "<< inversionCheckResult<<" for matrix "<<A<<"\n";
496  return false;
497  }
498 
499  std::copy_n(parameters.data(), parameters_size, ret.parameters);
500 
501  //std::cout << "Resulting polynomial is "<<ret<<"\n";
502 
503  //Test of gradient: ret.calculateGradient ();
504 
505  return true;
506 }
507 
508 #endif // PCL_POLYNOMIAL_CALCULATIONS_HPP_
509 
pcl::PolynomialCalculationsT::Parameters::zero_value
real zero_value
Every value below this is considered to be zero.
Definition: polynomial_calculations.h:61
pcl::PolynomialCalculationsT::bivariatePolynomialApproximation
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.
Definition: polynomial_calculations.hpp:398
pcl::PolynomialCalculationsT::sqrtIsNearlyZero
bool sqrtIsNearlyZero(real d) const
check if sqrt(std::abs(d))<zeroValue
Definition: polynomial_calculations.h:114
pcl::PolynomialCalculationsT::solveCubicEquation
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...
Definition: polynomial_calculations.hpp:144
pcl::BivariatePolynomialT::parameters
real * parameters
Definition: bivariate_polynomial.h:118
pcl::PolynomialCalculationsT::isNearlyZero
bool isNearlyZero(real d) const
check if std::abs(d)<zeroValue
Definition: polynomial_calculations.h:107
pcl::PolynomialCalculationsT::Parameters::setZeroValue
void setZeroValue(real new_zero_value)
Set zero_value.
Definition: polynomial_calculations.hpp:45
pcl::BivariatePolynomialT::getNoOfParametersFromDegree
static unsigned int getNoOfParametersFromDegree(int n)
How many parameters has a bivariate polynomial of the given degree.
Definition: bivariate_polynomial.h:114
pcl::PolynomialCalculationsT::solveLinearEquation
void solveLinearEquation(real a, real b, std::vector< real > &roots) const
Solves an equation of the form ax + b = 0.
Definition: polynomial_calculations.hpp:55
pcl::PolynomialCalculationsT::Parameters::sqr_zero_value
real sqr_zero_value
sqr of the above
Definition: polynomial_calculations.h:62
pcl::BivariatePolynomialT::setDegree
void setDegree(int new_degree)
Initialize members to default values.
Definition: bivariate_polynomial.hpp:73
pcl::BivariatePolynomialT
This represents a bivariate polynomial and provides some functionality for it.
Definition: bivariate_polynomial.h:53
pcl::PolynomialCalculationsT::solveQuarticEquation
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....
Definition: polynomial_calculations.hpp:246
pcl::PolynomialCalculationsT::solveQuadraticEquation
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.
Definition: polynomial_calculations.hpp:88