init
This commit is contained in:
@@ -0,0 +1,61 @@
|
||||
#pragma once
|
||||
|
||||
#include <array>
|
||||
|
||||
#include "autoopt/dual.hpp"
|
||||
|
||||
namespace autoopt {
|
||||
|
||||
template <typename T, class Func>
|
||||
T derivative(Func&& f, const T& x) {
|
||||
dual<T> a(x, T(1));
|
||||
dual<T> b = f(a);
|
||||
return b._dx;
|
||||
}
|
||||
|
||||
template <typename T, std::size_t N, class Func>
|
||||
std::array<T, N> gradient(Func&& f, const std::array<T, N>& x) {
|
||||
std::array<T, N> grad{};
|
||||
std::array<dual<T>, N> dual_x{};
|
||||
for (std::size_t i = 0; i < N; ++i) {
|
||||
dual_x[i] = dual<T>(x[i], T(0));
|
||||
}
|
||||
for (std::size_t i = 0; i < N; ++i) {
|
||||
dual_x[i]._dx = T(1);
|
||||
dual<T> dual_y = f(dual_x);
|
||||
grad[i] = dual_y._dx;
|
||||
dual_x[i]._dx = T(0);
|
||||
}
|
||||
return grad;
|
||||
}
|
||||
|
||||
template <typename T, std::size_t N, std::size_t M>
|
||||
using matrix_t = std::array<std::array<T, M>, N>;
|
||||
|
||||
template <typename T, std::size_t N, std::size_t M, class Func>
|
||||
matrix_t<T, M, N> jacobian(Func&& f, const std::array<T, N>& x) {
|
||||
matrix_t<T, M, N> jacob{};
|
||||
std::array<dual<T>, N> dual_x{};
|
||||
for (std::size_t i = 0; i < N; ++i) {
|
||||
dual_x[i] = dual<T>(x[i], T(0));
|
||||
}
|
||||
for (std::size_t i = 0; i < N; ++i) {
|
||||
dual_x[i]._dx = T(1);
|
||||
std::array<dual<T>, M> dual_y = f(dual_x);
|
||||
for (std::size_t j = 0; j < M; ++j) {
|
||||
jacob[j][i] = dual_y[j]._dx;
|
||||
}
|
||||
dual_x[i]._dx = T(0);
|
||||
}
|
||||
return jacob;
|
||||
}
|
||||
|
||||
template <typename T, std::size_t N, class Func>
|
||||
matrix_t<T, N, N> hessian(Func&& f, const std::array<T, N>& x) {
|
||||
auto helper_func = [&f]<typename U>(const std::array<U, N>& y) {
|
||||
return gradient<U, N>(f, y);
|
||||
};
|
||||
return jacobian<T, N, N>(helper_func, x);
|
||||
}
|
||||
|
||||
} // namespace autoopt
|
||||
@@ -0,0 +1,241 @@
|
||||
#pragma once
|
||||
|
||||
#include <cmath>
|
||||
|
||||
namespace autoopt {
|
||||
|
||||
template <typename T>
|
||||
struct dual {
|
||||
T _x;
|
||||
T _dx;
|
||||
|
||||
constexpr dual(T x = T{0}, T dx = T{0}) : _x(x), _dx(dx) {}
|
||||
|
||||
// allow arbitrary deeply-nested dual construction
|
||||
template <typename U>
|
||||
constexpr dual(U x) : _x(T(x)), _dx(T(0)) {}
|
||||
};
|
||||
|
||||
template <typename T>
|
||||
constexpr dual<T> operator+(const dual<T>& a, const dual<T>& b) {
|
||||
return dual<T>(a._x + b._x, a._dx + b._dx);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
constexpr dual<T> operator-(const dual<T>& a) {
|
||||
return dual<T>(-a._x, -a._dx);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
constexpr dual<T> operator-(const dual<T>& a, const dual<T>& b) {
|
||||
return dual<T>(a._x - b._x, a._dx - b._dx);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
constexpr dual<T> operator*(const dual<T>& a, const dual<T>& b) {
|
||||
return dual<T>(a._x * b._x, a._x * b._dx + a._dx * b._x);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
constexpr dual<T> operator/(const dual<T>& a, const dual<T>& b) {
|
||||
return dual<T>(a._x / b._x, (a._dx * b._x - a._x * b._dx) / (b._x * b._x));
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
constexpr bool operator==(const dual<T>& a, const dual<T>& b) {
|
||||
return a._x == b._x;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
constexpr bool operator!=(const dual<T>& a, const dual<T>& b) {
|
||||
return a._x != b._x;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
constexpr bool operator<(const dual<T>& a, const dual<T>& b) {
|
||||
return a._x < b._x;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
constexpr bool operator<=(const dual<T>& a, const dual<T>& b) {
|
||||
return a._x <= b._x;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
constexpr bool operator>(const dual<T>& a, const dual<T>& b) {
|
||||
return a._x > b._x;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
constexpr bool operator>=(const dual<T>& a, const dual<T>& b) {
|
||||
return a._x >= b._x;
|
||||
}
|
||||
|
||||
} // namespace autoopt
|
||||
|
||||
namespace std {
|
||||
|
||||
using autoopt::dual;
|
||||
|
||||
// forward declarations of standard functions
|
||||
|
||||
template <typename T>
|
||||
constexpr dual<T> abs(const dual<T>& a);
|
||||
|
||||
template <typename T>
|
||||
constexpr dual<T> exp(const dual<T>& a);
|
||||
|
||||
template <typename T>
|
||||
constexpr dual<T> log(const dual<T>& a);
|
||||
|
||||
template <typename T>
|
||||
constexpr dual<T> pow(const dual<T>& a, const dual<T>& b);
|
||||
|
||||
template <typename T>
|
||||
constexpr dual<T> sqrt(const dual<T>& a);
|
||||
|
||||
// forward declarations of trigonometric functions
|
||||
|
||||
template <typename T>
|
||||
constexpr dual<T> sin(const dual<T>& a);
|
||||
|
||||
template <typename T>
|
||||
constexpr dual<T> cos(const dual<T>& a);
|
||||
|
||||
template <typename T>
|
||||
constexpr dual<T> tan(const dual<T>& a);
|
||||
|
||||
template <typename T>
|
||||
constexpr dual<T> asin(const dual<T>& a);
|
||||
|
||||
template <typename T>
|
||||
constexpr dual<T> acos(const dual<T>& a);
|
||||
|
||||
template <typename T>
|
||||
constexpr dual<T> atan(const dual<T>& a);
|
||||
|
||||
template <typename T>
|
||||
constexpr dual<T> atan2(const dual<T>& y, const dual<T>& x);
|
||||
|
||||
// forward declarations of hyperbolic functions
|
||||
|
||||
template <typename T>
|
||||
constexpr dual<T> sinh(const dual<T>& a);
|
||||
|
||||
template <typename T>
|
||||
constexpr dual<T> cosh(const dual<T>& a);
|
||||
|
||||
template <typename T>
|
||||
constexpr dual<T> tanh(const dual<T>& a);
|
||||
|
||||
template <typename T>
|
||||
constexpr dual<T> asinh(const dual<T>& a);
|
||||
|
||||
template <typename T>
|
||||
constexpr dual<T> acosh(const dual<T>& a);
|
||||
|
||||
template <typename T>
|
||||
constexpr dual<T> atanh(const dual<T>& a);
|
||||
|
||||
// standard functions
|
||||
|
||||
template <typename T>
|
||||
constexpr dual<T> abs(const dual<T>& a) {
|
||||
return dual<T>(std::abs(a._x), (a._x >= T(0) ? T(1) : T(-1)) * a._dx);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
constexpr dual<T> exp(const dual<T>& a) {
|
||||
T exp_x = std::exp(a._x);
|
||||
return dual<T>(exp_x, exp_x * a._dx);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
constexpr dual<T> log(const dual<T>& a) {
|
||||
return dual<T>(std::log(a._x), (T(1) / a._x) * a._dx);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
constexpr dual<T> pow(const dual<T>& a, const dual<T>& b) {
|
||||
return std::exp(b * std::log(a));
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
constexpr dual<T> sqrt(const dual<T>& a) {
|
||||
T sqrt_x = std::sqrt(a._x);
|
||||
return dual<T>(sqrt_x, (T(1) / (T(2) * sqrt_x)) * a._dx);
|
||||
}
|
||||
|
||||
// trigonometric functions
|
||||
|
||||
template <typename T>
|
||||
constexpr dual<T> sin(const dual<T>& a) {
|
||||
return dual<T>(std::sin(a._x), std::cos(a._x) * a._dx);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
constexpr dual<T> cos(const dual<T>& a) {
|
||||
return dual<T>(std::cos(a._x), -std::sin(a._x) * a._dx);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
constexpr dual<T> tan(const dual<T>& a) {
|
||||
T cos_x = std::cos(a._x);
|
||||
return dual<T>(std::tan(a._x), (T(1) / (cos_x * cos_x)) * a._dx);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
constexpr dual<T> asin(const dual<T>& a) {
|
||||
return dual<T>(std::asin(a._x), (T(1) / std::sqrt(T(1) - a._x * a._x)) * a._dx);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
constexpr dual<T> acos(const dual<T>& a) {
|
||||
return dual<T>(std::acos(a._x), (-T(1) / std::sqrt(T(1) - a._x * a._x)) * a._dx);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
constexpr dual<T> atan(const dual<T>& a) {
|
||||
return dual<T>(std::atan(a._x), (T(1) / (T(1) + a._x * a._x)) * a._dx);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
constexpr dual<T> atan2(const dual<T>& y, const dual<T>& x) {
|
||||
return dual<T>(std::atan2(y._x, x._x),
|
||||
(x._x * y._dx - y._x * x._dx) / (x._x * x._x + y._x * y._x));
|
||||
}
|
||||
|
||||
// hyperbolic functions
|
||||
|
||||
template <typename T>
|
||||
constexpr dual<T> sinh(const dual<T>& a) {
|
||||
return dual<T>(std::sinh(a._x), std::cosh(a._x) * a._dx);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
constexpr dual<T> cosh(const dual<T>& a) {
|
||||
return dual<T>(std::cosh(a._x), std::sinh(a._x) * a._dx);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
constexpr dual<T> tanh(const dual<T>& a) {
|
||||
T cosh_x = std::cosh(a._x);
|
||||
return dual<T>(std::tanh(a._x), (T(1) / (cosh_x * cosh_x)) * a._dx);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
constexpr dual<T> asinh(const dual<T>& a) {
|
||||
return dual<T>(std::asinh(a._x), (T(1) / std::sqrt(a._x * a._x + T(1))) * a._dx);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
constexpr dual<T> acosh(const dual<T>& a) {
|
||||
return dual<T>(std::acosh(a._x), (T(1) / std::sqrt(a._x * a._x - T(1))) * a._dx);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
constexpr dual<T> atanh(const dual<T>& a) {
|
||||
return dual<T>(std::atanh(a._x), (T(1) / (T(1) - a._x * a._x)) * a._dx);
|
||||
}
|
||||
|
||||
} // namespace std
|
||||
@@ -0,0 +1,50 @@
|
||||
#pragma once
|
||||
|
||||
#include "autoopt/quadric.hpp"
|
||||
|
||||
namespace autoopt {
|
||||
|
||||
template <typename T>
|
||||
struct ellipse {
|
||||
T left_arm;
|
||||
T right_arm;
|
||||
T entrance_angle;
|
||||
|
||||
constexpr quadric<T> to_quadric() const {
|
||||
T a = (left_arm + right_arm) / T{2};
|
||||
T a2 = a * a;
|
||||
|
||||
T left_x = -left_arm * std::cos(entrance_angle);
|
||||
T left_y = left_arm * std::sin(entrance_angle);
|
||||
|
||||
T right_x = right_arm * std::cos(entrance_angle);
|
||||
T right_y = right_arm * std::sin(entrance_angle);
|
||||
|
||||
T c_x = (left_x + right_x) / T{2};
|
||||
T c_y = (left_y + right_y) / T{2};
|
||||
|
||||
T c2 = (left_x - c_x) * (left_x - c_x) + (left_y - c_y) * (left_y - c_y);
|
||||
T b2 = a2 - c2;
|
||||
|
||||
|
||||
// source: https://en.wikipedia.org/wiki/Ellipse#General_ellipse
|
||||
|
||||
T theta = std::atan2(right_y - left_y, right_x - left_x);
|
||||
T cos_t = std::cos(theta);
|
||||
T sin_t = std::sin(theta);
|
||||
T cos_t2 = cos_t * cos_t;
|
||||
T sin_t2 = sin_t * sin_t;
|
||||
T sico_t = sin_t * cos_t;
|
||||
|
||||
T A = a2 * sin_t2 + b2 * cos_t2;
|
||||
T B = T{2} * (b2 - a2) * sico_t;
|
||||
T C = a2 * cos_t2 + b2 * sin_t2;
|
||||
T D = -T{2} * A * c_x - B * c_y;
|
||||
T E = -B * c_x - T{2} * C * c_y;
|
||||
T F = A * c_x * c_x + B * c_x * c_y + C * c_y * c_y - a2 * b2;
|
||||
|
||||
return quadric<T>(A, B, C, D, E, F);
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace autoopt
|
||||
@@ -0,0 +1,111 @@
|
||||
#pragma once
|
||||
|
||||
#include <array>
|
||||
#include <cstddef>
|
||||
|
||||
#include "autoopt/derivative.hpp"
|
||||
|
||||
namespace autoopt {
|
||||
|
||||
template <typename T, size_t N>
|
||||
struct optimization_problem {
|
||||
virtual std::array<T, N> initial_guess() = 0;
|
||||
|
||||
virtual T objective(const std::array<T, N>& params) = 0;
|
||||
virtual std::array<T, N> gradient(const std::array<T, N>& params) = 0;
|
||||
virtual matrix_t<T, N, N> hessian(const std::array<T, N>& params) = 0;
|
||||
};
|
||||
|
||||
template <typename T, size_t N, class Func>
|
||||
struct auto_diff_optimization_problem : public optimization_problem<T, N> {
|
||||
Func _objective_func;
|
||||
std::array<T, N> _initial_guess;
|
||||
|
||||
auto_diff_optimization_problem(
|
||||
Func objective_func, std::array<T, N> initial_guess = std::array<T, N>{})
|
||||
: _objective_func(objective_func), _initial_guess(initial_guess) {}
|
||||
|
||||
std::array<T, N> initial_guess() override { return _initial_guess; }
|
||||
|
||||
T objective(const std::array<T, N>& params) override {
|
||||
return _objective_func(params);
|
||||
}
|
||||
|
||||
std::array<T, N> gradient(const std::array<T, N>& params) override {
|
||||
return autoopt::gradient<T, N>(_objective_func, params);
|
||||
}
|
||||
|
||||
matrix_t<T, N, N> hessian(const std::array<T, N>& params) override {
|
||||
return autoopt::hessian<T, N>(_objective_func, params);
|
||||
}
|
||||
};
|
||||
|
||||
template <typename T, size_t N>
|
||||
struct log_barrier_optimization_problem
|
||||
: public optimization_problem<T, N> {
|
||||
optimization_problem<T, N>& _base_problem;
|
||||
std::array<T, N> _delta;
|
||||
T _barrier_strength;
|
||||
|
||||
log_barrier_optimization_problem(
|
||||
optimization_problem<T, N>& base_problem,
|
||||
std::array<T, N> delta,
|
||||
T barrier_strength = T{1e-3})
|
||||
: _base_problem(base_problem),
|
||||
_delta(delta),
|
||||
_barrier_strength(barrier_strength) {}
|
||||
|
||||
std::array<T, N> initial_guess() override {
|
||||
return _base_problem.initial_guess();
|
||||
}
|
||||
|
||||
T objective(const std::array<T, N>& params) override {
|
||||
T base_obj = _base_problem.objective(params);
|
||||
T barrier = barrier_term(params);
|
||||
return base_obj + barrier;
|
||||
}
|
||||
|
||||
std::array<T, N> gradient(const std::array<T, N>& params) override {
|
||||
auto base_grad = _base_problem.gradient(params);
|
||||
std::array<T, N> barrier_grad = autoopt::gradient<T, N>(
|
||||
[this]<typename U>(const std::array<U, N>& p) {
|
||||
return barrier_term<U>(p);
|
||||
},
|
||||
params);
|
||||
std::array<T, N> total_grad;
|
||||
for (size_t i = 0; i < N; ++i) {
|
||||
total_grad[i] = base_grad[i] + barrier_grad[i];
|
||||
}
|
||||
return total_grad;
|
||||
}
|
||||
|
||||
matrix_t<T, N, N> hessian(const std::array<T, N>& params) override {
|
||||
auto base_hess = _base_problem.hessian(params);
|
||||
matrix_t<T, N, N> barrier_hess = autoopt::hessian<T, N>(
|
||||
[this]<typename U>(const std::array<U, N>& p) {
|
||||
return barrier_term<U>(p);
|
||||
},
|
||||
params);
|
||||
matrix_t<T, N, N> 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];
|
||||
}
|
||||
}
|
||||
return total_hess;
|
||||
}
|
||||
|
||||
private:
|
||||
template <typename U>
|
||||
U barrier_term(const std::array<U, N>& 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]);
|
||||
}
|
||||
return -U{_barrier_strength} * barrier;
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace autoopt
|
||||
@@ -0,0 +1,60 @@
|
||||
#pragma once
|
||||
|
||||
#include <cmath>
|
||||
#include <autoopt/derivative.hpp>
|
||||
|
||||
namespace autoopt {
|
||||
|
||||
template <typename T>
|
||||
struct quadric {
|
||||
T _A; // x² coefficient
|
||||
T _B; // xy coefficient
|
||||
T _C; // y² coefficient
|
||||
T _D; // x coefficient
|
||||
T _E; // y coefficient
|
||||
T _F; // constant term
|
||||
|
||||
constexpr quadric(T A = 0, T B = 0, T C = 0, T D = 0, T E = 0, T F = 0)
|
||||
: _A(A), _B(B), _C(C), _D(D), _E(E), _F(F) {}
|
||||
|
||||
constexpr quadric rotated_by(T angle_rad) const {
|
||||
T co = std::cos(angle_rad);
|
||||
T si = std::sin(angle_rad);
|
||||
T co_2 = co * co;
|
||||
T si_2 = si * si;
|
||||
T co_si = co * si;
|
||||
|
||||
T A_new = _A * co_2 - _B * co_si + _C * si_2;
|
||||
T B_new = T{2} * (_A - _C) * co_si + _B * (co_2 - si_2);
|
||||
T C_new = _A * si_2 + _B * co_si + _C * co_2;
|
||||
T D_new = _D * co - _E * si;
|
||||
T E_new = _D * si + _E * co;
|
||||
T F_new = _F;
|
||||
|
||||
return quadric(A_new, B_new, C_new, D_new, E_new, F_new);
|
||||
}
|
||||
|
||||
constexpr T at(T x) const {
|
||||
T sign = T{-1};
|
||||
T Bx_E = _B * x + _E;
|
||||
T Ax2_Dx_F = _A * x * x + _D * x + _F;
|
||||
|
||||
if (_C == T{0}) {
|
||||
return -Ax2_Dx_F / Bx_E;
|
||||
}
|
||||
|
||||
T discriminant = Bx_E * Bx_E - T{4} * _C * Ax2_Dx_F;
|
||||
T num = sign * std::sqrt(discriminant) - Bx_E;
|
||||
T denom = T{2} * _C;
|
||||
return num / denom;
|
||||
}
|
||||
|
||||
constexpr T slope_at(T x) const {
|
||||
return derivative([&]<typename U>(U x_val) {
|
||||
quadric<U> q{U{_A}, U{_B}, U{_C}, U{_D}, U{_E}, U{_F}};
|
||||
return q.at(x_val);
|
||||
}, x);
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace autoopt
|
||||
@@ -0,0 +1,38 @@
|
||||
#pragma once
|
||||
|
||||
#include <cmath>
|
||||
|
||||
namespace autoopt {
|
||||
|
||||
template <typename T>
|
||||
T rad2deg(T radians) {
|
||||
return radians * (T{180} / M_PI);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
T deg2rad(T degrees) {
|
||||
return degrees * (M_PI / T{180});
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
T rad2arcmin(T radians) {
|
||||
return radians * (T{180 * 60} / M_PI);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
T arcmin2rad(T arcminutes) {
|
||||
return arcminutes * (M_PI / T{180 * 60});
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
T rad2arcsec(T radians) {
|
||||
return radians * (T{180 * 60 * 60} / M_PI);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
T arcsec2rad(T arcseconds) {
|
||||
return arcseconds * (M_PI / T{180 * 60 * 60});
|
||||
}
|
||||
|
||||
|
||||
} // namespace autoopt
|
||||
Reference in New Issue
Block a user