Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2010, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the copyright holder(s) nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 * 00034 */ 00035 00036 #ifndef PCL_POLYNOMIAL_CALCULATIONS_H_ 00037 #define PCL_POLYNOMIAL_CALCULATIONS_H_ 00038 00039 #include <pcl/common/eigen.h> 00040 #include <pcl/common/bivariate_polynomial.h> 00041 00042 namespace pcl 00043 { 00044 /** \brief This provides some functionality for polynomials, 00045 * like finding roots or approximating bivariate polynomials 00046 * \author Bastian Steder 00047 * \ingroup common 00048 */ 00049 template <typename real> 00050 class PolynomialCalculationsT 00051 { 00052 public: 00053 // =====CONSTRUCTOR & DESTRUCTOR===== 00054 PolynomialCalculationsT (); 00055 ~PolynomialCalculationsT (); 00056 00057 // =====PUBLIC STRUCTS===== 00058 //! Parameters used in this class 00059 struct Parameters 00060 { 00061 Parameters () : zero_value (), sqr_zero_value () { setZeroValue (1e-6);} 00062 //! Set zero_value 00063 void 00064 setZeroValue (real new_zero_value); 00065 00066 real zero_value; //!< Every value below this is considered to be zero 00067 real sqr_zero_value; //!< sqr of the above 00068 }; 00069 00070 // =====PUBLIC METHODS===== 00071 /** Solves an equation of the form ax^4 + bx^3 + cx^2 +dx + e = 0 00072 * See http://en.wikipedia.org/wiki/Quartic_equation#Summary_of_Ferrari.27s_method */ 00073 inline void 00074 solveQuarticEquation (real a, real b, real c, real d, real e, std::vector<real>& roots) const; 00075 00076 /** Solves an equation of the form ax^3 + bx^2 + cx + d = 0 00077 * See http://en.wikipedia.org/wiki/Cubic_equation */ 00078 inline void 00079 solveCubicEquation (real a, real b, real c, real d, std::vector<real>& roots) const; 00080 00081 /** Solves an equation of the form ax^2 + bx + c = 0 00082 * See http://en.wikipedia.org/wiki/Quadratic_equation */ 00083 inline void 00084 solveQuadraticEquation (real a, real b, real c, std::vector<real>& roots) const; 00085 00086 /** Solves an equation of the form ax + b = 0 */ 00087 inline void 00088 solveLinearEquation (real a, real b, std::vector<real>& roots) const; 00089 00090 /** Get the bivariate polynomial approximation for Z(X,Y) from the given sample points. 00091 * The parameters a,b,c,... for the polynom are returned. 00092 * The order is, e.g., for degree 1: ax+by+c and for degree 2: ax²+bxy+cx+dy²+ey+f. 00093 * error is set to true if the approximation did not work for any reason 00094 * (not enough points, matrix not invertible, etc.) */ 00095 inline BivariatePolynomialT<real> 00096 bivariatePolynomialApproximation (std::vector<Eigen::Matrix<real, 3, 1> >& samplePoints, 00097 unsigned int polynomial_degree, bool& error) const; 00098 00099 //! Same as above, using a reference for the return value 00100 inline bool 00101 bivariatePolynomialApproximation (std::vector<Eigen::Matrix<real, 3, 1> >& samplePoints, 00102 unsigned int polynomial_degree, BivariatePolynomialT<real>& ret) const; 00103 00104 //! Set the minimum value under which values are considered zero 00105 inline void 00106 setZeroValue (real new_zero_value) { parameters_.setZeroValue(new_zero_value); } 00107 00108 protected: 00109 // =====PROTECTED METHODS===== 00110 //! check if fabs(d)<zeroValue 00111 inline bool 00112 isNearlyZero (real d) const 00113 { 00114 return (fabs (d) < parameters_.zero_value); 00115 } 00116 00117 //! check if sqrt(fabs(d))<zeroValue 00118 inline bool 00119 sqrtIsNearlyZero (real d) const 00120 { 00121 return (fabs (d) < parameters_.sqr_zero_value); 00122 } 00123 00124 // =====PROTECTED MEMBERS===== 00125 Parameters parameters_; 00126 }; 00127 00128 typedef PolynomialCalculationsT<double> PolynomialCalculationsd; 00129 typedef PolynomialCalculationsT<float> PolynomialCalculations; 00130 00131 } // end namespace 00132 00133 #include <pcl/common/impl/polynomial_calculations.hpp> 00134 00135 #endif