diff --git a/CMakeLists.txt b/CMakeLists.txt index 116105a..64396f5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,8 +6,11 @@ set(CMAKE_CXX_STANDARD 23) add_compile_options(-Wall -Werror -Wpedantic) +find_package(Eigen3 REQUIRED) + add_library(autoopt INTERFACE) target_include_directories(autoopt INTERFACE include) +target_link_libraries(autoopt INTERFACE Eigen3::Eigen) install(DIRECTORY include/autoopt DESTINATION include) diff --git a/flake.nix b/flake.nix index 110641d..856efe7 100644 --- a/flake.nix +++ b/flake.nix @@ -22,6 +22,7 @@ cmake, ninja, lib, + eigen, gtest, withTests ? false, }: @@ -46,6 +47,7 @@ nativeBuildInputs = [ cmake ninja + eigen ]; }; in diff --git a/include/autoopt/btls.hpp b/include/autoopt/btls.hpp new file mode 100644 index 0000000..5ed04ba --- /dev/null +++ b/include/autoopt/btls.hpp @@ -0,0 +1,49 @@ +#pragma once + +#include + +#include "autoopt/optimization_problem.hpp" + +namespace autoopt { + +template +struct btls_parameters { + T step_decrease = T{0.5}; + T step_increase = T{1.5}; + T sufficient_decrease = T{1e-2}; + T tolerance = T{1e-9}; + size_t max_iters = 1000; +}; + +template +void btls(optimization_problem& problem, + const btls_parameters& params = btls_parameters()) { + Eigen::VectorX& x = problem.x(); + T step_size = T{1.0}; + + for (size_t iter = 0; iter < params.max_iters; ++iter) { + T obj_value = problem.objective(x); + + std::cout << "Iter " << iter << ": obj = " << obj_value + << ", x = " << x.transpose() << ", step_size = " << step_size + << std::endl; + + Eigen::VectorX grad = -problem.gradient(x); + + Eigen::VectorX step_dir = grad.normalized(); + while (problem.objective(x + step_size * step_dir) > + obj_value + + params.sufficient_decrease * step_size * grad.dot(step_dir)) { + step_size *= params.step_decrease; + } + x += step_size * step_dir; + + if (step_size < params.tolerance) { + break; + } + } + + problem.x() = x; +} + +} // namespace autoopt \ No newline at end of file diff --git a/include/autoopt/derivative.hpp b/include/autoopt/derivative.hpp index 73ea64d..3822c2a 100644 --- a/include/autoopt/derivative.hpp +++ b/include/autoopt/derivative.hpp @@ -1,6 +1,6 @@ #pragma once -#include +#include #include "autoopt/dual.hpp" @@ -13,49 +13,46 @@ T derivative(Func&& f, const T& x) { return b._dx; } -template -std::array gradient(Func&& f, const std::array& x) { - std::array grad{}; - std::array, N> dual_x{}; - for (std::size_t i = 0; i < N; ++i) { - dual_x[i] = dual(x[i], T(0)); +template +Eigen::VectorX gradient(Func&& f, const Eigen::VectorX& x) { + Eigen::VectorX grad{x.size()}; + Eigen::VectorX> dual_x{x.size()}; + for (int i = 0; i < x.size(); ++i) { + dual_x(i) = dual(x(i), T(0)); } - for (std::size_t i = 0; i < N; ++i) { - dual_x[i]._dx = T(1); + for (int i = 0; i < x.size(); ++i) { + dual_x(i)._dx = T(1); dual dual_y = f(dual_x); - grad[i] = dual_y._dx; - dual_x[i]._dx = T(0); + grad(i) = dual_y._dx; + dual_x(i)._dx = T(0); } return grad; } -template -using matrix_t = std::array, N>; - -template -matrix_t jacobian(Func&& f, const std::array& x) { - matrix_t jacob{}; - std::array, N> dual_x{}; - for (std::size_t i = 0; i < N; ++i) { - dual_x[i] = dual(x[i], T(0)); +template +Eigen::MatrixX jacobian(Func&& f, const Eigen::VectorX& x) { + Eigen::MatrixX jacob(f(x).size(), x.size()); + Eigen::VectorX> dual_x(x.size()); + for (int i = 0; i < x.size(); ++i) { + dual_x(i) = dual(x(i), T(0)); } - for (std::size_t i = 0; i < N; ++i) { - dual_x[i]._dx = T(1); - std::array, M> dual_y = f(dual_x); - for (std::size_t j = 0; j < M; ++j) { - jacob[j][i] = dual_y[j]._dx; + for (int i = 0; i < x.size(); ++i) { + dual_x(i)._dx = T(1); + Eigen::VectorX> dual_y = f(dual_x); + for (int j = 0; j < dual_y.size(); ++j) { + jacob(j, i) = dual_y(j)._dx; } - dual_x[i]._dx = T(0); + dual_x(i)._dx = T(0); } return jacob; } -template -matrix_t hessian(Func&& f, const std::array& x) { - auto helper_func = [&f](const std::array& y) { - return gradient(f, y); +template +Eigen::MatrixX hessian(Func&& f, const Eigen::VectorX& x) { + auto helper_func = [&f](const Eigen::VectorX& y) { + return gradient(f, y); }; - return jacobian(helper_func, x); + return jacobian(helper_func, x); } } // namespace autoopt \ No newline at end of file diff --git a/include/autoopt/optimization_problem.hpp b/include/autoopt/optimization_problem.hpp index 8ac78c7..d863592 100644 --- a/include/autoopt/optimization_problem.hpp +++ b/include/autoopt/optimization_problem.hpp @@ -1,108 +1,109 @@ #pragma once -#include #include #include "autoopt/derivative.hpp" namespace autoopt { -template +template struct optimization_problem { - virtual std::array initial_guess() = 0; + virtual Eigen::VectorX& initial_guess() = 0; + virtual Eigen::VectorX& x() = 0; - virtual T objective(const std::array& params) = 0; - virtual std::array gradient(const std::array& params) = 0; - virtual matrix_t hessian(const std::array& params) = 0; + virtual T objective(const Eigen::VectorX& params) = 0; + virtual Eigen::VectorX gradient(const Eigen::VectorX& params) = 0; + virtual Eigen::MatrixX hessian(const Eigen::VectorX& params) = 0; }; -template -struct auto_diff_optimization_problem : public optimization_problem { +template +struct auto_diff_optimization_problem : public optimization_problem { Func _objective_func; - std::array _initial_guess; + Eigen::VectorX _initial_guess; + Eigen::VectorX _x; auto_diff_optimization_problem( - Func objective_func, std::array initial_guess = std::array{}) - : _objective_func(objective_func), _initial_guess(initial_guess) {} + Func objective_func, Eigen::VectorX initial_guess = Eigen::VectorX{}) + : _objective_func(objective_func), _initial_guess(initial_guess), _x(initial_guess) {} - std::array initial_guess() override { return _initial_guess; } + Eigen::VectorX& initial_guess() override { return _initial_guess; } - T objective(const std::array& params) override { + Eigen::VectorX& x() override { return _x; } + + T objective(const Eigen::VectorX& params) override { return _objective_func(params); } - std::array gradient(const std::array& params) override { - return autoopt::gradient(_objective_func, params); + Eigen::VectorX gradient(const Eigen::VectorX& params) override { + return autoopt::gradient(_objective_func, params); } - matrix_t hessian(const std::array& params) override { - return autoopt::hessian(_objective_func, params); + Eigen::MatrixX hessian(const Eigen::VectorX& params) override { + return autoopt::hessian(_objective_func, params); } }; -template +template struct log_barrier_optimization_problem - : public optimization_problem { - optimization_problem& _base_problem; - std::array _delta; + : public optimization_problem { + optimization_problem& _base_problem; + Eigen::VectorX _delta; T _barrier_strength; log_barrier_optimization_problem( - optimization_problem& base_problem, - std::array delta, + optimization_problem& base_problem, + Eigen::VectorX delta, T barrier_strength = T{1e-3}) : _base_problem(base_problem), _delta(delta), _barrier_strength(barrier_strength) {} - std::array initial_guess() override { + Eigen::VectorX& initial_guess() override { return _base_problem.initial_guess(); } - T objective(const std::array& params) override { + Eigen::VectorX& x() override { + return _base_problem.x(); + } + + T objective(const Eigen::VectorX& params) override { T base_obj = _base_problem.objective(params); T barrier = barrier_term(params); return base_obj + barrier; } - std::array gradient(const std::array& params) override { + Eigen::VectorX gradient(const Eigen::VectorX& params) override { auto base_grad = _base_problem.gradient(params); - std::array barrier_grad = autoopt::gradient( - [this](const std::array& p) { + Eigen::VectorX barrier_grad = autoopt::gradient( + [this](const Eigen::VectorX& p) { return barrier_term(p); }, params); - std::array total_grad; - for (size_t i = 0; i < N; ++i) { - total_grad[i] = base_grad[i] + barrier_grad[i]; - } + Eigen::VectorX total_grad(params.size()); + total_grad = base_grad + barrier_grad; return total_grad; } - matrix_t hessian(const std::array& params) override { + Eigen::MatrixX hessian(const Eigen::VectorX& params) override { auto base_hess = _base_problem.hessian(params); - matrix_t barrier_hess = autoopt::hessian( - [this](const std::array& p) { + Eigen::MatrixX barrier_hess = autoopt::hessian( + [this](const Eigen::VectorX& p) { return barrier_term(p); }, params); - matrix_t total_hess; - for (size_t i = 0; i < N; ++i) { - for (size_t j = 0; j < N; ++j) { - total_hess[i][j] = base_hess[i][j] + barrier_hess[i][j]; - } - } + Eigen::MatrixX total_hess(params.size(), params.size()); + total_hess = base_hess + barrier_hess; return total_hess; } private: template - U barrier_term(const std::array& params) { + U barrier_term(const Eigen::VectorX& params) { U barrier = U{0}; - for (size_t i = 0; i < N; ++i) { - U lb = _base_problem.initial_guess()[i] - _delta[i]; - U ub = _base_problem.initial_guess()[i] + _delta[i]; - barrier = barrier + std::log(params[i] - lb) + std::log(ub - params[i]); + for (int i = 0; i < params.size(); ++i) { + U lb = U{_base_problem.initial_guess()(i) - _delta(i)}; + U ub = U{_base_problem.initial_guess()(i) + _delta(i)}; + barrier = barrier + std::log(params(i) - lb) + std::log(ub - params(i)); } return -U{_barrier_strength} * barrier; } diff --git a/tests/dual.cpp b/tests/dual.cpp index 91cd555..463c165 100644 --- a/tests/dual.cpp +++ b/tests/dual.cpp @@ -50,58 +50,63 @@ TEST(DualTest, DerivativeFunction) { } TEST(DualTest, GradientFunction) { - auto func = [](const std::array& x) { - return x[0] * x[0] + std::sin(x[1]); + auto func = [](const Eigen::VectorX& x) { + return x(0) * x(0) + std::sin(x(1)); }; - std::array point = {1.0, 0.0}; - std::array grad = gradient(func, point); + Eigen::VectorX point(2); + point << 1.0, 0.0; + Eigen::VectorX grad = gradient(func, point); - EXPECT_DOUBLE_EQ(grad[0], 2.0 * point[0]); // d/dx1 - EXPECT_DOUBLE_EQ(grad[1], std::cos(point[1])); // d/dx2 + EXPECT_DOUBLE_EQ(grad(0), 2.0 * point(0)); // d/dx1 + EXPECT_DOUBLE_EQ(grad(1), std::cos(point(1))); // d/dx2 } TEST(DualTest, JacobianFunction) { - auto func = [](const std::array& x) { - return std::array{x[0] * x[0], std::sin(x[1])}; + auto func = [](const Eigen::VectorX& x) { + Eigen::VectorX y(2); + y << x(0) * x(0), std::sin(x(1)); + return y; }; - std::array point = {1.0, 0.0}; - auto jacob = jacobian(func, point); + Eigen::VectorX point(2); + point << 1.0, 0.0; + auto jacob = jacobian(func, point); - EXPECT_DOUBLE_EQ(jacob[0][0], 2.0 * point[0]); // d(f1)/d(x1) - EXPECT_DOUBLE_EQ(jacob[0][1], 0.0); // d(f1)/d(x2) - EXPECT_DOUBLE_EQ(jacob[1][0], 0.0); // d(f2)/d(x1) - EXPECT_DOUBLE_EQ(jacob[1][1], std::cos(point[1])); // d(f2)/d(x2) + EXPECT_DOUBLE_EQ(jacob(0, 0), 2.0 * point(0)); // d(f1)/d(x1) + EXPECT_DOUBLE_EQ(jacob(0, 1), 0.0); // d(f1)/d(x2) + EXPECT_DOUBLE_EQ(jacob(1, 0), 0.0); // d(f2)/d(x1) + EXPECT_DOUBLE_EQ(jacob(1, 1), std::cos(point(1))); // d(f2)/d(x2) } TEST(DualTest, HessianFunction) { - auto func = [](const std::array& x) { - return x[0] * x[0] + x[1] * x[1]; + auto func = [](const Eigen::VectorX& x) { + return x(0) * x(0) + x(1) * x(1); }; - std::array point = {1.0, 2.0}; - auto hess = hessian(func, point); + Eigen::VectorX point(2); + point << 1.0, 2.0; + auto hess = hessian(func, point); - EXPECT_DOUBLE_EQ(hess[0][0], 2.0); // d²f/dx1² - EXPECT_DOUBLE_EQ(hess[0][1], 0.0); // d²f/dx1dx2 - EXPECT_DOUBLE_EQ(hess[1][0], 0.0); // d²f/dx2dx1 - EXPECT_DOUBLE_EQ(hess[1][1], 2.0); // d²f/dx2² + EXPECT_DOUBLE_EQ(hess(0, 0), 2.0); // d²f/dx1² + EXPECT_DOUBLE_EQ(hess(0, 1), 0.0); // d²f/dx1dx2 + EXPECT_DOUBLE_EQ(hess(1, 0), 0.0); // d²f/dx2dx1 + EXPECT_DOUBLE_EQ(hess(1, 1), 2.0); // d²f/dx2² } struct opti_func { - std::vector> test_data; + std::vector> test_data; // loss function template - T operator()(const std::array& params) const { + T operator()(const Eigen::VectorX& params) const { T sum = T(0); for (const auto& data_point : test_data) { - T x = T{data_point[0]}; - T y_true = T{data_point[1]}; - T a = params[0]; - T b = params[1]; - T c = params[2]; + T x = T{data_point(0)}; + T y_true = T{data_point(1)}; + T a = params(0); + T b = params(1); + T c = params(2); T y_pred = a * x * x + b * x + c; T error = y_pred - y_true; sum = sum + error * error; @@ -120,18 +125,19 @@ TEST(DualTest, OptimizationFunction) { {4.0, 4.0}, }; - std::array params = {1.0, -4.0, 4.0}; + Eigen::VectorX params(3); + params << 1.0, -4.0, 4.0; - auto grad = gradient(f, params); + auto grad = gradient(f, params); - EXPECT_DOUBLE_EQ(grad[0], 0.0); // dL/da - EXPECT_DOUBLE_EQ(grad[1], 0.0); // dL/db - EXPECT_DOUBLE_EQ(grad[2], 0.0); // dL/dc + EXPECT_DOUBLE_EQ(grad(0), 0.0); // dL/da + EXPECT_DOUBLE_EQ(grad(1), 0.0); // dL/db + EXPECT_DOUBLE_EQ(grad(2), 0.0); // dL/dc - auto hess = hessian(f, params); + auto hess = hessian(f, params); for (std::size_t i = 0; i < 3; ++i) { for (std::size_t j = 0; j < 3; ++j) { - EXPECT_GE(hess[i][j], 0.0); // Hessian should be positive semi-definite + EXPECT_GE(hess(i, j), 0.0); // Hessian should be positive semi-definite } } } diff --git a/tests/ellipse.cpp b/tests/ellipse.cpp index 41cd5dc..6af8f64 100644 --- a/tests/ellipse.cpp +++ b/tests/ellipse.cpp @@ -3,6 +3,7 @@ #include #include #include +#include #include #include @@ -21,11 +22,12 @@ TEST(Ellipse, ParamGradient) { std::vector> data_points = { {-10.0, -0.001}, {0.0, 0.0}, {10, 0.0009}}; - std::array params = {100, 1000, deg2rad(1.0), 0.0}; + Eigen::VectorX params(4); + params << 100, 1000, deg2rad(1.0), 0.0; - auto loss_func = [&data_points](const std::array& p) { - ellipse e{T{p[0]}, T{p[1]}, T{p[2]}}; - quadric q = e.to_quadric().rotated_by(T{p[3]}); + auto loss_func = [&data_points](const Eigen::VectorX& p) { + ellipse e{T{p(0)}, T{p(1)}, T{p(2)}}; + quadric q = e.to_quadric().rotated_by(T{p(3)}); T loss = T{0}; for (const auto& [x, y_true] : data_points) { T y_pred = q.slope_at(T{x}); @@ -35,44 +37,36 @@ TEST(Ellipse, ParamGradient) { return loss / T(data_points.size()); }; - auto_diff_optimization_problem problem(loss_func, params); + auto_diff_optimization_problem problem(loss_func, params); auto grad = problem.gradient(params); - EXPECT_NEAR(grad[0], -2.0789313126683308e-10, 1e-15); // d/d(left_arm) - EXPECT_NEAR(grad[1], -1.7464984353858657e-12, 1e-15); // d/d(right_arm) - EXPECT_NEAR(grad[2], 1.2013025455499119e-06, 1e-15); // d/d(entrance_angle) - EXPECT_NEAR(grad[3], -2.0332702665822054e-05, 1e-15); // d/d(rotation_angle) - std::cout << "Gradient:\n"; - for (size_t i = 0; i < 4; ++i) { - std::cout << grad[i] << "\n"; - } + std::cout << grad << std::endl; + + EXPECT_NEAR(grad(0), -2.0789313126683308e-10, 1e-15); // d/d(left_arm) + EXPECT_NEAR(grad(1), -1.7464984353858657e-12, 1e-15); // d/d(right_arm) + EXPECT_NEAR(grad(2), 1.2013025455499119e-06, 1e-15); // d/d(entrance_angle) + EXPECT_NEAR(grad(3), -2.0332702665822054e-05, 1e-15); // d/d(rotation_angle) auto hess = problem.hessian(params); - // set formatting for easier reading - std::cout << std::scientific; - // set field width for alignment - std::cout << "Hessian matrix:\n"; + std::cout << hess << std::endl; - for (size_t i = 0; i < 4; ++i) { - ; - for (size_t j = 0; j < 4; ++j) { - std::cout << std::setprecision(5) << std::setw(15) << hess[i][j]; - } - std::cout << "\n"; - } + Eigen::VectorX params_delta(4); + params_delta << 1.0, 1.0, deg2rad(0.1), deg2rad(0.1); + + params(0) += 0.9; // left_arm // log barrier - log_barrier_optimization_problem log_barrier_problem( + log_barrier_optimization_problem log_barrier_problem( problem, - {1.0, 1.0, deg2rad(0.1), deg2rad(0.1)}, + params_delta, 1e-3); auto log_barrier_grad = log_barrier_problem.gradient(params); - std::cout << "Log Barrier Gradient:\n"; - for (size_t i = 0; i < 4; ++i) { - std::cout << log_barrier_grad[i] << "\n"; - } + std::cout << "Log barrier gradient:" << std::endl; + std::cout << log_barrier_grad << std::endl; + + btls(problem); } \ No newline at end of file