Point Cloud Library (PCL)
1.3.1
|
00001 template<typename real> 00002 pcl::BivariatePolynomialT<real>::BivariatePolynomialT (int new_degree) : 00003 degree(0), parameters(NULL), gradient_x(NULL), gradient_y(NULL) 00004 { 00005 setDegree(new_degree); 00006 } 00007 00008 template<typename real> 00009 pcl::BivariatePolynomialT<real>::BivariatePolynomialT (const BivariatePolynomialT& other) : 00010 degree(0), parameters(NULL), gradient_x(NULL), gradient_y(NULL) 00011 { 00012 deepCopy (other); 00013 } 00014 00015 template<typename real> 00016 pcl::BivariatePolynomialT<real>::~BivariatePolynomialT () 00017 { 00018 memoryCleanUp (); 00019 } 00020 00021 template<typename real> 00022 void 00023 pcl::BivariatePolynomialT<real>::setDegree (int newDegree) 00024 { 00025 if (newDegree <= 0) 00026 { 00027 degree = -1; 00028 memoryCleanUp(); 00029 return; 00030 } 00031 int oldDegree = degree; 00032 degree = newDegree; 00033 if (oldDegree != degree) 00034 { 00035 delete[] parameters; 00036 parameters = new real[getNoOfParameters ()]; 00037 } 00038 delete gradient_x; gradient_x = NULL; 00039 delete gradient_y; gradient_y = NULL; 00040 } 00041 00042 template<typename real> 00043 void 00044 pcl::BivariatePolynomialT<real>::memoryCleanUp () 00045 { 00046 delete[] parameters; parameters = NULL; 00047 delete gradient_x; gradient_x = NULL; 00048 delete gradient_y; gradient_y = NULL; 00049 } 00050 00051 template<typename real> 00052 void 00053 pcl::BivariatePolynomialT<real>::deepCopy (const pcl::BivariatePolynomialT<real>& other) 00054 { 00055 if (this == &other) return; 00056 if (degree != other.degree) { 00057 memoryCleanUp (); 00058 degree = other.degree; 00059 parameters = new real[getNoOfParameters ()]; 00060 } 00061 if (other.gradient_x == NULL) { 00062 delete gradient_x; gradient_x=NULL; 00063 delete gradient_y; gradient_y=NULL; 00064 } 00065 else if (gradient_x==NULL) { 00066 gradient_x = new pcl::BivariatePolynomialT<real> (); 00067 gradient_y = new pcl::BivariatePolynomialT<real> (); 00068 } 00069 real* tmpParameters1 = parameters; 00070 const real* tmpParameters2 = other.parameters; 00071 unsigned int noOfParameters = getNoOfParameters (); 00072 for (unsigned int i=0; i<noOfParameters; i++) 00073 *tmpParameters1++ = *tmpParameters2++; 00074 00075 if (other.gradient_x != NULL) { 00076 gradient_x->deepCopy (*other.gradient_x); 00077 gradient_y->deepCopy (*other.gradient_y); 00078 } 00079 } 00080 00081 template<typename real> 00082 void 00083 pcl::BivariatePolynomialT<real>::calculateGradient (bool forceRecalc) 00084 { 00085 if (gradient_x!=NULL && !forceRecalc) { 00086 //cout << "Gradient already exists\n"; 00087 return; 00088 } 00089 00090 if (gradient_x == NULL) 00091 gradient_x = new pcl::BivariatePolynomialT<real> (degree-1); 00092 if (gradient_y == NULL) 00093 gradient_y = new pcl::BivariatePolynomialT<real> (degree-1); 00094 00095 unsigned int parameterPosDx=0, parameterPosDy=0; 00096 for (int xDegree=degree; xDegree>=0; xDegree--) { 00097 for (int yDegree=degree-xDegree; yDegree>=0; yDegree--) { 00098 if (xDegree > 0) { 00099 gradient_x->parameters[parameterPosDx] = xDegree * parameters[parameterPosDx]; 00100 parameterPosDx++; 00101 } 00102 if (yDegree > 0) { 00103 gradient_y->parameters[parameterPosDy] = yDegree * parameters[ ( (degree+2-xDegree)* (degree+1-xDegree))/2 - 00104 yDegree - 1]; 00105 parameterPosDy++; 00106 } 00107 } 00108 } 00109 //cout << "Original polynom: "<<*this<<"\n"<<"d/dx: "<<*gradient_x<<"\n"<<"d/dy: "<<*gradient_y<<"\n"; 00110 } 00111 00112 template<typename real> 00113 real 00114 pcl::BivariatePolynomialT<real>::getValue (real x, real y) const 00115 { 00116 unsigned int parametersSize = getNoOfParameters (); 00117 real* tmpParameter = ¶meters[parametersSize-1]; 00118 real tmpX=1.0, tmpY, ret=0; 00119 for (int xDegree=0; xDegree<=degree; xDegree++) { 00120 tmpY = 1.0; 00121 for (int yDegree=0; yDegree<=degree-xDegree; yDegree++) 00122 { 00123 ret += (*tmpParameter)*tmpX*tmpY; 00124 tmpY *= y; 00125 tmpParameter--; 00126 } 00127 tmpX *= x; 00128 } 00129 return ret; 00130 } 00131 00132 template<typename real> 00133 void 00134 pcl::BivariatePolynomialT<real>::getValueOfGradient (real x, real y, real& gradX, real& gradY) 00135 { 00136 calculateGradient (); 00137 gradX = gradient_x->getValue (x, y); 00138 gradY = gradient_y->getValue (x, y); 00139 //cout << "Gradient at "<<x<<", "<<y<<" is "<<gradX<<", "<<gradY<<"\n"; 00140 } 00141 00142 //elliptischer Punkt hyperbolischer Punkt parabolischer Punkt 00143 //Bei Funktionen in zwei Ver¨anderlichen kann der Typ anhand der Determinante der Hesse- 00144 //Matrix klassifiziert werden. Ist det(Hf) > 0 (< 0), so handelt es sich um ein Extremum (einen 00145 //Sattelpunkt). F¨ur ein Minimum bzw. ein Maximum ist Spur(Hf) > 0 bzw. < 0 . Verschwindet 00146 //die Determinante und ist die Hesse-Matrix nicht Null, so ist der Punkt parabolisch. 00147 00148 template<typename real> 00149 void 00150 pcl::BivariatePolynomialT<real>::findCriticalPoints (std::vector<real>& x_values, std::vector<real>& y_values, 00151 std::vector<int>& types) const 00152 { 00153 x_values.clear (); 00154 y_values.clear (); 00155 types.clear (); 00156 00157 if (degree == 2) 00158 { 00159 real x = (real(2)*parameters[2]*parameters[3] - parameters[1]*parameters[4]) / 00160 (parameters[1]*parameters[1] - real(4)*parameters[0]*parameters[3]), 00161 y = (real(-2)*parameters[0]*x - parameters[2]) / parameters[1]; 00162 00163 if (!pcl_isfinite(x) || !pcl_isfinite(y)) 00164 return; 00165 00166 int type = 2; 00167 real det_H = real(4)*parameters[0]*parameters[3] - parameters[1]*parameters[1]; 00168 //std::cout << "det(H) = "<<det_H<<"\n"; 00169 if (det_H > real(0)) // Check Hessian determinant 00170 { 00171 if (parameters[0]+parameters[3] < real(0)) // Check Hessian trace 00172 type = 0; 00173 else 00174 type = 1; 00175 } 00176 x_values.push_back(x); 00177 y_values.push_back(y); 00178 types.push_back(type); 00179 00180 //real gx, gy; 00181 //((BivariatePolynomialT<real>*)this)->getValueOfGradient (x, y, gx, gy); 00182 //std::cout << "Check: p'("<<x<<", "<<y<<") = ("<<gx<<", "<<gy<<")\n"; 00183 } 00184 else 00185 { 00186 std::cerr << __PRETTY_FUNCTION__ << " is not implemented for polynomials of degree "<<degree<<". Sorry.\n"; 00187 } 00188 } 00189 00190 template<typename real> 00191 std::ostream& 00192 pcl::operator<< (std::ostream& os, const pcl::BivariatePolynomialT<real>& p) 00193 { 00194 real* tmpParameter = p.parameters; 00195 bool first = true; 00196 real currentParameter; 00197 for (int xDegree=p.degree; xDegree>=0; xDegree--) { 00198 for (int yDegree=p.degree-xDegree; yDegree>=0; yDegree--) { 00199 currentParameter = *tmpParameter; 00200 if (!first) { 00201 os << (currentParameter<0.0?" - ":" + "); 00202 currentParameter = fabs (currentParameter); 00203 } 00204 os << currentParameter; 00205 if (xDegree>0) { 00206 os << "x"; 00207 if (xDegree>1) 00208 os<<"^"<<xDegree; 00209 } 00210 if (yDegree>0) { 00211 os << "y"; 00212 if (yDegree>1) 00213 os<<"^"<<yDegree; 00214 } 00215 00216 first = false; 00217 tmpParameter++; 00218 } 00219 } 00220 return os; 00221 } 00222 00223 template<typename real> 00224 void 00225 pcl::BivariatePolynomialT<real>::writeBinary (std::ostream& os) const 00226 { 00227 os.write ( (char*)°ree, sizeof (int)); 00228 unsigned int paramCnt = getNoOfParametersFromDegree (this->degree); 00229 os.write ( (char*) (this->parameters), paramCnt * sizeof (real)); 00230 } 00231 00232 template<typename real> 00233 void 00234 pcl::BivariatePolynomialT<real>::writeBinary (const char* filename) const 00235 { 00236 std::ofstream fout (filename); 00237 writeBinary (fout); 00238 } 00239 00240 template<typename real> 00241 void 00242 pcl::BivariatePolynomialT<real>::readBinary (std::istream& os) 00243 { 00244 memoryCleanUp (); 00245 os.read ( (char*)& (this->degree), sizeof (int)); 00246 unsigned int paramCnt = getNoOfParametersFromDegree (this->degree); 00247 parameters = new real[paramCnt]; 00248 os.read ( (char*)& (*this->parameters), paramCnt * sizeof (real)); 00249 } 00250 00251 template<typename real> 00252 void 00253 pcl::BivariatePolynomialT<real>::readBinary (const char* filename) 00254 { 00255 std::ifstream fin (filename); 00256 readBinary (fin); 00257 } 00258 00259 00260