Point Cloud Library (PCL)  1.14.0-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 
39 #pragma once
40 
41 #include <pcl/common/polynomial_calculations.h>
42 
43 
44 namespace pcl
45 {
46 
47 template <typename real>
48 inline void
50 {
51  zero_value = new_zero_value;
53 }
54 
55 
56 template <typename real>
57 inline void
58 PolynomialCalculationsT<real>::solveLinearEquation (real a, real b, std::vector<real>& roots) const
59 {
60  //std::cout << "Trying to solve "<<a<<"x + "<<b<<" = 0\n";
61 
62  if (isNearlyZero (b))
63  {
64  roots.push_back (0.0);
65  }
66  if (!isNearlyZero (a/b))
67  {
68  roots.push_back (-b/a);
69  }
70 
71 #if 0
72  std::cout << __PRETTY_FUNCTION__ << ": Found "<<roots.size ()<<" roots.\n";
73  for (unsigned int i=0; i<roots.size (); i++)
74  {
75  real x=roots[i];
76  real result = a*x + b;
77  if (!isNearlyZero (result))
78  {
79  std::cout << "Something went wrong during solving of polynomial "<<a<<"x + "<<b<<" = 0\n";
80  //roots.clear ();
81  }
82  std::cout << "Root "<<i<<" = "<<roots[i]<<". ("<<a<<"x^ + "<<b<<" = "<<result<<")\n";
83  }
84 #endif
85 }
86 
87 
88 template <typename real>
89 inline void
90 PolynomialCalculationsT<real>::solveQuadraticEquation (real a, real b, real c, std::vector<real>& roots) const
91 {
92  //std::cout << "Trying to solve "<<a<<"x^2 + "<<b<<"x + "<<c<<" = 0\n";
93 
94  if (isNearlyZero (a))
95  {
96  //std::cout << "Highest order element is 0 => Calling solveLineaqrEquation.\n";
97  solveLinearEquation (b, c, roots);
98  return;
99  }
100 
101  if (isNearlyZero (c))
102  {
103  roots.push_back (0.0);
104  //std::cout << "Constant element is 0 => Adding root 0 and calling solveLinearEquation.\n";
105  std::vector<real> tmpRoots;
106  solveLinearEquation (a, b, tmpRoots);
107  for (const auto& tmpRoot: tmpRoots)
108  if (!isNearlyZero (tmpRoot))
109  roots.push_back (tmpRoot);
110  return;
111  }
112 
113  real tmp = b*b - 4*a*c;
114  if (tmp>0)
115  {
116  tmp = sqrt (tmp);
117  real tmp2 = 1.0/ (2*a);
118  roots.push_back ( (-b + tmp)*tmp2);
119  roots.push_back ( (-b - tmp)*tmp2);
120  }
121  else if (sqrtIsNearlyZero (tmp))
122  {
123  roots.push_back (-b/ (2*a));
124  }
125 
126 #if 0
127  std::cout << __PRETTY_FUNCTION__ << ": Found "<<roots.size ()<<" roots.\n";
128  for (unsigned int i=0; i<roots.size (); i++)
129  {
130  real x=roots[i], x2=x*x;
131  real result = a*x2 + b*x + c;
132  if (!isNearlyZero (result))
133  {
134  std::cout << "Something went wrong during solving of polynomial "<<a<<"x^2 + "<<b<<"x + "<<c<<" = 0\n";
135  //roots.clear ();
136  }
137  //std::cout << "Root "<<i<<" = "<<roots[i]<<". ("<<a<<"x^2 + "<<b<<"x + "<<c<<" = "<<result<<")\n";
138  }
139 #endif
140 }
141 
142 
143 template<typename real>
144 inline void
145 PolynomialCalculationsT<real>::solveCubicEquation (real a, real b, real c, real d, std::vector<real>& roots) const
146 {
147  //std::cout << "Trying to solve "<<a<<"x^3 + "<<b<<"x^2 + "<<c<<"x + "<<d<<" = 0\n";
148 
149  if (isNearlyZero (a))
150  {
151  //std::cout << "Highest order element is 0 => Calling solveQuadraticEquation.\n";
152  solveQuadraticEquation (b, c, d, roots);
153  return;
154  }
155 
156  if (isNearlyZero (d))
157  {
158  roots.push_back (0.0);
159  //std::cout << "Constant element is 0 => Adding root 0 and calling solveQuadraticEquation.\n";
160  std::vector<real> tmpRoots;
161  solveQuadraticEquation (a, b, c, tmpRoots);
162  for (const auto& tmpRoot: tmpRoots)
163  if (!isNearlyZero (tmpRoot))
164  roots.push_back (tmpRoot);
165  return;
166  }
167 
168  double a2 = a*a,
169  a3 = a2*a,
170  b2 = b*b,
171  b3 = b2*b,
172  alpha = ( (3.0*a*c-b2)/ (3.0*a2)),
173  beta = (2*b3/ (27.0*a3)) - ( (b*c)/ (3.0*a2)) + (d/a),
174  alpha2 = alpha*alpha,
175  alpha3 = alpha2*alpha,
176  beta2 = beta*beta;
177 
178  // Value for resubstitution:
179  double resubValue = b/ (3*a);
180 
181  //std::cout << "Trying to solve y^3 + "<<alpha<<"y + "<<beta<<"\n";
182 
183  double discriminant = (alpha3/27.0) + 0.25*beta2;
184 
185  //std::cout << "Discriminant is "<<discriminant<<"\n";
186 
187  if (isNearlyZero (discriminant))
188  {
189  if (!isNearlyZero (alpha) || !isNearlyZero (beta))
190  {
191  roots.push_back ( (-3.0*beta)/ (2.0*alpha) - resubValue);
192  roots.push_back ( (3.0*beta)/alpha - resubValue);
193  }
194  else
195  {
196  roots.push_back (-resubValue);
197  }
198  }
199  else if (discriminant > 0)
200  {
201  double sqrtDiscriminant = sqrt (discriminant);
202  double d1 = -0.5*beta + sqrtDiscriminant,
203  d2 = -0.5*beta - sqrtDiscriminant;
204  if (d1 < 0)
205  d1 = -pow (-d1, 1.0/3.0);
206  else
207  d1 = pow (d1, 1.0/3.0);
208 
209  if (d2 < 0)
210  d2 = -pow (-d2, 1.0/3.0);
211  else
212  d2 = pow (d2, 1.0/3.0);
213 
214  //std::cout << PVAR (d1)<<", "<<PVAR (d2)<<"\n";
215  roots.push_back (d1 + d2 - resubValue);
216  }
217  else
218  {
219  double tmp1 = sqrt (- (4.0/3.0)*alpha),
220  tmp2 = std::acos (-sqrt (-27.0/alpha3)*0.5*beta)/3.0;
221  roots.push_back (tmp1*std::cos (tmp2) - resubValue);
222  roots.push_back (-tmp1*std::cos (tmp2 + M_PI/3.0) - resubValue);
223  roots.push_back (-tmp1*std::cos (tmp2 - M_PI/3.0) - resubValue);
224  }
225 
226 #if 0
227  std::cout << __PRETTY_FUNCTION__ << ": Found "<<roots.size ()<<" roots.\n";
228  for (unsigned int i=0; i<roots.size (); i++)
229  {
230  real x=roots[i], x2=x*x, x3=x2*x;
231  real result = a*x3 + b*x2 + c*x + d;
232  if (std::abs (result) > 1e-4)
233  {
234  std::cout << "Something went wrong:\n";
235  //roots.clear ();
236  }
237  std::cout << "Root "<<i<<" = "<<roots[i]<<". ("<<a<<"x^3 + "<<b<<"x^2 + "<<c<<"x + "<<d<<" = "<<result<<")\n";
238  }
239  std::cout << "\n\n";
240 #endif
241 }
242 
243 
244 template<typename real>
245 inline void
246 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 template<typename real>
398  std::vector<Eigen::Matrix<real, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<real, 3, 1> > >& samplePoints, unsigned int polynomial_degree, bool& error) const
399 {
401  error = bivariatePolynomialApproximation (samplePoints, polynomial_degree, ret);
402  return ret;
403 }
404 
405 
406 template<typename real>
407 inline bool
409  std::vector<Eigen::Matrix<real, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<real, 3, 1> > >& samplePoints, unsigned int polynomial_degree,
411 {
412  const auto parameters_size = BivariatePolynomialT<real>::getNoOfParametersFromDegree (polynomial_degree);
413 
414  // Too many parameters for this number of equations (points)?
415  if (parameters_size > samplePoints.size ())
416  {
417  return false;
418  // Reduce degree of polynomial
419  //polynomial_degree = (unsigned int) (0.5f* (std::sqrt (8*samplePoints.size ()+1) - 3));
420  //parameters_size = BivariatePolynomialT<real>::getNoOfParametersFromDegree (polynomial_degree);
421  //std::cout << "Not enough points, so degree of polynomial was decreased to "<<polynomial_degree
422  // << " ("<<samplePoints.size ()<<" points => "<<parameters_size<<" parameters)\n";
423  }
424 
425  ret.setDegree (polynomial_degree);
426 
427  // A is a symmetric matrix
428  Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> A (parameters_size, parameters_size);
429  A.setZero();
430  Eigen::Matrix<real, Eigen::Dynamic, 1> b (parameters_size);
431  b.setZero();
432 
433  {
434  std::vector<real> C (parameters_size);
435  for (const auto& point: samplePoints)
436  {
437  real currentX = point[0], currentY = point[1], currentZ = point[2];
438 
439  {
440  auto CRevPtr = C.rbegin ();
441  real tmpX = 1.0;
442  for (unsigned int xDegree=0; xDegree<=polynomial_degree; ++xDegree)
443  {
444  real tmpY = 1.0;
445  for (unsigned int yDegree=0; yDegree<=polynomial_degree-xDegree; ++yDegree, ++CRevPtr)
446  {
447  *CRevPtr = tmpX*tmpY;
448  tmpY *= currentY;
449  }
450  tmpX *= currentX;
451  }
452  }
453 
454  for (std::size_t i=0; i<parameters_size; ++i)
455  {
456  b[i] += currentZ * C[i];
457  // fill the upper right triangular matrix
458  for (std::size_t j=i; j<parameters_size; ++j)
459  {
460  A (i, j) += C[i] * C[j];
461  }
462  }
463  //A += DMatrix<real>::outProd (C);
464  //b += currentZ * C;
465  }
466  }
467 
468  // The Eigen only solution is slow for small matrices. Maybe file a bug
469  // A.traingularView<Eigen::StrictlyLower> = A.transpose();
470  // copy upper-right elements to lower-left
471  for (std::size_t i = 0; i < parameters_size; ++i)
472  {
473  for (std::size_t j = 0; j < i; ++j)
474  {
475  A (i, j) = A (j, i);
476  }
477  }
478 
479  Eigen::Matrix<real, Eigen::Dynamic, 1> parameters;
480  //double choleskyStartTime=-get_time ();
481  //parameters = A.choleskySolve (b);
482  //std::cout << "Cholesky took "<< (choleskyStartTime+get_time ())*1000<<"ms.\n";
483 
484  //double invStartTime=-get_time ();
485  parameters = A.inverse () * b;
486  //std::cout << "Inverse took "<< (invStartTime+get_time ())*1000<<"ms.\n";
487 
488  //std::cout << PVARC (A)<<PVARC (b)<<PVARN (parameters);
489 
490  real inversionCheckResult = (A*parameters - b).norm ();
491  if (inversionCheckResult > 1e-5)
492  {
493  //std::cout << "Inversion result: "<< inversionCheckResult<<" for matrix "<<A<<"\n";
494  return false;
495  }
496 
497  std::copy_n(parameters.data(), parameters_size, ret.parameters);
498 
499  //std::cout << "Resulting polynomial is "<<ret<<"\n";
500 
501  //Test of gradient: ret.calculateGradient ();
502 
503  return true;
504 }
505 
506 } // namespace pcl
507 
This represents a bivariate polynomial and provides some functionality for it.
void setDegree(int new_degree)
Initialize members to default values.
static unsigned int getNoOfParametersFromDegree(int n)
How many parameters has a bivariate polynomial of the given degree.
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 https://en.wikipedia.org/wiki/Cubic_equat...
bool isNearlyZero(real d) const
check if std::abs(d)<zeroValue
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.
bool sqrtIsNearlyZero(real d) const
check if sqrt(std::abs(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 https://en.wikipedia....
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 https://en.wikipedia.org/wiki/Quadratic_equation...
void solveLinearEquation(real a, real b, std::vector< real > &roots) const
Solves an equation of the form ax + b = 0.
#define M_PI
Definition: pcl_macros.h:201
void setZeroValue(real new_zero_value)
Set zero_value.
real zero_value
Every value below this is considered to be zero.