83 lines
2.1 KiB
C++
83 lines
2.1 KiB
C++
#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;
|
|
}
|