Point Cloud Library (PCL)
1.7.0
|
00001 #ifndef PCL_FOR_EIGEN_BFGS_H 00002 #define PCL_FOR_EIGEN_BFGS_H 00003 00004 #if defined __GNUC__ 00005 # pragma GCC system_header 00006 #endif 00007 00008 #include <pcl/registration/eigen.h> 00009 00010 namespace Eigen 00011 { 00012 template< typename _Scalar > 00013 class PolynomialSolver<_Scalar,2> : public PolynomialSolverBase<_Scalar,2> 00014 { 00015 public: 00016 typedef PolynomialSolverBase<_Scalar,2> PS_Base; 00017 EIGEN_POLYNOMIAL_SOLVER_BASE_INHERITED_TYPES( PS_Base ) 00018 00019 public: 00020 00021 virtual ~PolynomialSolver () {} 00022 00023 template< typename OtherPolynomial > 00024 inline PolynomialSolver( const OtherPolynomial& poly, bool& hasRealRoot ) 00025 { 00026 compute( poly, hasRealRoot ); 00027 } 00028 00029 /** Computes the complex roots of a new polynomial. */ 00030 template< typename OtherPolynomial > 00031 void compute( const OtherPolynomial& poly, bool& hasRealRoot) 00032 { 00033 const Scalar ZERO(0); 00034 Scalar a2(2 * poly[2]); 00035 assert( ZERO != poly[poly.size()-1] ); 00036 Scalar discriminant ((poly[1] * poly[1]) - (4 * poly[0] * poly[2])); 00037 if (ZERO < discriminant) 00038 { 00039 Scalar discriminant_root (std::sqrt (discriminant)); 00040 m_roots[0] = (-poly[1] - discriminant_root) / (a2) ; 00041 m_roots[1] = (-poly[1] + discriminant_root) / (a2) ; 00042 hasRealRoot = true; 00043 } 00044 else { 00045 if (ZERO == discriminant) 00046 { 00047 m_roots.resize (1); 00048 m_roots[0] = -poly[1] / a2; 00049 hasRealRoot = true; 00050 } 00051 else 00052 { 00053 Scalar discriminant_root (std::sqrt (-discriminant)); 00054 m_roots[0] = RootType (-poly[1] / a2, -discriminant_root / a2); 00055 m_roots[1] = RootType (-poly[1] / a2, discriminant_root / a2); 00056 hasRealRoot = false; 00057 } 00058 } 00059 } 00060 00061 template< typename OtherPolynomial > 00062 void compute( const OtherPolynomial& poly) 00063 { 00064 bool hasRealRoot; 00065 compute(poly, hasRealRoot); 00066 } 00067 00068 protected: 00069 using PS_Base::m_roots; 00070 }; 00071 } 00072 00073 template<typename _Scalar, int NX=Eigen::Dynamic> 00074 struct BFGSDummyFunctor 00075 { 00076 typedef _Scalar Scalar; 00077 enum { InputsAtCompileTime = NX }; 00078 typedef Eigen::Matrix<Scalar,InputsAtCompileTime,1> VectorType; 00079 00080 const int m_inputs; 00081 00082 BFGSDummyFunctor() : m_inputs(InputsAtCompileTime) {} 00083 BFGSDummyFunctor(int inputs) : m_inputs(inputs) {} 00084 00085 virtual ~BFGSDummyFunctor() {} 00086 int inputs() const { return m_inputs; } 00087 00088 virtual double operator() (const VectorType &x) = 0; 00089 virtual void df(const VectorType &x, VectorType &df) = 0; 00090 virtual void fdf(const VectorType &x, Scalar &f, VectorType &df) = 0; 00091 }; 00092 00093 namespace BFGSSpace { 00094 enum Status { 00095 NegativeGradientEpsilon = -3, 00096 NotStarted = -2, 00097 Running = -1, 00098 Success = 0, 00099 NoProgress = 1 00100 }; 00101 } 00102 00103 /** 00104 * BFGS stands for Broyden–Fletcher–Goldfarb–Shanno (BFGS) method for solving 00105 * unconstrained nonlinear optimization problems. 00106 * For further details please visit: http://en.wikipedia.org/wiki/BFGS_method 00107 * The method provided here is almost similar to the one provided by GSL. 00108 * It reproduces Fletcher's original algorithm in Practical Methods of Optimization 00109 * algorithms : 2.6.2 and 2.6.4 and uses the same politics in GSL with cubic 00110 * interpolation whenever it is possible else falls to quadratic interpolation for 00111 * alpha parameter. 00112 */ 00113 template<typename FunctorType> 00114 class BFGS 00115 { 00116 public: 00117 typedef typename FunctorType::Scalar Scalar; 00118 typedef typename FunctorType::VectorType FVectorType; 00119 00120 BFGS(FunctorType &_functor) 00121 : pnorm(0), g0norm(0), iter(-1), functor(_functor) { } 00122 00123 typedef Eigen::DenseIndex Index; 00124 00125 struct Parameters { 00126 Parameters() 00127 : max_iters(400) 00128 , bracket_iters(100) 00129 , section_iters(100) 00130 , rho(0.01) 00131 , sigma(0.01) 00132 , tau1(9) 00133 , tau2(0.05) 00134 , tau3(0.5) 00135 , step_size(1) 00136 , order(3) {} 00137 Index max_iters; // maximum number of function evaluation 00138 Index bracket_iters; 00139 Index section_iters; 00140 Scalar rho; 00141 Scalar sigma; 00142 Scalar tau1; 00143 Scalar tau2; 00144 Scalar tau3; 00145 Scalar step_size; 00146 Index order; 00147 }; 00148 00149 BFGSSpace::Status minimize(FVectorType &x); 00150 BFGSSpace::Status minimizeInit(FVectorType &x); 00151 BFGSSpace::Status minimizeOneStep(FVectorType &x); 00152 BFGSSpace::Status testGradient(Scalar epsilon); 00153 void resetParameters(void) { parameters = Parameters(); } 00154 00155 Parameters parameters; 00156 Scalar f; 00157 FVectorType gradient; 00158 private: 00159 00160 BFGS& operator=(const BFGS&); 00161 BFGSSpace::Status lineSearch (Scalar rho, Scalar sigma, 00162 Scalar tau1, Scalar tau2, Scalar tau3, 00163 int order, Scalar alpha1, Scalar &alpha_new); 00164 Scalar interpolate (Scalar a, Scalar fa, Scalar fpa, 00165 Scalar b, Scalar fb, Scalar fpb, Scalar xmin, Scalar xmax, 00166 int order); 00167 void checkExtremum (const Eigen::Matrix<Scalar, 4, 1>& coefficients, Scalar x, Scalar& xmin, Scalar& fmin); 00168 void moveTo (Scalar alpha); 00169 Scalar slope (); 00170 Scalar applyF (Scalar alpha); 00171 Scalar applyDF (Scalar alpha); 00172 void applyFDF (Scalar alpha, Scalar &f, Scalar &df); 00173 void updatePosition (Scalar alpha, FVectorType& x, Scalar& f, FVectorType& g); 00174 void changeDirection (); 00175 00176 Scalar delta_f, fp0; 00177 FVectorType x0, dx0, dg0, g0, dx, p; 00178 Scalar pnorm, g0norm; 00179 00180 Scalar f_alpha; 00181 Scalar df_alpha; 00182 FVectorType x_alpha; 00183 FVectorType g_alpha; 00184 00185 // cache "keys" 00186 Scalar f_cache_key; 00187 Scalar df_cache_key; 00188 Scalar x_cache_key; 00189 Scalar g_cache_key; 00190 00191 Index iter; 00192 FunctorType &functor; 00193 }; 00194 00195 00196 template<typename FunctorType> void 00197 BFGS<FunctorType>::checkExtremum(const Eigen::Matrix<Scalar, 4, 1>& coefficients, Scalar x, Scalar& xmin, Scalar& fmin) 00198 { 00199 Scalar y = Eigen::poly_eval(coefficients, x); 00200 if(y < fmin) { xmin = x; fmin = y; } 00201 } 00202 00203 template<typename FunctorType> void 00204 BFGS<FunctorType>::moveTo(Scalar alpha) 00205 { 00206 x_alpha = x0 + alpha * p; 00207 x_cache_key = alpha; 00208 } 00209 00210 template<typename FunctorType> typename BFGS<FunctorType>::Scalar 00211 BFGS<FunctorType>::slope() 00212 { 00213 return (g_alpha.dot (p)); 00214 } 00215 00216 template<typename FunctorType> typename BFGS<FunctorType>::Scalar 00217 BFGS<FunctorType>::applyF(Scalar alpha) 00218 { 00219 if (alpha == f_cache_key) return f_alpha; 00220 moveTo (alpha); 00221 f_alpha = functor (x_alpha); 00222 f_cache_key = alpha; 00223 return (f_alpha); 00224 } 00225 00226 template<typename FunctorType> typename BFGS<FunctorType>::Scalar 00227 BFGS<FunctorType>::applyDF(Scalar alpha) 00228 { 00229 if (alpha == df_cache_key) return df_alpha; 00230 moveTo (alpha); 00231 if(alpha != g_cache_key) 00232 { 00233 functor.df (x_alpha, g_alpha); 00234 g_cache_key = alpha; 00235 } 00236 df_alpha = slope (); 00237 df_cache_key = alpha; 00238 return (df_alpha); 00239 } 00240 00241 template<typename FunctorType> void 00242 BFGS<FunctorType>::applyFDF(Scalar alpha, Scalar& f, Scalar& df) 00243 { 00244 if(alpha == f_cache_key && alpha == df_cache_key) 00245 { 00246 f = f_alpha; 00247 df = df_alpha; 00248 return; 00249 } 00250 00251 if(alpha == f_cache_key || alpha == df_cache_key) 00252 { 00253 f = applyF (alpha); 00254 df = applyDF (alpha); 00255 return; 00256 } 00257 00258 moveTo (alpha); 00259 functor.fdf (x_alpha, f_alpha, g_alpha); 00260 f_cache_key = alpha; 00261 g_cache_key = alpha; 00262 df_alpha = slope (); 00263 df_cache_key = alpha; 00264 f = f_alpha; 00265 df = df_alpha; 00266 } 00267 00268 template<typename FunctorType> void 00269 BFGS<FunctorType>::updatePosition (Scalar alpha, FVectorType &x, Scalar &f, FVectorType &g) 00270 { 00271 { 00272 Scalar f_alpha, df_alpha; 00273 applyFDF (alpha, f_alpha, df_alpha); 00274 } ; 00275 00276 f = f_alpha; 00277 x = x_alpha; 00278 g = g_alpha; 00279 } 00280 00281 template<typename FunctorType> void 00282 BFGS<FunctorType>::changeDirection () 00283 { 00284 x_alpha = x0; 00285 x_cache_key = 0.0; 00286 f_cache_key = 0.0; 00287 g_alpha = g0; 00288 g_cache_key = 0.0; 00289 df_alpha = slope (); 00290 df_cache_key = 0.0; 00291 } 00292 00293 template<typename FunctorType> BFGSSpace::Status 00294 BFGS<FunctorType>::minimize(FVectorType &x) 00295 { 00296 BFGSSpace::Status status = minimizeInit(x); 00297 do { 00298 status = minimizeOneStep(x); 00299 iter++; 00300 } while (status==BFGSSpace::Success && iter < parameters.max_iters); 00301 return status; 00302 } 00303 00304 template<typename FunctorType> BFGSSpace::Status 00305 BFGS<FunctorType>::minimizeInit(FVectorType &x) 00306 { 00307 iter = 0; 00308 delta_f = 0; 00309 dx.setZero (); 00310 functor.fdf(x, f, gradient); 00311 x0 = x; 00312 g0 = gradient; 00313 g0norm = g0.norm (); 00314 p = gradient * -1/g0norm; 00315 pnorm = p.norm (); 00316 fp0 = -g0norm; 00317 00318 { 00319 x_alpha = x0; x_cache_key = 0; 00320 00321 f_alpha = f; f_cache_key = 0; 00322 00323 g_alpha = g0; g_cache_key = 0; 00324 00325 df_alpha = slope (); df_cache_key = 0; 00326 } 00327 00328 return BFGSSpace::NotStarted; 00329 } 00330 00331 template<typename FunctorType> BFGSSpace::Status 00332 BFGS<FunctorType>::minimizeOneStep(FVectorType &x) 00333 { 00334 Scalar alpha = 0.0, alpha1; 00335 Scalar f0 = f; 00336 if (pnorm == 0.0 || g0norm == 0.0 || fp0 == 0) 00337 { 00338 dx.setZero (); 00339 return BFGSSpace::NoProgress; 00340 } 00341 00342 if (delta_f < 0) 00343 { 00344 Scalar del = std::max (-delta_f, 10 * std::numeric_limits<Scalar>::epsilon() * fabs(f0)); 00345 alpha1 = std::min (1.0, 2.0 * del / (-fp0)); 00346 } 00347 else 00348 alpha1 = fabs(parameters.step_size); 00349 00350 BFGSSpace::Status status = lineSearch(parameters.rho, parameters.sigma, 00351 parameters.tau1, parameters.tau2, parameters.tau3, 00352 parameters.order, alpha1, alpha); 00353 00354 if(status != BFGSSpace::Success) 00355 return status; 00356 00357 updatePosition(alpha, x, f, gradient); 00358 00359 delta_f = f - f0; 00360 00361 /* Choose a new direction for the next step */ 00362 { 00363 /* This is the BFGS update: */ 00364 /* p' = g1 - A dx - B dg */ 00365 /* A = - (1+ dg.dg/dx.dg) B + dg.g/dx.dg */ 00366 /* B = dx.g/dx.dg */ 00367 00368 Scalar dxg, dgg, dxdg, dgnorm, A, B; 00369 00370 /* dx0 = x - x0 */ 00371 dx0 = x - x0; 00372 dx = dx0; /* keep a copy */ 00373 00374 /* dg0 = g - g0 */ 00375 dg0 = gradient - g0; 00376 dxg = dx0.dot (gradient); 00377 dgg = dg0.dot (gradient); 00378 dxdg = dx0.dot (dg0); 00379 dgnorm = dg0.norm (); 00380 00381 if (dxdg != 0) 00382 { 00383 B = dxg / dxdg; 00384 A = -(1.0 + dgnorm * dgnorm / dxdg) * B + dgg / dxdg; 00385 } 00386 else 00387 { 00388 B = 0; 00389 A = 0; 00390 } 00391 00392 p = -A * dx0; 00393 p+= gradient; 00394 p+= -B * dg0 ; 00395 } 00396 00397 g0 = gradient; 00398 x0 = x; 00399 g0norm = g0.norm (); 00400 pnorm = p.norm (); 00401 00402 Scalar dir = ((p.dot (gradient)) > 0) ? -1.0 : 1.0; 00403 p*= dir / pnorm; 00404 pnorm = p.norm (); 00405 fp0 = p.dot (g0); 00406 00407 changeDirection(); 00408 return BFGSSpace::Success; 00409 } 00410 00411 template<typename FunctorType> typename BFGSSpace::Status 00412 BFGS<FunctorType>::testGradient(Scalar epsilon) 00413 { 00414 if(epsilon < 0) 00415 return BFGSSpace::NegativeGradientEpsilon; 00416 else 00417 { 00418 if(gradient.norm () < epsilon) 00419 return BFGSSpace::Success; 00420 else 00421 return BFGSSpace::Running; 00422 } 00423 } 00424 00425 template<typename FunctorType> typename BFGS<FunctorType>::Scalar 00426 BFGS<FunctorType>::interpolate (Scalar a, Scalar fa, Scalar fpa, 00427 Scalar b, Scalar fb, Scalar fpb, 00428 Scalar xmin, Scalar xmax, 00429 int order) 00430 { 00431 /* Map [a,b] to [0,1] */ 00432 Scalar y, alpha, ymin, ymax, fmin; 00433 00434 ymin = (xmin - a) / (b - a); 00435 ymax = (xmax - a) / (b - a); 00436 00437 // Ensure ymin <= ymax 00438 if (ymin > ymax) { Scalar tmp = ymin; ymin = ymax; ymax = tmp; }; 00439 00440 if (order > 2 && !(fpb != fpb) && fpb != std::numeric_limits<Scalar>::infinity ()) 00441 { 00442 fpa = fpa * (b - a); 00443 fpb = fpb * (b - a); 00444 00445 Scalar eta = 3 * (fb - fa) - 2 * fpa - fpb; 00446 Scalar xi = fpa + fpb - 2 * (fb - fa); 00447 Scalar c0 = fa, c1 = fpa, c2 = eta, c3 = xi; 00448 Scalar y0, y1; 00449 Eigen::Matrix<Scalar, 4, 1> coefficients; 00450 coefficients << c0, c1, c2, c3; 00451 00452 y = ymin; 00453 // Evaluate the cubic polyinomial at ymin; 00454 fmin = Eigen::poly_eval (coefficients, ymin); 00455 checkExtremum (coefficients, ymax, y, fmin); 00456 { 00457 // Solve quadratic polynomial for the derivate 00458 Eigen::Matrix<Scalar, 3, 1> coefficients2; 00459 coefficients2 << c1, 2 * c2, 3 * c3; 00460 bool real_roots; 00461 Eigen::PolynomialSolver<Scalar, 2> solver (coefficients2, real_roots); 00462 if(real_roots) 00463 { 00464 if ((solver.roots ()).size () == 2) /* found 2 roots */ 00465 { 00466 y0 = std::real (solver.roots () [0]); 00467 y1 = std::real (solver.roots () [1]); 00468 if(y0 > y1) { Scalar tmp (y0); y0 = y1; y1 = tmp; } 00469 if (y0 > ymin && y0 < ymax) 00470 checkExtremum (coefficients, y0, y, fmin); 00471 if (y1 > ymin && y1 < ymax) 00472 checkExtremum (coefficients, y1, y, fmin); 00473 } 00474 else if ((solver.roots ()).size () == 1) /* found 1 root */ 00475 { 00476 y0 = std::real (solver.roots () [0]); 00477 if (y0 > ymin && y0 < ymax) 00478 checkExtremum (coefficients, y0, y, fmin); 00479 } 00480 } 00481 } 00482 } 00483 else 00484 { 00485 fpa = fpa * (b - a); 00486 Scalar fl = fa + ymin*(fpa + ymin*(fb - fa -fpa)); 00487 Scalar fh = fa + ymax*(fpa + ymax*(fb - fa -fpa)); 00488 Scalar c = 2 * (fb - fa - fpa); /* curvature */ 00489 y = ymin; fmin = fl; 00490 00491 if (fh < fmin) { y = ymax; fmin = fh; } 00492 00493 if (c > a) /* positive curvature required for a minimum */ 00494 { 00495 Scalar z = -fpa / c; /* location of minimum */ 00496 if (z > ymin && z < ymax) { 00497 Scalar f = fa + z*(fpa + z*(fb - fa -fpa)); 00498 if (f < fmin) { y = z; fmin = f; }; 00499 } 00500 } 00501 } 00502 00503 alpha = a + y * (b - a); 00504 return alpha; 00505 } 00506 00507 template<typename FunctorType> BFGSSpace::Status 00508 BFGS<FunctorType>::lineSearch(Scalar rho, Scalar sigma, 00509 Scalar tau1, Scalar tau2, Scalar tau3, 00510 int order, Scalar alpha1, Scalar &alpha_new) 00511 { 00512 Scalar f0, fp0, falpha, falpha_prev, fpalpha, fpalpha_prev, delta, alpha_next; 00513 Scalar alpha = alpha1, alpha_prev = 0.0; 00514 Scalar a, b, fa, fb, fpa, fpb; 00515 Index i = 0; 00516 00517 applyFDF (0.0, f0, fp0); 00518 00519 falpha_prev = f0; 00520 fpalpha_prev = fp0; 00521 00522 /* Avoid uninitialized variables morning */ 00523 a = 0.0; b = alpha; 00524 fa = f0; fb = 0.0; 00525 fpa = fp0; fpb = 0.0; 00526 00527 /* Begin bracketing */ 00528 00529 while (i++ < parameters.bracket_iters) 00530 { 00531 falpha = applyF (alpha); 00532 00533 if (falpha > f0 + alpha * rho * fp0 || falpha >= falpha_prev) 00534 { 00535 a = alpha_prev; fa = falpha_prev; fpa = fpalpha_prev; 00536 b = alpha; fb = falpha; fpb = std::numeric_limits<Scalar>::quiet_NaN (); 00537 break; 00538 } 00539 00540 fpalpha = applyDF (alpha); 00541 00542 /* Fletcher's sigma test */ 00543 if (fabs (fpalpha) <= -sigma * fp0) 00544 { 00545 alpha_new = alpha; 00546 return BFGSSpace::Success; 00547 } 00548 00549 if (fpalpha >= 0) 00550 { 00551 a = alpha; fa = falpha; fpa = fpalpha; 00552 b = alpha_prev; fb = falpha_prev; fpb = fpalpha_prev; 00553 break; /* goto sectioning */ 00554 } 00555 00556 delta = alpha - alpha_prev; 00557 00558 { 00559 Scalar lower = alpha + delta; 00560 Scalar upper = alpha + tau1 * delta; 00561 00562 alpha_next = interpolate (alpha_prev, falpha_prev, fpalpha_prev, 00563 alpha, falpha, fpalpha, lower, upper, order); 00564 00565 } 00566 00567 alpha_prev = alpha; 00568 falpha_prev = falpha; 00569 fpalpha_prev = fpalpha; 00570 alpha = alpha_next; 00571 } 00572 /* Sectioning of bracket [a,b] */ 00573 while (i++ < parameters.section_iters) 00574 { 00575 delta = b - a; 00576 00577 { 00578 Scalar lower = a + tau2 * delta; 00579 Scalar upper = b - tau3 * delta; 00580 00581 alpha = interpolate (a, fa, fpa, b, fb, fpb, lower, upper, order); 00582 } 00583 falpha = applyF (alpha); 00584 if ((a-alpha)*fpa <= std::numeric_limits<Scalar>::epsilon ()) { 00585 /* roundoff prevents progress */ 00586 return BFGSSpace::NoProgress; 00587 }; 00588 00589 if (falpha > f0 + rho * alpha * fp0 || falpha >= fa) 00590 { 00591 /* a_next = a; */ 00592 b = alpha; fb = falpha; fpb = std::numeric_limits<Scalar>::quiet_NaN (); 00593 } 00594 else 00595 { 00596 fpalpha = applyDF (alpha); 00597 00598 if (fabs(fpalpha) <= -sigma * fp0) 00599 { 00600 alpha_new = alpha; 00601 return BFGSSpace::Success; /* terminate */ 00602 } 00603 00604 if ( ((b-a) >= 0 && fpalpha >= 0) || ((b-a) <=0 && fpalpha <= 0)) 00605 { 00606 b = a; fb = fa; fpb = fpa; 00607 a = alpha; fa = falpha; fpa = fpalpha; 00608 } 00609 else 00610 { 00611 a = alpha; fa = falpha; fpa = fpalpha; 00612 } 00613 } 00614 } 00615 return BFGSSpace::Success; 00616 } 00617 #endif // PCL_FOR_EIGEN_BFGS_H 00618