Search code examples
c++pseudocodeeigen

How to fit a bounding ellipse around a set of 2D points

Given a set of 2d points (in Cartesian form), I need to find the minimum-area ellipse such that every point in the set lies either on or inside the ellipse.

I have found the solution in the form of pseudo-code on this site, but my attempt to implement the solution in C++ has been unsuccessful.

The following image illustrates graphically what the solution to my problem looks like:

a set of points with a bounding ellipse

In my attempt, I used the Eigen library for the various operations on matrices.

//The tolerance for error in fitting the ellipse
double tolerance = 0.2;
int n = 10; // number of points
int d = 2; // dimension
MatrixXd p = MatrixXd::Random(d,n); //Fill matrix with random points

MatrixXd q = p;
q.conservativeResize(p.rows() + 1, p.cols());

for(size_t i = 0; i < q.cols(); i++)
{
    q(q.rows() - 1, i) = 1;
}

int count = 1;
double err = 1;

const double init_u = 1.0 / (double) n;
MatrixXd u = MatrixXd::Constant(n, 1, init_u);


while(err > tolerance)
{
    MatrixXd Q_tr = q.transpose();
    cout << "1 " << endl;
    MatrixXd X = q * u.asDiagonal() * Q_tr;
    cout << "1a " << endl;
    MatrixXd M = (Q_tr * X.inverse() * q).asDiagonal();
    cout << "1b " << endl;



    int j_x, j_y;
    double maximum = M.maxCoeff(&j_x, &j_y);
    double step_size = (maximum - d - 1) / ((d + 1) * (maximum + 1));

    MatrixXd new_u = (1 - step_size) * u;
    new_u(j_x, 0) += step_size;

    cout << "2 " << endl;

    //Find err
    MatrixXd u_diff = new_u - u;
    for(size_t i = 0; i < u_diff.rows(); i++)
    {
        for(size_t j = 0; j < u_diff.cols(); j++)
            u_diff(i, j) *= u_diff(i, j); // Square each element of the matrix
    }
    err = sqrt(u_diff.sum());
    count++;
    u = new_u;
}

cout << "3 " << endl;
MatrixXd U = u.asDiagonal();
MatrixXd A = (1.0 / (double) d) * (p * U * p.transpose() - (p * u) * (p * u).transpose()).inverse();
MatrixXd c = p * u;

The error occurs on the following line:

MatrixXd M = (Q_tr * X.inverse() * q).asDiagonal();

and it reads as follows:

    run: /usr/include/eigen3/Eigen/src/Core/DenseBase.h:261: void Eigen::DenseBase<Derived>::resize(Eigen::Index, Eigen::Index) [with Derived = Eigen::Diagonal<Eigen::Matrix<double, -1, -1>, 0>; Eigen::Index = long int]: Assertion `rows == this->rows() && cols == this->cols() && "DenseBase::resize() does not actually allow to resize."' failed.
Aborted (core dumped)

Can someone please point out why this error is occurring or even better, give me some advice on how to fit an ellipse to a set of points using C++?

Solution

  • With Eigen, you can get the diagonal vector from a matrix with .diagonal(); you can treat a vector as a diagonal matrix with .asDiagonal(); but you cannot treat a dense matrix as a diagonal matrix. So that line should be 

    MatrixXd M = (Q_tr * X.inverse() * q).diagonal();