I pared it down to only needing 3 points.
EllipseParameters extractEllipseParameters(const Eigen::VectorXd& coefficients)
{
//A(i, 0) = x * x;
//A(i, 1) = 0;// x* y;
//A(i, 2) = x * x;
//A(i, 3) = 0;// x;
//A(i, 4) = y;
double a = coefficients(0);
double b = 0;
double c = coefficients(1);
double d = 0;
double e = coefficients(2);
double f = 1;
// Calculate center
double centerX = (2 * c * d - b * e) / (b * b - 4 * a * c);
double centerY = (2 * a * e - b * d) / (b * b - 4 * a * c);
// Calculate rotation angle
double theta = 0.5 * atan2(b, (a - c));
// Calculate semi-axes
double ct = cos(theta);
double st = sin(theta);
double ct2 = ct * ct;
double st2 = st * st;
double a2 = a * ct2 + b * ct * st + c * st2;
double c2 = a * st2 - b * ct * st + c * ct2;
// Calculate constants
double term = 2 * (a * centerX * centerX + b * centerX * centerY +
c * centerY * centerY + d * centerX + e * centerY + f);
double semiMajor = sqrt(abs(term / (2 * std::min(a2, c2))));
double semiMinor = sqrt(abs(term / (2 * std::max(a2, c2))));
if (a2 > c2)
{
cout << "Rotating ellipse" << endl;
std::swap(semiMajor, semiMinor);
theta += pi / 2;
}
EllipseParameters params;
params.centerX = centerX;
params.centerY = centerY;
params.semiMajor = semiMinor;
params.semiMinor = semiMajor;
params.angle = theta;
return params;
}
EllipseParameters fitEllipse(const std::vector<cartesian_point>& points)
{
if (points.size() != 3 ) {
std::cerr << "Error: 3 points are required.\n";
return EllipseParameters();
}
Eigen::MatrixXd A(3, 3);
Eigen::VectorXd b(3);
// Fill the matrix A and vector b with the equations from the points
for (size_t i = 0; i < 3; ++i)
{
double x = points[i].x;
double y = points[i].y;
A(i, 0) = x * x;
A(i, 1) = y * y;
A(i, 2) = y;
b(i) = -1;
}
// Solve for the ellipse parameters
Eigen::VectorXd ellipseParams = A.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV).solve(b);
// Compute center of ellipse
EllipseParameters ep = extractEllipseParameters(ellipseParams);
global_ep.angle = ep.angle;
global_ep.centerX = ep.centerX;
global_ep.centerY = ep.centerY;
global_ep.semiMajor = ep.semiMajor;
global_ep.semiMinor = ep.semiMinor;
return ep;
}