Point Cloud Library (PCL)  1.14.1-dev
polynomial_calculations.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2010, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the copyright holder(s) nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *
34  */
35 
36 #pragma once
37 
38 #include <pcl/common/eigen.h>
39 #include <pcl/common/bivariate_polynomial.h>
40 
41 namespace pcl
42 {
43  /** \brief This provides some functionality for polynomials,
44  * like finding roots or approximating bivariate polynomials
45  * \author Bastian Steder
46  * \ingroup common
47  */
48  template <typename real>
50  {
51  public:
52  // =====PUBLIC STRUCTS=====
53  //! Parameters used in this class
54  struct Parameters
55  {
56  Parameters () { setZeroValue (1e-6);}
57  //! Set zero_value
58  void
59  setZeroValue (real new_zero_value);
60 
61  real zero_value = {}; //!< Every value below this is considered to be zero
62  real sqr_zero_value = {}; //!< sqr of the above
63  };
64 
65  // =====PUBLIC METHODS=====
66  /** Solves an equation of the form ax^4 + bx^3 + cx^2 +dx + e = 0
67  * See https://en.wikipedia.org/wiki/Quartic_equation#Summary_of_Ferrari.27s_method */
68  inline void
69  solveQuarticEquation (real a, real b, real c, real d, real e, std::vector<real>& roots) const;
70 
71  /** Solves an equation of the form ax^3 + bx^2 + cx + d = 0
72  * See https://en.wikipedia.org/wiki/Cubic_equation */
73  inline void
74  solveCubicEquation (real a, real b, real c, real d, std::vector<real>& roots) const;
75 
76  /** Solves an equation of the form ax^2 + bx + c = 0
77  * See https://en.wikipedia.org/wiki/Quadratic_equation */
78  inline void
79  solveQuadraticEquation (real a, real b, real c, std::vector<real>& roots) const;
80 
81  /** Solves an equation of the form ax + b = 0 */
82  inline void
83  solveLinearEquation (real a, real b, std::vector<real>& roots) const;
84 
85  /** Get the bivariate polynomial approximation for Z(X,Y) from the given sample points.
86  * The parameters a,b,c,... for the polynom are returned.
87  * The order is, e.g., for degree 1: ax+by+c and for degree 2: ax²+bxy+cx+dy²+ey+f.
88  * error is set to true if the approximation did not work for any reason
89  * (not enough points, matrix not invertible, etc.) */
91  bivariatePolynomialApproximation (std::vector<Eigen::Matrix<real, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<real, 3, 1> > >& samplePoints,
92  unsigned int polynomial_degree, bool& error) const;
93 
94  //! Same as above, using a reference for the return value
95  inline bool
96  bivariatePolynomialApproximation (std::vector<Eigen::Matrix<real, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<real, 3, 1> > >& samplePoints,
97  unsigned int polynomial_degree, BivariatePolynomialT<real>& ret) const;
98 
99  //! Set the minimum value under which values are considered zero
100  inline void
101  setZeroValue (real new_zero_value) { parameters_.setZeroValue(new_zero_value); }
102 
103  protected:
104  // =====PROTECTED METHODS=====
105  //! check if std::abs(d)<zeroValue
106  inline bool
107  isNearlyZero (real d) const
108  {
109  return (std::abs (d) < parameters_.zero_value);
110  }
111 
112  //! check if sqrt(std::abs(d))<zeroValue
113  inline bool
114  sqrtIsNearlyZero (real d) const
115  {
116  return (std::abs (d) < parameters_.sqr_zero_value);
117  }
118 
119  // =====PROTECTED MEMBERS=====
121  };
122 
125 
126 } // end namespace
127 
128 #include <pcl/common/impl/polynomial_calculations.hpp>
This represents a bivariate polynomial and provides some functionality for it.
This provides some functionality for polynomials, like finding roots or approximating bivariate polyn...
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...
void setZeroValue(real new_zero_value)
Set the minimum value under which values are considered zero.
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.
void setZeroValue(real new_zero_value)
Set zero_value.
real zero_value
Every value below this is considered to be zero.