From f12e3c45ca22212294f4866028dbcd6e6abea642 Mon Sep 17 00:00:00 2001 From: Jerome Date: Sat, 18 Mar 2023 16:13:17 +0100 Subject: [PATCH] Robustified C++ version and added callback management. --- cpp/include/CubicLagrangeMinimize.hpp | 23 ++++++++++- cpp/src/CubicLagrangeMinimize.cpp | 57 ++++++++++++++++++++++++--- cpp/src/tests.cpp | 8 ++-- 3 files changed, 78 insertions(+), 10 deletions(-) diff --git a/cpp/include/CubicLagrangeMinimize.hpp b/cpp/include/CubicLagrangeMinimize.hpp index 941246f..6e02bb6 100644 --- a/cpp/include/CubicLagrangeMinimize.hpp +++ b/cpp/include/CubicLagrangeMinimize.hpp @@ -9,6 +9,24 @@ using Eigen::VectorXd; using Eigen::Matrix4d; using Eigen::Vector4d; +/// @brief Returns a callback function that does nothing. +#define CubicLagrangeMinimize_GetEmptyCallback() [](Eigen::VectorXd const&){} + +/// @brief Returns a callback function that prints the current best estimate of the minimum and the iteration number. +#define CubicLagrangeMinimize_GetSimpleCallback() [](Eigen::VectorXd const& callback_args){ std::cout << "Iteration " << callback_args[2] << " : f(" << callback_args[0] << ") = " << callback_args[1] << "\n"; } + +/// @brief Returns a callback function that prints the current best estimate of the minimum, the iteration number, the current interval, and the cubic polynomial coefficients. +#define CubicLagrangeMinimize_GetDetailedCallback() \ +[](Eigen::VectorXd const& callback_args){ \ + std::cout << "--------------------------------------------------------------------\n";\ + std::cout << " Iteration " << callback_args[2] << "\n";\ + std::cout << "--------------------------------------------------------------------\n";\ + std::cout << "X values : " << callback_args[8] << ", " << callback_args[9] << ", " << callback_args[10] << ", " << callback_args[11] << "\n";\ + std::cout << "Cubic poly coeffs : " << callback_args[12] << ", " << callback_args[13] << ", " << callback_args[14] << ", " << callback_args[15] << "\n";\ + std::cout << "Quadratic solution : delta = " << callback_args[3] << ", x_sol_1 = " << callback_args[4] << ", x_sol_2 = " << callback_args[5] << ", y_sol_1 = " << callback_args[6] << ", y_sol_2 = " << callback_args[7] << "\n";\ + std::cout << "Current solution : f(" << callback_args[0] << ") = " << callback_args[1] << "\n";\ +} + /// @brief Function to find minimum of f over interval [a, b] using cubic Lagrange polynomial interpolation. /// If the function is monotonic, then the minimum is one of the bounds of the interval, and the minimum is found in a single iteration. /// The best estimate of the minimum within the current interval is returned once the interval is smaller than the tolerance. @@ -18,7 +36,10 @@ using Eigen::Vector4d; /// @param a The lower bound of the interval. /// @param b The upper bound of the interval. /// @param tol The tolerance on the interval width. +/// @param callback A function to call at each iteration with the current best estimate of the minimum and other internal variables. The vector passed to the callback function contains the following variables : (x_sol, y_sol, i, delta, x_sol_1, x_sol_2, y_sol_1, y_sol_2, x0, x1, x2, x3, a0, a1, a2, a3). /// @return The best estimate of the minimum within the current interval once its width is smaller than the tolerance. -double CubicLagrangeMinimize(std::function f, double a, double b, double tol=1e-9); +double CubicLagrangeMinimize(std::function f, double a, double b, double tol=1e-9, std::function callback = [](Eigen::VectorXd const&){}); + +// (x_sol, y_sol, i, delta, x_sol_1, x_sol_2, y_sol_1, y_sol_2, x0, x1, x2, x3, a0, a1, a2, a3). #endif \ No newline at end of file diff --git a/cpp/src/CubicLagrangeMinimize.cpp b/cpp/src/CubicLagrangeMinimize.cpp index 9a5b9cf..0d73ddf 100644 --- a/cpp/src/CubicLagrangeMinimize.cpp +++ b/cpp/src/CubicLagrangeMinimize.cpp @@ -1,5 +1,9 @@ #include +#ifndef M_PI +#define M_PI 3.1415926535897932384626433832795028841971693993751 +#endif + /// @brief Linear least-squares fit of a cubic polynomial to four points. /// @param x1 Abscissa of first point. /// @param x2 Abscissa of second point. @@ -11,7 +15,7 @@ /// @param y4 Ordinate of fourth point. /// @return 4-dimensional vector of polynomial coefficients from low to high order : [a0 a1 a2 a3] -> a0 + a1*x + a2*x^2 + a3*x^3 Vector4d polyfit4(double x1, double x2, double x3, double x4, double y1, double y2, double y3, double y4) { - Matrix4d A(4, 4); + Matrix4d A; double x1x1 = x1*x1, x2x2 = x2*x2, x3x3 = x3*x3, x4x4 = x4*x4; double x1x1x1 = x1x1*x1, x2x2x2 = x2x2*x2, x3x3x3 = x3x3*x3, x4x4x4 = x4x4*x4; A << 1, x1, x1x1, x1x1x1, @@ -23,6 +27,26 @@ Vector4d polyfit4(double x1, double x2, double x3, double x4, double y1, double return (A.transpose() * A).colPivHouseholderQr().solve(A.transpose() * y); } +/// @brief Linear least-squares fit of a cubic polynomial to four points (-pi/3, y1), (-pi/9, y2), (pi/9, y3), (pi/3, y4). +/// @param y1 Ordinate of first point. +/// @param y2 Ordinate of second point. +/// @param y3 Ordinate of third point. +/// @param y4 Ordinate of fourth point. +/// @return 4-dimensional vector of polynomial coefficients from low to high order : [a0 a1 a2 a3] -> a0 + a1*x + a2*x^2 + a3*x^3 +Vector4d polyfit4_mpi3_pi3(double y1, double y2, double y3, double y4) { + static Matrix4d AT = (Matrix4d() << 1,1,1,1, + -M_PI/3.,-M_PI/9.,M_PI/9.,M_PI/3., + pow(M_PI,2)/9.,pow(M_PI,2)/81.,pow(M_PI,2)/81.,pow(M_PI,2)/9., + -pow(M_PI,3)/27.,-pow(M_PI,3)/729.,pow(M_PI,3)/729.,pow(M_PI,3)/27.).finished();// A^T + static Matrix4d ATAinv = (Matrix4d() << 41./64.,0,-405./(64.*pow(M_PI,2)),0, + 0,3285/(64.*pow(M_PI,2)),0,-29889/(64.*pow(M_PI,4)), + -405/(64.*pow(M_PI,2)),0,6561/(64.*pow(M_PI,4)),0, + 0,-29889/(64.*pow(M_PI,4)),0,295245/(64.*pow(M_PI,6))).finished();// (A^T * A)^-1 + VectorXd y(4); + y << y1, y2, y3, y4; + return ATAinv*(AT*y); +} + /// @brief Cubic polynomial function. /// @param x Abscissa. /// @param a0 Constant term. @@ -34,15 +58,26 @@ double cubic_poly(double x, double a0, double a1, double a2, double a3) { return a0 + a1*x + a2*x*x + a3*x*x*x; } -double CubicLagrangeMinimize(std::function f, double a, double b, double tol) { +/// @brief Maps a value or array "x" from the interval [a1, b1] to the interval [a2, b2]. +/// @param x Value to map. +/// @param a1 Lower bound of the interval to map from. +/// @param b1 Upper bound of the interval to map from. +/// @param a2 Lower bound of the interval to map to. +/// @param b2 Upper bound of the interval to map to. +/// @return Mapped value. +double map_interval(double x, double a1, double b1, double a2, double b2) { return (x - a1) * (b2 - a2) / (b1 - a1) + a2; } + +double CubicLagrangeMinimize(std::function f, double a, double b, double tol, std::function callback) { // initialize interval endpoints and function values double x0 = a, x1 = a*2./3. + b*1./3., x2 = a*1./3. + b*2./3., x3 = b; // endpoints and two points in the interval double f0 = f(x0), f1 = 0., f2 = 0., f3 = f(x3); // function values at the endpoints and two points in the interval - double x_prev = x0; // previous value of x_sol to track convergence of the solution + double x_prev = (x0 + x3)/2.; // previous value of x_sol to track convergence of the solution double a0, a1, a2, a3; // coefficients of the cubic Lagrange polynomial double delta; // determinant of the quadratic equation of the Lagrange polynomial double x_sol, x_sol_1, x_sol_2, y_sol, y_sol_1, y_sol_2; // solutions of the Lagrange polynomial constexpr double small_coefficient = 1e-9; // threshold for small coefficients to avoid ill-conditioning of the quadratic equation + constexpr double x0m = -M_PI/3.; // mapped value of x0 from the interval [x0 x3] to [-pi/3 pi/3] + constexpr double x3m = M_PI/3.; // mapped value of x3 from the interval [x0 x3] to [-pi/3 pi/3] unsigned int Niter = static_cast(std::ceil(std::log(std::fabs(b - a)/tol)/std::log(3.)));// number of iterations to reduce interval width by a factor of 3 @@ -52,15 +87,17 @@ double CubicLagrangeMinimize(std::function f, double a, double b f1 = f(x1), f2 = f(x2); // compute Lagrange polynomial using least-squares fit to 4 points, which is equivalent to the cubic Lagrange polynomial - Vector4d A = polyfit4(x0, x1, x2, x3, f0, f1, f2, f3); + // Use x values remapped to the interval [-pi/3, pi/3] to minimize the condition number of the matrix (A^T * A) in the least-squares fit + Vector4d A = polyfit4_mpi3_pi3(f0, f1, f2, f3);// Equivalent to Vector4d A = polyfit4(x0, x1, x2, x3, f0, f1, f2, f3); a0 = A[0]; a1 = A[1]; a2 = A[2]; a3 = A[3]; + x_sol_1 = 0.; x_sol_2 = 0.; y_sol_1 = 0.; y_sol_2 = 0., delta = 0.; // Solve the first derivative of the Lagrange polynomial for a zero if(std::fabs(a3) > small_coefficient) { delta = -3*a1*a3 + a2*a2; if(delta < 0) { - x_sol = (f0 < f3) ? x0 : x3; // just choose the interval tha contains the minimum of the linear polynomial + x_sol = (f0 < f3) ? x0m : x3m; // just choose the interval tha contains the minimum of the linear polynomial y_sol = cubic_poly(x_sol, a0, a1, a2, a3); } else { // solve for the two solutions of the quadratic equation of the first derivative of the Lagrange polynomial x_sol_1 = (-a2 + std::sqrt(delta))/(3.*a3); @@ -77,10 +114,18 @@ double CubicLagrangeMinimize(std::function f, double a, double b x_sol = -a1/(2.*a2); y_sol = cubic_poly(x_sol, a0, a1, a2, a3); } else { // if a3 and a2 are zero, then the Lagrange polynomial is a linear polynomial - x_sol = (f0 < f3) ? x0 : x3; // just choose the interval tha contains the minimum of the linear polynomial + x_sol = (f0 < f3) ? x0m : x3m; // just choose the interval tha contains the minimum of the linear polynomial y_sol = cubic_poly(x_sol, a0, a1, a2, a3); } + // transform the solution back to the original interval + x_sol = map_interval(x_sol, x0m, x3m, x0, x3), + x_sol_1 = map_interval(x_sol_1, x0m, x3m, x0, x3); + x_sol_2 = map_interval(x_sol_2, x0m, x3m, x0, x3); + + // Call the callback function to print progress + callback((Eigen::VectorXd(16) << x_sol, y_sol, i, delta, x_sol_1, x_sol_2, y_sol_1, y_sol_2, x0, x1, x2, x3, a0, a1, a2, a3).finished()); + // Check convergence if(std::fabs(x_sol - x_prev) < tol) { break; } diff --git a/cpp/src/tests.cpp b/cpp/src/tests.cpp index 09022f8..56cac75 100644 --- a/cpp/src/tests.cpp +++ b/cpp/src/tests.cpp @@ -64,11 +64,13 @@ double dfct_07(double) { void test_CubicLagrangeMinimize() { std::vector> fcts = {fct_01, fct_02, fct_03, fct_04, fct_05, fct_06, fct_07}; std::vector> dfcts = {dfct_01, dfct_02, dfct_03, dfct_04, dfct_05, dfct_06, dfct_07}; - std::vector mins = {-1.2, -10., -1.5, -10., -1., -2., -3.}; - std::vector maxs = {1.5, 10., 3., 5., 6., 3., 4.}; + std::vector mins = {-1.2, -2., -1.5, -10., -1., -2., -3.}; + std::vector maxs = {1.5, 3., 3., 5., 6., 3., 4.}; + constexpr double tol = 1e-9; for(unsigned int i = 0 ; i < fcts.size() ; ++i) { + cout << "----------------------------------------------------------------------------------------------------\n"; auto f = fcts[i]; auto df = dfcts[i]; - auto x_min = CubicLagrangeMinimize(f, mins[i], maxs[i]); + auto x_min = CubicLagrangeMinimize(f, mins[i], maxs[i], tol, CubicLagrangeMinimize_GetDetailedCallback()); cout << "[" << mins[i] << " " << maxs[i] << "]\tf(" << x_min << ") = " << f(x_min) << " df(x_min)/dt = " << df(x_min) << endl; } }