void CorrectionAlg::updateFailedCentroid(CorrectionData & c, int counter, double x0, double y0, double x1, double y1, double x2, double y2)
{
    std::vector<int> indices(3);
    // "Sort" the centroids along increasing Y
    if(y0 < y1)
    {
        if(y1 < y2)
            indices = { 0, 1, 2 };
        else // y2 < y1
        {
            if(y0 < y2)
                indices = { 0, 2, 1 };
            else // y2 < y0
                indices = { 2, 0, 1 };
        }
    }
    else // y1 < y0
    {
        if(y0 < y2)
            indices = { 1, 0, 2 };
        else // y2 < y0
        {
            if(y1 < y2)
                indices = { 1, 2, 0 };
            else // y2 < y1
                indices = { 2, 1, 0 };
        }
    }

    std::cout << x0 << ", " << y0 << std::endl;
    std::cout << x1 << ", " << y1 << std::endl;
    std::cout << x2 << ", " << y2 << std::endl;
    std::cout << indices[0] << indices[1] << indices[2] << std::endl;

    Mat centroids(3, 2, CV_32S);
    centroids.at<double>(0, 0) = x0;
    centroids.at<double>(0, 1) = y0;
    centroids.at<double>(1, 0) = x1;
    centroids.at<double>(1, 1) = y1;
    centroids.at<double>(2, 0) = x2;
    centroids.at<double>(2, 1) = y2;
    fillCstruct(c, counter, indices, centroids);
}