add py module

tests/ellipse.cpp

@@ -20,7 +20,7 @@ TEST(Ellipse, Slope) {
 
 TEST(Ellipse, ParamGradient) {
   std::vector<std::pair<double, double>> data_points = {
-      {-10.0, -0.001}, {0.0, 0.0}, {10, 0.0009}};
+      {-10.0, -0.00103}, {0.0, 0.0}, {10, 0.0009}};
 
   Eigen::VectorX<double> params(4);
   params << 100, 1000, deg2rad(1.0), 0.0;
@@ -43,30 +43,37 @@ TEST(Ellipse, ParamGradient) {
 
   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)
+  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 << 1.0, 1.0, deg2rad(0.1), deg2rad(0.1);
+  params_delta << 10.0, 10.0, deg2rad(0.1), deg2rad(0.1);
 
-  params(0) += 0.9;  // left_arm
 
   // log barrier
   log_barrier_optimization_problem<double> log_barrier_problem(
       problem,
       params_delta,
-      1e-3);
+      1e-5);
   
-  auto log_barrier_grad = log_barrier_problem.gradient(params);
+  while (log_barrier_problem._barrier_strength > 1e-20) {
+    btls(log_barrier_problem);
+    log_barrier_problem._barrier_strength *= 1e-2;
+  }
 
-  std::cout << "Log barrier gradient:" << std::endl;
-  std::cout << log_barrier_grad << std::endl;
-
-  btls(problem);
+  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;
 }

tests/test_fit.cpp

@@ -0,0 +1,82 @@
+#include <gtest/gtest.h>
+
+#include <autoopt/ellipse.hpp>
+#include <autoopt/optimization_problem.hpp>
+#include <autoopt/util.hpp>
+#include <autoopt/btls.hpp>
+#include <fstream>
+#include <unordered_map>
+#include <autoopt/interface.hpp>
+#include <iomanip>
+
+
+using namespace autoopt;
+
+std::vector<std::pair<double, double>> read_data(const std::string& filename) {
+  std::fstream file(filename, std::ios::in);
+
+  std::unordered_map<double, size_t> index_map;
+  std::vector<double> x_values;
+  std::vector<std::vector<double>> data_points;
+
+  std::string line;
+
+  size_t index = 0;
+
+  double x_avg = 0.0;
+
+  for(;std::getline(file, line);) {
+    if (line.empty() || line[0] == '#') {
+      continue;
+    }
+    std::istringstream iss(line);
+    double x, y;
+    iss >> x;
+    for (size_t i = 1; i < 5; ++i) {
+      iss >> y;  // skip unused columns
+    }
+
+    y = arcsec2rad(-y);
+
+    if (index_map.find(x) == index_map.end()) {
+      index_map[x] = index++;
+      x_values.push_back(x);
+      data_points.emplace_back();
+      data_points.back().push_back(y);
+      x_avg += x;
+      continue;
+    }
+    data_points[index_map[x]].push_back(y);
+  }
+  x_avg /= x_values.size();
+  std::vector<std::pair<double, double>> result;
+  for (size_t i = 40; i < x_values.size() - 40; ++i) {
+    double x = x_values[i];
+    double y_avg = 0.0;
+    for (double y : data_points[i]) {
+      y_avg += y;
+    }
+    y_avg /= data_points[i].size();
+    result.emplace_back(x - x_avg, y_avg);
+  }
+  return result;
+}
+
+TEST(Fit, EllipseFit) {
+  auto data_points = read_data("tests/input/ellipse.dat");
+
+  std::cout << "Read " << data_points.size() << " data points." << std::endl;
+
+  auto mid_point = data_points[data_points.size() / 2];
+
+  std::cout << "Mid point: (" << mid_point.first << ", " << mid_point.second << ")" << std::endl;
+
+  Eigen::VectorX<double> initial_params(4);
+  initial_params << 6900.0, 500.0, deg2rad(2.0), mid_point.second;
+  
+  Eigen::VectorX<double> delta(4);
+  delta << 10.0, 10.0, deg2rad(0.1), deg2rad(0.1);
+
+  auto res = fit_ellipse(data_points, initial_params, delta);
+  std::cout << res.transpose() << std::endl;
+}