Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2010-2012, Willow Garage, Inc. 00006 * 00007 * All rights reserved. 00008 * 00009 * Redistribution and use in source and binary forms, with or without 00010 * modification, are permitted provided that the following conditions 00011 * are met: 00012 * 00013 * * Redistributions of source code must retain the above copyright 00014 * notice, this list of conditions and the following disclaimer. 00015 * * Redistributions in binary form must reproduce the above 00016 * copyright notice, this list of conditions and the following 00017 * disclaimer in the documentation and/or other materials provided 00018 * with the distribution. 00019 * * Neither the name of the copyright holder(s) nor the names of its 00020 * contributors may be used to endorse or promote products derived 00021 * from this software without specific prior written permission. 00022 * 00023 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00024 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00025 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00026 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00027 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00028 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00029 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00030 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00031 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00032 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00033 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00034 * POSSIBILITY OF SUCH DAMAGE. 00035 * 00036 * $Id$ 00037 * 00038 */ 00039 #ifndef BIVARIATE_POLYNOMIAL_HPP 00040 #define BIVARIATE_POLYNOMIAL_HPP 00041 00042 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00043 template<typename real> 00044 pcl::BivariatePolynomialT<real>::BivariatePolynomialT (int new_degree) : 00045 degree(0), parameters(NULL), gradient_x(NULL), gradient_y(NULL) 00046 { 00047 setDegree(new_degree); 00048 } 00049 00050 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00051 template<typename real> 00052 pcl::BivariatePolynomialT<real>::BivariatePolynomialT (const BivariatePolynomialT& other) : 00053 degree(0), parameters(NULL), gradient_x(NULL), gradient_y(NULL) 00054 { 00055 deepCopy (other); 00056 } 00057 00058 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00059 template<typename real> 00060 pcl::BivariatePolynomialT<real>::~BivariatePolynomialT () 00061 { 00062 memoryCleanUp (); 00063 } 00064 00065 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00066 template<typename real> void 00067 pcl::BivariatePolynomialT<real>::setDegree (int newDegree) 00068 { 00069 if (newDegree <= 0) 00070 { 00071 degree = -1; 00072 memoryCleanUp(); 00073 return; 00074 } 00075 int oldDegree = degree; 00076 degree = newDegree; 00077 if (oldDegree != degree) 00078 { 00079 delete[] parameters; 00080 parameters = new real[getNoOfParameters ()]; 00081 } 00082 delete gradient_x; gradient_x = NULL; 00083 delete gradient_y; gradient_y = NULL; 00084 } 00085 00086 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00087 template<typename real> void 00088 pcl::BivariatePolynomialT<real>::memoryCleanUp () 00089 { 00090 delete[] parameters; parameters = NULL; 00091 delete gradient_x; gradient_x = NULL; 00092 delete gradient_y; gradient_y = NULL; 00093 } 00094 00095 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00096 template<typename real> void 00097 pcl::BivariatePolynomialT<real>::deepCopy (const pcl::BivariatePolynomialT<real>& other) 00098 { 00099 if (this == &other) return; 00100 if (degree != other.degree) 00101 { 00102 memoryCleanUp (); 00103 degree = other.degree; 00104 parameters = new real[getNoOfParameters ()]; 00105 } 00106 if (other.gradient_x == NULL) 00107 { 00108 delete gradient_x; gradient_x=NULL; 00109 delete gradient_y; gradient_y=NULL; 00110 } 00111 else if (gradient_x==NULL) 00112 { 00113 gradient_x = new pcl::BivariatePolynomialT<real> (); 00114 gradient_y = new pcl::BivariatePolynomialT<real> (); 00115 } 00116 real* tmpParameters1 = parameters; 00117 const real* tmpParameters2 = other.parameters; 00118 unsigned int noOfParameters = getNoOfParameters (); 00119 for (unsigned int i=0; i<noOfParameters; i++) 00120 *tmpParameters1++ = *tmpParameters2++; 00121 00122 if (other.gradient_x != NULL) 00123 { 00124 gradient_x->deepCopy (*other.gradient_x); 00125 gradient_y->deepCopy (*other.gradient_y); 00126 } 00127 } 00128 00129 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00130 template<typename real> void 00131 pcl::BivariatePolynomialT<real>::calculateGradient (bool forceRecalc) 00132 { 00133 if (gradient_x!=NULL && !forceRecalc) 00134 00135 if (gradient_x == NULL) 00136 gradient_x = new pcl::BivariatePolynomialT<real> (degree-1); 00137 if (gradient_y == NULL) 00138 gradient_y = new pcl::BivariatePolynomialT<real> (degree-1); 00139 00140 unsigned int parameterPosDx=0, parameterPosDy=0; 00141 for (int xDegree=degree; xDegree>=0; xDegree--) 00142 { 00143 for (int yDegree=degree-xDegree; yDegree>=0; yDegree--) 00144 { 00145 if (xDegree > 0) 00146 { 00147 gradient_x->parameters[parameterPosDx] = xDegree * parameters[parameterPosDx]; 00148 parameterPosDx++; 00149 } 00150 if (yDegree > 0) 00151 { 00152 gradient_y->parameters[parameterPosDy] = yDegree * parameters[ ( (degree+2-xDegree)* (degree+1-xDegree))/2 - 00153 yDegree - 1]; 00154 parameterPosDy++; 00155 } 00156 } 00157 } 00158 } 00159 00160 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00161 template<typename real> real 00162 pcl::BivariatePolynomialT<real>::getValue (real x, real y) const 00163 { 00164 unsigned int parametersSize = getNoOfParameters (); 00165 real* tmpParameter = ¶meters[parametersSize-1]; 00166 real tmpX=1.0, tmpY, ret=0; 00167 for (int xDegree=0; xDegree<=degree; xDegree++) 00168 { 00169 tmpY = 1.0; 00170 for (int yDegree=0; yDegree<=degree-xDegree; yDegree++) 00171 { 00172 ret += (*tmpParameter)*tmpX*tmpY; 00173 tmpY *= y; 00174 tmpParameter--; 00175 } 00176 tmpX *= x; 00177 } 00178 return ret; 00179 } 00180 00181 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00182 template<typename real> void 00183 pcl::BivariatePolynomialT<real>::getValueOfGradient (real x, real y, real& gradX, real& gradY) 00184 { 00185 calculateGradient (); 00186 gradX = gradient_x->getValue (x, y); 00187 gradY = gradient_y->getValue (x, y); 00188 } 00189 00190 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00191 template<typename real> void 00192 pcl::BivariatePolynomialT<real>::findCriticalPoints (std::vector<real>& x_values, std::vector<real>& y_values, 00193 std::vector<int>& types) const 00194 { 00195 x_values.clear (); 00196 y_values.clear (); 00197 types.clear (); 00198 00199 if (degree == 2) 00200 { 00201 real x = (real(2)*parameters[2]*parameters[3] - parameters[1]*parameters[4]) / 00202 (parameters[1]*parameters[1] - real(4)*parameters[0]*parameters[3]), 00203 y = (real(-2)*parameters[0]*x - parameters[2]) / parameters[1]; 00204 00205 if (!pcl_isfinite(x) || !pcl_isfinite(y)) 00206 return; 00207 00208 int type = 2; 00209 real det_H = real(4)*parameters[0]*parameters[3] - parameters[1]*parameters[1]; 00210 //std::cout << "det(H) = "<<det_H<<"\n"; 00211 if (det_H > real(0)) // Check Hessian determinant 00212 { 00213 if (parameters[0]+parameters[3] < real(0)) // Check Hessian trace 00214 type = 0; 00215 else 00216 type = 1; 00217 } 00218 x_values.push_back(x); 00219 y_values.push_back(y); 00220 types.push_back(type); 00221 } 00222 else 00223 { 00224 std::cerr << __PRETTY_FUNCTION__ << " is not implemented for polynomials of degree "<<degree<<". Sorry.\n"; 00225 } 00226 } 00227 00228 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00229 template<typename real> std::ostream& 00230 pcl::operator<< (std::ostream& os, const pcl::BivariatePolynomialT<real>& p) 00231 { 00232 real* tmpParameter = p.parameters; 00233 bool first = true; 00234 real currentParameter; 00235 for (int xDegree=p.degree; xDegree>=0; xDegree--) 00236 { 00237 for (int yDegree=p.degree-xDegree; yDegree>=0; yDegree--) 00238 { 00239 currentParameter = *tmpParameter; 00240 if (!first) 00241 { 00242 os << (currentParameter<0.0?" - ":" + "); 00243 currentParameter = fabs (currentParameter); 00244 } 00245 os << currentParameter; 00246 if (xDegree>0) 00247 { 00248 os << "x"; 00249 if (xDegree>1) 00250 os<<"^"<<xDegree; 00251 } 00252 if (yDegree>0) 00253 { 00254 os << "y"; 00255 if (yDegree>1) 00256 os<<"^"<<yDegree; 00257 } 00258 00259 first = false; 00260 tmpParameter++; 00261 } 00262 } 00263 return (os); 00264 } 00265 00266 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00267 template<typename real> void 00268 pcl::BivariatePolynomialT<real>::writeBinary (std::ostream& os) const 00269 { 00270 os.write (reinterpret_cast<char*> (°ree), sizeof (int)); 00271 unsigned int paramCnt = getNoOfParametersFromDegree (this->degree); 00272 os.write (reinterpret_cast<char*> (this->parameters), paramCnt * sizeof (real)); 00273 } 00274 00275 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00276 template<typename real> void 00277 pcl::BivariatePolynomialT<real>::writeBinary (const char* filename) const 00278 { 00279 std::ofstream fout (filename); 00280 writeBinary (fout); 00281 } 00282 00283 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00284 template<typename real> void 00285 pcl::BivariatePolynomialT<real>::readBinary (std::istream& os) 00286 { 00287 memoryCleanUp (); 00288 os.read (reinterpret_cast<char*> (&this->degree), sizeof (int)); 00289 unsigned int paramCnt = getNoOfParametersFromDegree (this->degree); 00290 parameters = new real[paramCnt]; 00291 os.read (reinterpret_cast<char*> (&(*this->parameters)), paramCnt * sizeof (real)); 00292 } 00293 00294 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00295 template<typename real> void 00296 pcl::BivariatePolynomialT<real>::readBinary (const char* filename) 00297 { 00298 std::ifstream fin (filename); 00299 readBinary (fin); 00300 } 00301 00302 #endif 00303