3-D Plane Filtering EVD RANSAC... where am I going wrong?

Question

Background

For a computer vision assignment I've been given the task of implementing RANSAC to fit a plane to a given set of points and filter that input list of points by the consensus model using Eigenvalue Decomposition.

I have spent days trying to tweak my code to achieve correct plane filtering behavior on an input set of test data. All you algorithm junkies, this one's for you.

My implementation uses a vector of a ROS data structure (Point32) as inputs, but this is transparent to the problem at hand.

What I've done

When I test for expected plane filtering behavior (correct elimination of outliers >95-99% of the time), I see in my implementation that I only eliminate outliers and extract the main plane of a test point cloud ~30-40% of the time. Other times, I filter a plane that ~somewhat~ fits the expected model, but leaves a lot of obvious outliers inside the consensus model. The fact that this works at all suggests that I'm doing some things right, and some things wrong.

I've tweaked my constants (distance threshold, max iterations, estimated % points fit) to London and back, and I only see small differences in the consensus model.

Implementation (long)

const float RANSAC_ESTIMATED_FIT_POINTS = .80f; // % points estimated to fit the model
const size_t RANSAC_MAX_ITER = 500; // max RANSAC iterations
const size_t RANDOM_MAX_TRIES = 100; // max RANSAC random point tries per iteration
const float RANSAC_THRESHOLD = 0.0000001f; // threshold to determine what constitutes a close point to a plane

/*
  Helper to randomly select an item from a STL container, from stackoverflow.
*/
template <typename I>
I random_element(I begin, I end)
{
    const unsigned long n = std::distance(begin, end);
    const unsigned long divisor = ((long)RAND_MAX + 1) / n;

    unsigned long k;
    do { k = std::rand() / divisor; } while (k >= n);

    std::advance(begin, k);
    return begin;
}

bool run_RANSAC(const std::vector<Point32> all_points, 
    Vector3f *out_p0, Vector3f *out_n,
     std::vector<Point32> *out_inlier_points)
{
    for (size_t iterations = 0; iterations < RANSAC_MAX_ITER; iterations ++)
    {
        Point32 p1,p2,p3;
        Vector3f v1;
        Vector3f v2;

        Vector3f n_hat; // keep track of the current plane model
        Vector3f P0;
        std::vector<Point32> points_agree; // list of points that agree with model within 

        bool found = false;

        // try RANDOM_MAX_TRIES times to get random 3 points
        for (size_t tries = 0; tries < RANDOM_MAX_TRIES; tries ++) // try to get unique random points 100 times
        {
            // get 3 random points 
            p1 = *random_element(all_points.begin(), all_points.end());
            p2 = *random_element(all_points.begin(), all_points.end());
            p3 = *random_element(all_points.begin(), all_points.end());

            v1 = Vector3f (p2.x - p1.x, 
                    p2.y - p1.y,
                    p2.z - p1.z ); //Vector P1P2
            v2 = Vector3f (p3.x - p1.x,
                    p3.y - p1.y,
                    p3.z - p1.z); //Vector P1P3

            if (std::abs(v1.dot(v2)) != 1.f) // dot product != 1 means we've found 3 nonlinear points
            {
                found = true;
                break; 
            }
        } // end try random element loop
        if (!found) // could not find 3 random nonlinear points in 100 tries, go to next iteration
        {
            ROS_ERROR("run_RANSAC(): Could not find 3 random nonlinear points in %ld tries, going on to iteration %ld", RANDOM_MAX_TRIES, iterations + 1);
            continue;
        }

        // nonlinear random points exist past here

        // fit a plane to p1, p2, p3

        Vector3f n = v1.cross(v2); // calculate normal of plane
        n_hat = n / n.norm();
        P0 = Vector3f(p1.x, p1.y, p1.z); 

        // at some point, the original p0, p1, p2 will be iterated over and added to agreed points

        // loop over all points, find points that are inliers to plane
        for (std::vector<Point32>::const_iterator it = all_points.begin(); 
         it != all_points.end(); it++)
        {
            Vector3f M  (it->x - P0.x(),
                         it->y - P0.y(),
                         it->z - P0.z()); // M = (P - P0)

            float d = M.dot(n_hat);  // calculate distance

            if (d <= RANSAC_THRESHOLD)
            {   // add to inlier points list
                points_agree.push_back(*it); 
            }
        } // end points loop 

        ROS_DEBUG("run_RANSAC() POINTS AGREED: %li=%f, RANSAC_ESTIMATED_FIT_POINTS: %f", points_agree.size(), 
                (float) points_agree.size() / all_points.size(), RANSAC_ESTIMATED_FIT_POINTS);
        if (((float) points_agree.size()) / all_points.size() > RANSAC_ESTIMATED_FIT_POINTS)
            {   // if points agree / total points > estimated % points fitting
                // fit to points_agree.size() points

                size_t n = points_agree.size();

                Vector3f sum(0.0f, 0.0f, 0.0f);

                for (std::vector<Point32>::iterator iter = points_agree.begin();
                    iter != points_agree.end(); iter++)
                {
                    sum += Vector3f(iter->x, iter->y, iter->z);
                }

                Vector3f centroid = sum / n; // calculate centroid

                Eigen::MatrixXf M(points_agree.size(), 3);

                for (size_t row = 0; row < points_agree.size(); row++)
                { // build distance vector matrix
                    Vector3f point(points_agree[row].x, 
                                    points_agree[row].y,
                                    points_agree[row].z);

                    for (size_t col = 0; col < 3; col ++)
                    {
                        M(row, col) = point(col) - centroid(col);
                    }
                }

                Matrix3f covariance_matrix = M.transpose() * M;

                Eigen::EigenSolver<Matrix3f> eigen_solver;
                eigen_solver.compute(covariance_matrix);

                Vector3f eigen_values = eigen_solver.eigenvalues().real();

                Matrix3f eigen_vectors = eigen_solver.eigenvectors().real();

                // find eigenvalue that is closest to 0

                size_t idx;

                // find minimum eigenvalue, get index
                float closest_eval = eigen_values.cwiseAbs().minCoeff(&idx);

                // find corresponding eigenvector
                Vector3f closest_evec = eigen_vectors.col(idx);

                std::stringstream logstr;
                logstr << "Closest eigenvalue : " << closest_eval << std::endl <<
                    "Corresponding eigenvector : " << std::endl << closest_evec << std::endl <<
                    "Centroid : " << std::endl << centroid;

                ROS_DEBUG("run_RANSAC(): %s", logstr.str().c_str());

                Vector3f all_fitted_n_hat = closest_evec / closest_evec.norm();

                // invoke copy constructors for outbound 
                *out_n = Vector3f(all_fitted_n_hat);
                *out_p0 = Vector3f(centroid); 
                *out_inlier_points = std::vector<Point32>(points_agree);

                ROS_DEBUG("run_RANSAC():: Success, total_size: %li, inlier_size: %li, %% agreement %f", 
                    all_points.size(), out_inlier_points->size(), (float) out_inlier_points->size() / all_points.size());

                return true;
            }
    } // end iterations loop
    return false;
}

