3D Least Squares Plane

Question

What\'s the algorithm for computing a least squares plane in (x, y, z) space, given a set of 3D data points? In other words, if I had a bunch of points like (1, 2, 3), (4, 5

Answer 1

It sounds like all you want to do is linear regression with 2 regressors. The wikipedia page on the subject should tell you all you need to know and then some.

Answer 2

See 'Least Squares Fitting of Data' by David Eberly for how I came up with this one to minimize the geometric fit (orthogonal distance from points to the plane).

bool Geom_utils::Fit_plane_direct(const arma::mat& pts_in, Plane& plane_out)
{
    bool success(false);
    int K(pts_in.n_cols);
    if(pts_in.n_rows == 3 && K > 2)  // check for bad sizing and indeterminate case
    {
        plane_out._p_3 = (1.0/static_cast<double>(K))*arma::sum(pts_in,1);
        arma::mat A(pts_in);
        A.each_col() -= plane_out._p_3; //[x1-p, x2-p, ..., xk-p]
        arma::mat33 M(A*A.t());
        arma::vec3 D;
        arma::mat33 V;
        if(arma::eig_sym(D,V,M))
        {
            // diagonalization succeeded
            plane_out._n_3 = V.col(0); // in ascending order by default
            if(plane_out._n_3(2) < 0)
            {
                plane_out._n_3 = -plane_out._n_3; // upward pointing
            }
            success = true;
        }
    }
    return success;
}

Timed at 37 micro seconds fitting a plane to 1000 points (Windows 7, i7, 32bit program)

Answer 3

All you'll have to do is to solve the system of equations.

If those are your points: (1, 2, 3), (4, 5, 6), (7, 8, 9)

That gives you the equations:

3=a*1 + b*2 + c
6=a*4 + b*5 + c
9=a*7 + b*8 + c

So your question actually should be: How do I solve a system of equations?

Therefore I recommend reading this SO question.

If I've misunderstood your question let us know.

EDIT:

Ignore my answer as you probably meant something else.

Answer 4

unless someone tells me how to type equations here, let me just write down the final computations you have to do:

first, given points r_i \n \R, i=1..N, calculate the center of mass of all points:

r_G = \frac{\sum_{i=1}^N r_i}{N}

then, calculate the normal vector n, that together with the base vector r_G defines the plane by calculating the 3x3 matrix A as

A = \sum_{i=1}^N (r_i - r_G)(r_i - r_G)^T

with this matrix, the normal vector n is now given by the eigenvector of A corresponding to the minimal eigenvalue of A.

To find out about the eigenvector/eigenvalue pairs, use any linear algebra library of your choice.

This solution is based on the Rayleight-Ritz Theorem for the Hermitian matrix A.

Answer 5

This reduces to the Total Least Squares problem, that can be solved using SVD decomposition.

C++ code using OpenCV:

float fitPlaneToSetOfPoints(const std::vector<cv::Point3f> &pts, cv::Point3f &p0, cv::Vec3f &nml) {
    const int SCALAR_TYPE = CV_32F;
    typedef float ScalarType;

    // Calculate centroid
    p0 = cv::Point3f(0,0,0);
    for (int i = 0; i < pts.size(); ++i)
        p0 = p0 + conv<cv::Vec3f>(pts[i]);
    p0 *= 1.0/pts.size();

    // Compose data matrix subtracting the centroid from each point
    cv::Mat Q(pts.size(), 3, SCALAR_TYPE);
    for (int i = 0; i < pts.size(); ++i) {
        Q.at<ScalarType>(i,0) = pts[i].x - p0.x;
        Q.at<ScalarType>(i,1) = pts[i].y - p0.y;
        Q.at<ScalarType>(i,2) = pts[i].z - p0.z;
    }

    // Compute SVD decomposition and the Total Least Squares solution, which is the eigenvector corresponding to the least eigenvalue
    cv::SVD svd(Q, cv::SVD::MODIFY_A|cv::SVD::FULL_UV);
    nml = svd.vt.row(2);

    // Calculate the actual RMS error
    float err = 0;
    for (int i = 0; i < pts.size(); ++i)
        err += powf(nml.dot(pts[i] - p0), 2);
    err = sqrtf(err / pts.size());

    return err;
}

Answer 6

As with any least-squares approach, you proceed like this:

Before you start coding

1. Write down an equation for a plane in some parameterization, say 0 = ax + by + z + d in thee parameters (a, b, d).

2. Find an expression D(\vec{v};a, b, d) for the distance from an arbitrary point \vec{v}.

3. Write down the sum S = \sigma_i=0,n D^2(\vec{x}_i), and simplify until it is expressed in terms of simple sums of the components of v like \sigma v_x, \sigma v_y^2, \sigma v_x*v_z ...

4. Write down the per parameter minimization expressions dS/dx_0 = 0, dS/dy_0 = 0 ... which gives you a set of three equations in three parameters and the sums from the previous step.

5. Solve this set of equations for the parameters.

(or for simple cases, just look up the form). Using a symbolic algebra package (like Mathematica) could make you life much easier.

The coding

• Write code to form the needed sums and find the parameters from the last set above.

Alternatives

Note that if you actually had only three points, you'd be better just finding the plane that goes through them.

Also, if the analytic solution in unfeasible (not the case for a plane, but possible in general) you can do steps 1 and 2, and use a Monte Carlo minimizer on the sum in step 3.