Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/common/include/pcl/common/polynomial_calculations.h
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