gauss method ellipse C++

#include <iostream>
#include <vector>
#include <Eigen/Dense>

using namespace Eigen;

// Function to fit an ellipse using Gauss method
void fitEllipse(const std::vector<double>& x, const std::vector<double>& y) {
    int n = x.size();
    if (n < 5) {
        std::cerr << "Need at least 5 points to fit an ellipse." << std::endl;
        return;
    }

    // Step 1: Construct the design matrix D
    MatrixXd D(n, 6);
    for (int i = 0; i < n; ++i) {
        D(i, 0) = x[i] * x[i];
        D(i, 1) = x[i] * y[i];
        D(i, 2) = y[i] * y[i];
        D(i, 3) = x[i];
        D(i, 4) = y[i];
        D(i, 5) = 1.0;
    }

    // Step 2: Compute the scatter matrix S = D^T * D
    MatrixXd S = D.transpose() * D;

    // Step 3: Define the constraint matrix C
    MatrixXd C(6, 6);
    C << 0, 0, 2, 0, 0, 0,
        0, -1, 0, 0, 0, 0,
        2, 0, 0, 0, 0, 0,
        0, 0, 0, 0, 0, 0,
        0, 0, 0, 0, 0, 0,
        0, 0, 0, 0, 0, 0;

    // Step 4: Solve the generalized eigenvalue problem S * v = λ * C * v
    EigenSolver<MatrixXd> solver(S);
    MatrixXd eigenvalues = solver.eigenvalues().real();
    MatrixXd eigenvectors = solver.eigenvectors().real();

    // Find the solution satisfying the ellipse constraint
    int bestIndex = -1;
    for (int i = 0; i < eigenvalues.size(); ++i) {
        VectorXd v = eigenvectors.col(i);
        if (v.transpose() * C * v > 0) {
            bestIndex = i;
            break;
        }
    }

    if (bestIndex == -1) {
        std::cerr << "No valid ellipse found." << std::endl;
        return;
    }

    // The coefficients of the ellipse equation
    VectorXd ellipseParams = eigenvectors.col(bestIndex);
    std::cout << "Ellipse Parameters: " << ellipseParams.transpose() << std::endl;
}

int main() {
    // Example data points
    std::vector<double> x = { 1, 2, 3, 4, 5 };
    std::vector<double> y = { 2, 3, 4, 5, 6 };

    fitEllipse(x, y);

    return 0;
}