Pseudocode from wikipedia for reference:

Given:
    data – a set of observed data points
    model – a model that can be fitted to data points
    n – minimum number of data points required to fit the model
    k – maximum number of iterations allowed in the algorithm
    t – threshold value to determine when a data point fits a model
    d – number of close data points required to assert that a model fits well to data

Return:
    bestfit – model parameters which best fit the data (or nul if no good model is found)

iterations = 0
bestfit = nul
besterr = something really large
while iterations < k {
    maybeinliers = n randomly selected values from data
    maybemodel = model parameters fitted to maybeinliers
    alsoinliers = empty set
    for every point in data not in maybeinliers {
        if point fits maybemodel with an error smaller than t
             add point to alsoinliers
    }
    if the number of elements in alsoinliers is > d {
        % this implies that we may have found a good model
        % now test how good it is
        bettermodel = model parameters fitted to all points in maybeinliers and alsoinliers
        thiserr = a measure of how well model fits these points
        if thiserr < besterr {
            bestfit = bettermodel
            besterr = thiserr
        }
    }
    increment iterations
}
return bestfit

The only difference between my implementation and the wikipedia pseudocode is the following:

thiserr = a measure of how well model fits these points
if thiserr < besterr {
    bestfit = bettermodel
    besterr = thiserr
}

My guess is that I need to do something related to comparing the (closest_eval) with some sentinel value for the expected minimum eigenvalue corresponding to a normal for planes that tend to fit the model. However this was not covered in class and I have no idea where to start figuring out what's wrong.

Answer 1

Heh, it's funny how thinking about how to present the problem to others can actually solve the problem I'm having.

Solved by simply implementing this with a std::numeric_limits::max() starting best fit eigenvalue. This is because the best fit plane extracted on any n-th iteration of RANSAC is not guaranteed to be THE best fit plane and may have a huge error in consensus amongst each constituent point, so I need to converge on that for each iteration. Woops.

thiserr = a measure of how well model fits these points
if thiserr < besterr {
    bestfit = bettermodel
    besterr = thiserr
}