79 lines
2.6 KiB
C++
79 lines
2.6 KiB
C++
#include <gtest/gtest.h>
|
|
|
|
#include <autoopt/ellipse.hpp>
|
|
#include <autoopt/optimization_problem.hpp>
|
|
#include <autoopt/util.hpp>
|
|
#include <autoopt/btls.hpp>
|
|
#include <iomanip>
|
|
#include <iostream>
|
|
|
|
using namespace autoopt;
|
|
|
|
TEST(Ellipse, Slope) {
|
|
ellipse<double> e{100, 1000, deg2rad(1.0)}; // entrance angle 1 degree
|
|
quadric<double> q = e.to_quadric();
|
|
|
|
EXPECT_NEAR(q.slope_at(-10), -0.0010305116165301856, 1e-9);
|
|
EXPECT_NEAR(q.slope_at(0), 0.0, 1e-9);
|
|
EXPECT_NEAR(q.slope_at(10), 0.00090001261192696272, 1e-9);
|
|
}
|
|
|
|
TEST(Ellipse, ParamGradient) {
|
|
std::vector<std::pair<double, double>> data_points = {
|
|
{-10.0, -0.00103}, {0.0, 0.0}, {10, 0.0009}};
|
|
|
|
Eigen::VectorX<double> params(4);
|
|
params << 100, 1000, deg2rad(1.0), 0.0;
|
|
|
|
auto loss_func = [&data_points]<typename T>(const Eigen::VectorX<T>& p) {
|
|
ellipse<T> e{T{p(0)}, T{p(1)}, T{p(2)}};
|
|
quadric<T> 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});
|
|
T error = y_pred - T{y_true};
|
|
loss = loss + error * error;
|
|
}
|
|
return loss / T(data_points.size());
|
|
};
|
|
|
|
auto_diff_optimization_problem<double, decltype(loss_func)> problem(loss_func, params);
|
|
|
|
auto grad = problem.gradient(params);
|
|
|
|
std::cout << grad << std::endl;
|
|
|
|
EXPECT_NEAR(grad(0), -3.54845114759293e-12, 1e-15); // d/d(left_arm)
|
|
EXPECT_NEAR(grad(1), -3.0016523630530093e-14, 1e-15); // d/d(right_arm)
|
|
EXPECT_NEAR(grad(2), 2.0569619167404501e-08, 1e-15); // d/d(entrance_angle)
|
|
EXPECT_NEAR(grad(3), -3.3267028547673413e-07, 1e-15); // d/d(rotation_angle)
|
|
|
|
auto hess = problem.hessian(params);
|
|
|
|
std::cout << hess << std::endl;
|
|
|
|
Eigen::VectorX<double> params_delta(4);
|
|
params_delta << 10.0, 10.0, deg2rad(0.1), deg2rad(0.1);
|
|
|
|
|
|
// log barrier
|
|
log_barrier_optimization_problem<double> log_barrier_problem(
|
|
problem,
|
|
params_delta,
|
|
1e-5);
|
|
|
|
while (log_barrier_problem._barrier_strength > 1e-20) {
|
|
btls(log_barrier_problem);
|
|
log_barrier_problem._barrier_strength *= 1e-2;
|
|
}
|
|
|
|
std::cout << "Optimum params:" << std::endl;
|
|
std::cout << "left_arm: " << log_barrier_problem.x()(0) << std::endl;
|
|
std::cout << "right_arm: " << log_barrier_problem.x()(1) << std::endl;
|
|
std::cout << "entrance_angle: " << rad2deg(log_barrier_problem.x()(2)) << std::endl;
|
|
std::cout << "rotation_angle: " << rad2deg(log_barrier_problem.x()(3)) << std::endl;
|
|
std::cout << "Optimum objective:" << std::endl;
|
|
std::cout << problem.objective(problem.x()) << std::endl;
|
|
std::cout << "Optimum grad:" << std::endl;
|
|
std::cout << problem.gradient(problem.x()) << std::endl;
|
|
} |