Fitting a plane to many points in 3D

Let say you have a set of n points in 3D and want to fit a plane to them. In this article I will derive a simple, numerically stable method and give you the source code for it. Sounds like fun? Let's go!

First of all - if you look around the web for an answer you'll get answers that include doing a singular value decomposition on a covariance matrix to find the eigenvector of the smallest eigenvalue. It turns out, however, that this is making things more complicated than they need to be. Let's start with the basics:

A plane is generally described by a normal vector n = [a, b, c] and a distance d so that for point p = [x, y, z] on the plane · p + d = 0. We can write this as:

ax + by + cz + d = 0

Note, however, that this is overdetermined - the solution space (a plane) is three-dimensional, but the above description uses four values. So let’s start off with removing one component by constraining the solution space. We do that by arbitrarily assigning c = 1, i.e. that the z-component of the plane normal is always one (note that the length of the normal does not need to be one). If you think this is a potentially problematic assumption you are right - we shall return to this later. For now, let’s define:

 ax + by + z + d = 0
 
ax + by + d = -z

and solve for a,b,d. In matrix form:

Next we take this matrix, transpose it, and multiply it from the left to perform a linear least squares:

After multiplying in the transpose:

Where N is the number of points. Now here’s the clever part: let us define the x,y,z above to be relative to the centroid (average) of point cloud. Now Σx = Σy = Σz = 0 and so we can simplify to:

And thus, d = 0. This means that if all points are relative to the centroid of the point cloud, then the plane runs through the origin. In other words: the plane always runs through the average of the input points. We can now get rid of a dimension:

Cramer's rule gives us:

D = Σxx*Σyy - Σxy*Σxy
a = (Σyz*Σxy - Σxz*Σyy) / D
b = (Σxy*Σxz - Σxx*Σyz) / D
n = [a, b, 1]

And that's it! But remember our assumption: the z-component of the plane normal is non-zero. What if it is zero? Then it can be shown that the determinant D above becomes zero and we have division by zero. And even if it is not exactly zero, but close, we still get bad conditioning and thus bad results. So what can we do? Well, at least one of the components of the normal must be non-zero if the points do span a plane. So let’s do the above calculations for three separate assumptions of which component is non-zero. Then we simply pick the most well-behaved one, i.e. the one with the largest determinant.

Edit: as the commenter Paul pointed out, this method will minimize the squares of the residuals as perpendicular to the main axis, not the residuals perpendicular to the plane. If the residuals are small (i.e. your points all lie close to the resulting plane), then this method will probably suffice. However, if your points are more spread then this method may not be the best fit.

Here’s the code (in Rust):

// Constructs a plane from a collection of points
// so that the summed squared distance to all points is minimzized
fn plane_from_points(points: &[Vec3]) -> Plane {
    let n = points.len();
    assert!(n >= 3, "At least three points required");

    let mut sum = Vec3{x:0.0, y:0.0, z:0.0};
    for p in points {
        sum = &sum + &p;
    }
    let centroid = &sum * (1.0 / (n as f64));

    // Calc full 3x3 covariance matrix, excluding symmetries:
    let mut xx = 0.0; let mut xy = 0.0; let mut xz = 0.0;
    let mut yy = 0.0; let mut yz = 0.0; let mut zz = 0.0;

    for p in points {
        let r = p - &centroid;
        xx += r.x * r.x;
        xy += r.x * r.y;
        xz += r.x * r.z;
        yy += r.y * r.y;
        yz += r.y * r.z;
        zz += r.z * r.z;
    }

    let det_x = yy*zz - yz*yz;
    let det_y = xx*zz - xz*xz;
    let det_z = xx*yy - xy*xy;

    let det_max = max3(det_x, det_y, det_z);
    assert!(det_max > 0.0, "The points don't span a plane");

    // Pick path with best conditioning:
    let dir =
        if det_max == det_x {
            let a = (xz*yz - xy*zz) / det_x;
            let b = (xy*yz - xz*yy) / det_x;
            Vec3{x: 1.0, y: a, z: b}
        } else if det_max == det_y {
            let a = (yz*xz - xy*zz) / det_y;
            let b = (xy*xz - yz*xx) / det_y;
            Vec3{x: a, y: 1.0, z: b}
        } else {
            let a = (yz*xy - xz*yy) / det_z;
            let b = (xz*xy - yz*xx) / det_z;
            Vec3{x: a, y: b, z: 1.0}
        };

    plane_from_point_and_normal(&centroid, &normalize(dir))
}