Point Cloud Library (PCL)
1.3.1
|
00001 00002 00003 template <typename real> 00004 pcl::PolynomialCalculationsT<real>::PolynomialCalculationsT () 00005 { 00006 } 00007 00009 00010 template <typename real> 00011 pcl::PolynomialCalculationsT<real>:: ~PolynomialCalculationsT () 00012 { 00013 } 00014 00016 00017 template <typename real> 00018 inline void 00019 pcl::PolynomialCalculationsT<real>::Parameters::setZeroValue (real new_zero_value) 00020 { 00021 zero_value = new_zero_value; 00022 sqr_zero_value = zero_value*zero_value; 00023 } 00024 00026 00027 template <typename real> 00028 inline void 00029 pcl::PolynomialCalculationsT<real>::solveLinearEquation (real a, real b, std::vector<real>& roots) const 00030 { 00031 //cout << "Trying to solve "<<a<<"x + "<<b<<" = 0\n"; 00032 00033 if (isNearlyZero (b)) 00034 { 00035 roots.push_back (0.0); 00036 } 00037 if (!isNearlyZero (a/b)) 00038 { 00039 roots.push_back (-b/a); 00040 } 00041 00042 #if 0 00043 cout << __PRETTY_FUNCTION__ << ": Found "<<roots.size ()<<" roots.\n"; 00044 for (unsigned int i=0; i<roots.size (); i++) 00045 { 00046 real x=roots[i]; 00047 real result = a*x + b; 00048 if (!isNearlyZero (result)) 00049 { 00050 cout << "Something went wrong during solving of polynomial "<<a<<"x + "<<b<<" = 0\n"; 00051 //roots.clear (); 00052 } 00053 cout << "Root "<<i<<" = "<<roots[i]<<". ("<<a<<"x^ + "<<b<<" = "<<result<<")\n"; 00054 } 00055 #endif 00056 } 00057 00059 00060 template <typename real> 00061 inline void 00062 pcl::PolynomialCalculationsT<real>::solveQuadraticEquation (real a, real b, real c, std::vector<real>& roots) const 00063 { 00064 //cout << "Trying to solve "<<a<<"x^2 + "<<b<<"x + "<<c<<" = 0\n"; 00065 00066 if (isNearlyZero (a)) 00067 { 00068 //cout << "Highest order element is 0 => Calling solveLineaqrEquation.\n"; 00069 solveLinearEquation (b, c, roots); 00070 return; 00071 } 00072 00073 if (isNearlyZero (c)) 00074 { 00075 roots.push_back (0.0); 00076 //cout << "Constant element is 0 => Adding root 0 and calling solveLinearEquation.\n"; 00077 std::vector<real> tmpRoots; 00078 solveLinearEquation (a, b, tmpRoots); 00079 for (unsigned int i=0; i<tmpRoots.size (); i++) 00080 if (!isNearlyZero (tmpRoots[i])) 00081 roots.push_back (tmpRoots[i]); 00082 return; 00083 } 00084 00085 real tmp = b*b - 4*a*c; 00086 if (tmp>0) 00087 { 00088 tmp = sqrt (tmp); 00089 real tmp2 = 1.0/ (2*a); 00090 roots.push_back ( (-b + tmp)*tmp2); 00091 roots.push_back ( (-b - tmp)*tmp2); 00092 } 00093 else if (sqrtIsNearlyZero (tmp)) 00094 { 00095 roots.push_back (-b/ (2*a)); 00096 } 00097 00098 #if 0 00099 cout << __PRETTY_FUNCTION__ << ": Found "<<roots.size ()<<" roots.\n"; 00100 for (unsigned int i=0; i<roots.size (); i++) 00101 { 00102 real x=roots[i], x2=x*x; 00103 real result = a*x2 + b*x + c; 00104 if (!isNearlyZero (result)) 00105 { 00106 cout << "Something went wrong during solving of polynomial "<<a<<"x^2 + "<<b<<"x + "<<c<<" = 0\n"; 00107 //roots.clear (); 00108 } 00109 //cout << "Root "<<i<<" = "<<roots[i]<<". ("<<a<<"x^2 + "<<b<<"x + "<<c<<" = "<<result<<")\n"; 00110 } 00111 #endif 00112 } 00113 00115 00116 template<typename real> 00117 inline void 00118 pcl::PolynomialCalculationsT<real>::solveCubicEquation (real a, real b, real c, real d, std::vector<real>& roots) const 00119 { 00120 //cout << "Trying to solve "<<a<<"x^3 + "<<b<<"x^2 + "<<c<<"x + "<<d<<" = 0\n"; 00121 00122 if (isNearlyZero (a)) 00123 { 00124 //cout << "Highest order element is 0 => Calling solveQuadraticEquation.\n"; 00125 solveQuadraticEquation (b, c, d, roots); 00126 return; 00127 } 00128 00129 if (isNearlyZero (d)) 00130 { 00131 roots.push_back (0.0); 00132 //cout << "Constant element is 0 => Adding root 0 and calling solveQuadraticEquation.\n"; 00133 std::vector<real> tmpRoots; 00134 solveQuadraticEquation (a, b, c, tmpRoots); 00135 for (unsigned int i=0; i<tmpRoots.size (); i++) 00136 if (!isNearlyZero (tmpRoots[i])) 00137 roots.push_back (tmpRoots[i]); 00138 return; 00139 } 00140 00141 double a2 = a*a, 00142 a3 = a2*a, 00143 b2 = b*b, 00144 b3 = b2*b, 00145 alpha = ( (3.0*a*c-b2)/ (3.0*a2)), 00146 beta = (2*b3/ (27.0*a3)) - ( (b*c)/ (3.0*a2)) + (d/a), 00147 alpha2 = alpha*alpha, 00148 alpha3 = alpha2*alpha, 00149 beta2 = beta*beta; 00150 00151 // Value for resubstitution: 00152 double resubValue = b/ (3*a); 00153 00154 //cout << "Trying to solve y^3 + "<<alpha<<"y + "<<beta<<"\n"; 00155 00156 double discriminant = (alpha3/27.0) + 0.25*beta2; 00157 00158 //cout << "Discriminant is "<<discriminant<<"\n"; 00159 00160 if (isNearlyZero (discriminant)) 00161 { 00162 if (!isNearlyZero (alpha) || !isNearlyZero (beta)) 00163 { 00164 roots.push_back ( (-3.0*beta)/ (2.0*alpha) - resubValue); 00165 roots.push_back ( (3.0*beta)/alpha - resubValue); 00166 } 00167 else 00168 { 00169 roots.push_back (-resubValue); 00170 } 00171 } 00172 else if (discriminant > 0) 00173 { 00174 double sqrtDiscriminant = sqrt (discriminant); 00175 double d1 = -0.5*beta + sqrtDiscriminant, 00176 d2 = -0.5*beta - sqrtDiscriminant; 00177 if (d1 < 0) 00178 d1 = -pow (-d1, 1.0/3.0); 00179 else 00180 d1 = pow (d1, 1.0/3.0); 00181 00182 if (d2 < 0) 00183 d2 = -pow (-d2, 1.0/3.0); 00184 else 00185 d2 = pow (d2, 1.0/3.0); 00186 00187 //cout << PVAR (d1)<<", "<<PVAR (d2)<<"\n"; 00188 roots.push_back (d1 + d2 - resubValue); 00189 } 00190 else 00191 { 00192 double tmp1 = sqrt (- (4.0/3.0)*alpha), 00193 tmp2 = acos (-sqrt (-27.0/alpha3)*0.5*beta)/3.0; 00194 roots.push_back (tmp1*cos (tmp2) - resubValue); 00195 roots.push_back (-tmp1*cos (tmp2 + M_PI/3.0) - resubValue); 00196 roots.push_back (-tmp1*cos (tmp2 - M_PI/3.0) - resubValue); 00197 } 00198 00199 #if 0 00200 cout << __PRETTY_FUNCTION__ << ": Found "<<roots.size ()<<" roots.\n"; 00201 for (unsigned int i=0; i<roots.size (); i++) 00202 { 00203 real x=roots[i], x2=x*x, x3=x2*x; 00204 real result = a*x3 + b*x2 + c*x + d; 00205 if (fabs (result) > 1e-4) 00206 { 00207 cout << "Something went wrong:\n"; 00208 //roots.clear (); 00209 } 00210 cout << "Root "<<i<<" = "<<roots[i]<<". ("<<a<<"x^3 + "<<b<<"x^2 + "<<c<<"x + "<<d<<" = "<<result<<")\n"; 00211 } 00212 cout << "\n\n"; 00213 #endif 00214 } 00215 00217 00218 template<typename real> 00219 inline void 00220 pcl::PolynomialCalculationsT<real>::solveQuarticEquation (real a, real b, real c, real d, real e, 00221 std::vector<real>& roots) const 00222 { 00223 //cout << "Trying to solve "<<a<<"x^4 + "<<b<<"x^3 + "<<c<<"x^2 + "<<d<<"x + "<<e<<" = 0\n"; 00224 00225 if (isNearlyZero (a)) 00226 { 00227 //cout << "Highest order element is 0 => Calling solveCubicEquation.\n"; 00228 solveCubicEquation (b, c, d, e, roots); 00229 return; 00230 } 00231 00232 if (isNearlyZero (e)) 00233 { 00234 roots.push_back (0.0); 00235 //cout << "Constant element is 0 => Adding root 0 and calling solveCubicEquation.\n"; 00236 std::vector<real> tmpRoots; 00237 solveCubicEquation (a, b, c, d, tmpRoots); 00238 for (unsigned int i=0; i<tmpRoots.size (); i++) 00239 if (!isNearlyZero (tmpRoots[i])) 00240 roots.push_back (tmpRoots[i]); 00241 return; 00242 } 00243 00244 double root1, root2, root3, root4, 00245 a2 = a*a, 00246 a3 = a2*a, 00247 a4 = a2*a2, 00248 b2 = b*b, 00249 b3 = b2*b, 00250 b4 = b2*b2, 00251 alpha = ( (-3.0*b2)/ (8.0*a2)) + (c/a), 00252 beta = (b3/ (8.0*a3)) - ( (b*c)/ (2.0*a2)) + (d/a), 00253 gamma = ( (-3.0*b4)/ (256.0*a4)) + ( (c*b2)/ (16.0*a3)) - ( (b*d)/ (4.0*a2)) + (e/a), 00254 alpha2 = alpha*alpha; 00255 00256 // Value for resubstitution: 00257 double resubValue = b/ (4*a); 00258 00259 //cout << "Trying to solve y^4 + "<<alpha<<"y^2 + "<<beta<<"y + "<<gamma<<"\n"; 00260 00261 if (isNearlyZero (beta)) 00262 { // y^4 + alpha*y^2 + gamma\n"; 00263 //cout << "Using beta=0 condition\n"; 00264 std::vector<real> tmpRoots; 00265 solveQuadraticEquation (1.0, alpha, gamma, tmpRoots); 00266 for (unsigned int i=0; i<tmpRoots.size (); i++) 00267 { 00268 double qudraticRoot = tmpRoots[i]; 00269 if (sqrtIsNearlyZero (qudraticRoot)) 00270 { 00271 roots.push_back (-resubValue); 00272 } 00273 else if (qudraticRoot > 0.0) 00274 { 00275 root1 = sqrt (qudraticRoot); 00276 roots.push_back (root1 - resubValue); 00277 roots.push_back (-root1 - resubValue); 00278 } 00279 } 00280 } 00281 else 00282 { 00283 //cout << "beta != 0\n"; 00284 double alpha3 = alpha2*alpha, 00285 beta2 = beta*beta, 00286 p = (-alpha2/12.0)-gamma, 00287 q = (-alpha3/108.0)+ ( (alpha*gamma)/3.0)- (beta2/8.0), 00288 q2 = q*q, 00289 p3 = p*p*p, 00290 u = (0.5*q) + sqrt ( (0.25*q2)+ (p3/27.0)); 00291 if (u > 0.0) 00292 u = pow (u, 1.0/3.0); 00293 else if (isNearlyZero (u)) 00294 u = 0.0; 00295 else 00296 u = -pow (-u, 1.0/3.0); 00297 00298 double y = (-5.0/6.0)*alpha - u; 00299 if (!isNearlyZero (u)) 00300 y += p/ (3.0*u); 00301 00302 double w = alpha + 2.0*y; 00303 00304 if (w > 0) 00305 { 00306 w = sqrt (w); 00307 } 00308 else if (isNearlyZero (w)) 00309 { 00310 w = 0; 00311 } 00312 else 00313 { 00314 //cout << "Found no roots\n"; 00315 return; 00316 } 00317 00318 double tmp1 = - (3.0*alpha + 2.0*y + 2.0* (beta/w)), 00319 tmp2 = - (3.0*alpha + 2.0*y - 2.0* (beta/w)); 00320 00321 if (tmp1 > 0) 00322 { 00323 tmp1 = sqrt (tmp1); 00324 root1 = - (b/ (4.0*a)) + 0.5* (w+tmp1); 00325 root2 = - (b/ (4.0*a)) + 0.5* (w-tmp1); 00326 roots.push_back (root1); 00327 roots.push_back (root2); 00328 } 00329 else if (isNearlyZero (tmp1)) 00330 { 00331 root1 = - (b/ (4.0*a)) + 0.5*w; 00332 roots.push_back (root1); 00333 } 00334 00335 if (tmp2 > 0) 00336 { 00337 tmp2 = sqrt (tmp2); 00338 root3 = - (b/ (4.0*a)) + 0.5* (-w+tmp2); 00339 root4 = - (b/ (4.0*a)) + 0.5* (-w-tmp2); 00340 roots.push_back (root3); 00341 roots.push_back (root4); 00342 } 00343 else if (isNearlyZero (tmp2)) 00344 { 00345 root3 = - (b/ (4.0*a)) - 0.5*w; 00346 roots.push_back (root3); 00347 } 00348 00349 //cout << "Test: " << alpha<<", "<<beta<<", "<<gamma<<", "<<p<<", "<<q<<", "<<u <<", "<<y<<", "<<w<<"\n"; 00350 } 00351 00352 #if 0 00353 cout << __PRETTY_FUNCTION__ << ": Found "<<roots.size ()<<" roots.\n"; 00354 for (unsigned int i=0; i<roots.size (); i++) 00355 { 00356 real x=roots[i], x2=x*x, x3=x2*x, x4=x2*x2; 00357 real result = a*x4 + b*x3 + c*x2 + d*x + e; 00358 if (fabs (result) > 1e-4) 00359 { 00360 cout << "Something went wrong:\n"; 00361 //roots.clear (); 00362 } 00363 cout << "Root "<<i<<" = "<<roots[i] 00364 << ". ("<<a<<"x^4 + "<<b<<"x^3 + "<<c<<"x^2 + "<<d<<"x + "<<e<<" = "<<result<<")\n"; 00365 } 00366 cout << "\n\n"; 00367 #endif 00368 } 00369 00371 00372 template<typename real> 00373 inline pcl::BivariatePolynomialT<real> 00374 pcl::PolynomialCalculationsT<real>::bivariatePolynomialApproximation ( 00375 std::vector<Eigen::Matrix<real, 3, 1> >& samplePoints, unsigned int polynomial_degree, bool& error) const 00376 { 00377 pcl::BivariatePolynomialT<real> ret; 00378 error = bivariatePolynomialApproximation (samplePoints, polynomial_degree, ret); 00379 return ret; 00380 } 00381 00383 00384 template<typename real> 00385 inline bool 00386 pcl::PolynomialCalculationsT<real>::bivariatePolynomialApproximation ( 00387 std::vector<Eigen::Matrix<real, 3, 1> >& samplePoints, unsigned int polynomial_degree, 00388 pcl::BivariatePolynomialT<real>& ret) const 00389 { 00390 //MEASURE_FUNCTION_TIME; 00391 unsigned int parameters_size = BivariatePolynomialT<real>::getNoOfParametersFromDegree (polynomial_degree); 00392 //cout << PVARN (parameters_size); 00393 00394 //cout << "Searching for the "<<parameters_size<<" parameters for the bivariate polynom of degree " 00395 // << polynomial_degree<<" using "<<samplePoints.size ()<<" points.\n"; 00396 00397 if (parameters_size > samplePoints.size ()) // Too many parameters for this number of equations (points)? 00398 { 00399 return false; 00400 // Reduce degree of polynomial 00401 //polynomial_degree = (unsigned int) (0.5f* (sqrtf (8*samplePoints.size ()+1) - 3)); 00402 //parameters_size = BivariatePolynomialT<real>::getNoOfParametersFromDegree (polynomial_degree); 00403 //cout << "Not enough points, so degree of polynomial was decreased to "<<polynomial_degree 00404 // << " ("<<samplePoints.size ()<<" points => "<<parameters_size<<" parameters)\n"; 00405 } 00406 00407 ret.setDegree (polynomial_degree); 00408 00409 //double coeffStuffStartTime=-get_time (); 00410 Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> A (parameters_size, parameters_size); 00411 A.setZero(); 00412 Eigen::Matrix<real, Eigen::Dynamic, 1> b (parameters_size); 00413 b.setZero(); 00414 real currentX, currentY, currentZ; 00415 real tmpX, tmpY; 00416 real *tmpC = new real[parameters_size]; 00417 real* tmpCEndPtr = &tmpC[parameters_size-1]; 00418 for (typename std::vector<Eigen::Matrix<real, 3, 1> >::const_iterator it=samplePoints.begin (); 00419 it!=samplePoints.end (); ++it) 00420 { 00421 currentX= (*it)[0]; currentY= (*it)[1]; currentZ= (*it)[2]; 00422 //cout << "current point: "<<currentX<<","<<currentY<<" => "<<currentZ<<"\n"; 00423 //unsigned int posInC = parameters_size-1; 00424 real* tmpCPtr = tmpCEndPtr; 00425 tmpX = 1.0; 00426 for (unsigned int xDegree=0; xDegree<=polynomial_degree; ++xDegree) 00427 { 00428 tmpY = 1.0; 00429 for (unsigned int yDegree=0; yDegree<=polynomial_degree-xDegree; ++yDegree) 00430 { 00431 * (tmpCPtr--) = tmpX*tmpY; 00432 //cout << "x="<<currentX<<", y="<<currentY<<", Pos "<<posInC--<<": "<<tmpX<<"*"<<tmpY<<"="<<tmpC[posInC]<<"\n"; 00433 tmpY *= currentY; 00434 } 00435 tmpX *= currentX; 00436 } 00437 00438 real* APtr = &A(0,0); 00439 real* bPtr = &b[0]; 00440 real* tmpCPtr1=tmpC; 00441 for (unsigned int i=0; i<parameters_size; ++i) 00442 { 00443 * (bPtr++) += currentZ * *tmpCPtr1; 00444 00445 real* tmpCPtr2=tmpC; 00446 for (unsigned int j=0; j<parameters_size; ++j) 00447 { 00448 * (APtr++) += *tmpCPtr1 * * (tmpCPtr2++); 00449 } 00450 00451 ++tmpCPtr1; 00452 } 00453 //A += DMatrix<real>::outProd (tmpC); 00454 //b += currentZ * tmpC; 00455 } 00456 //cout << "Calculating matrix A and vector b (size "<<b.size ()<<") from "<<samplePoints.size ()<<" points took " 00457 //<< (coeffStuffStartTime+get_time ())*1000<<"ms using constant memory.\n"; 00458 //cout << PVARC (A)<<PVARN (b); 00459 00460 00461 //double coeffStuffStartTime=-get_time (); 00462 //DMatrix<real> A (parameters_size, parameters_size); 00463 //DVector<real> b (parameters_size); 00464 //real currentX, currentY, currentZ; 00465 //unsigned int posInC; 00466 //real tmpX, tmpY; 00467 //DVector<real> tmpC (parameters_size); 00468 //for (typename std::vector<Eigen::Matrix<real, 3, 1> >::const_iterator it=samplePoints.begin (); 00469 // it!=samplePoints.end (); ++it) 00470 //{ 00471 //currentX= (*it)[0]; currentY= (*it)[1]; currentZ= (*it)[2]; 00473 //posInC = parameters_size-1; 00474 //tmpX = 1.0; 00475 //for (unsigned int xDegree=0; xDegree<=polynomial_degree; xDegree++) 00476 //{ 00477 //tmpY = 1.0; 00478 //for (unsigned int yDegree=0; yDegree<=polynomial_degree-xDegree; yDegree++) 00479 //{ 00480 //tmpC[posInC] = tmpX*tmpY; 00482 //tmpY *= currentY; 00483 //posInC--; 00484 //} 00485 //tmpX *= currentX; 00486 //} 00487 //A += DMatrix<real>::outProd (tmpC); 00488 //b += currentZ * tmpC; 00489 //} 00490 //cout << "Calculating matrix A and vector b (size "<<b.size ()<<") from "<<samplePoints.size ()<<" points took " 00491 //<< (coeffStuffStartTime+get_time ())*1000<<"ms.\n"; 00492 00493 Eigen::Matrix<real, Eigen::Dynamic, 1> parameters; 00494 //double choleskyStartTime=-get_time (); 00495 //parameters = A.choleskySolve (b); 00496 //cout << "Cholesky took "<< (choleskyStartTime+get_time ())*1000<<"ms.\n"; 00497 00498 //double invStartTime=-get_time (); 00499 parameters = A.inverse () * b; 00500 //cout << "Inverse took "<< (invStartTime+get_time ())*1000<<"ms.\n"; 00501 00502 //cout << PVARC (A)<<PVARC (b)<<PVARN (parameters); 00503 00504 real inversionCheckResult = (A*parameters - b).norm (); 00505 if (inversionCheckResult > 1e-5) 00506 { 00507 //cout << "Inversion result: "<< inversionCheckResult<<" for matrix "<<A<<"\n"; 00508 return false; 00509 } 00510 00511 for (unsigned int i=0; i<parameters_size; i++) 00512 ret.parameters[i] = parameters[i]; 00513 00514 //cout << "Resulting polynomial is "<<ret<<"\n"; 00515 00516 //Test of gradient: ret.calculateGradient (); 00517 00518 delete [] tmpC; 00519 return true; 00520 }