Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
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
44namespace pcl
45{
46
47template <typename real>
48inline void
54
55
56template <typename real>
57inline void
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
88template <typename real>
89inline void
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";
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;
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
143template<typename real>
144inline void
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";
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;
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),
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
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 {
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
244template<typename real>
245inline void
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),
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;
291 for (const auto& quadraticRoot: tmpRoots)
292 {
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
395template<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{
402 return ret;
403}
404
405
406template<typename real>
407inline bool
409 std::vector<Eigen::Matrix<real, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<real, 3, 1> > >& samplePoints, unsigned int polynomial_degree,
411{
413
414 // Too many parameters for this number of equations (points)?
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
static unsigned int getNoOfParametersFromDegree(int n)
How many parameters has a bivariate polynomial of the given degree.
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
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...
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 http://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 http://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.