Best Fitting Plane given a Set of Points

Solution 1:

Subtract out the centroid, form a $3\times N$ matrix $\mathbf X$ out of the resulting coordinates and calculate its singular value decomposition. The normal vector of the best-fitting plane is the left singular vector corresponding to the least singular value. See this answer for an explanation why this is numerically preferable to calculating the eigenvector of $\mathbf X\mathbf X^\top$ corresponding to the least eigenvalue.

Here's a Python implementation, as requested:

import numpy as np

# generate some random test points 
m = 20 # number of points
delta = 0.01 # size of random displacement
origin = np.random.rand(3, 1) # random origin for the plane
basis = np.random.rand(3, 2) # random basis vectors for the plane
coefficients = np.random.rand(2, m) # random coefficients for points on the plane

# generate random points on the plane and add random displacement
points = basis @ coefficients \
         + np.tile(origin, (1, m)) \
         + delta * np.random.rand(3, m)

# now find the best-fitting plane for the test points

# subtract out the centroid and take the SVD
svd = np.linalg.svd(points - np.mean(points, axis=1, keepdims=True))

# Extract the left singular vectors
left = svd[0]

1 2

# the corresponding left singular vector is the normal vector of the best-fitting plane

left[:, -1]

2

# its dot product with the basis vectors of the plane is approximately zero

left[:, -1] @ basis

2

Solution 2:

A simple least squares solution should do the trick.

The equation for a plane is: $ax + by + c = z$. So set up matrices like this with all your data:

$$ \begin{bmatrix} x_0 & y_0 & 1 \\ x_1 & y_1 & 1 \\ &... \\ x_n & y_n & 1 \\ \end{bmatrix} \begin{bmatrix} a \\ b \\ c \end{bmatrix} = \begin{bmatrix} z_0 \\ z_1 \\ ... \\ z_n \end{bmatrix} $$ In other words:

$$Ax = B$$

Now solve for $x$ which are your coefficients. But since (I assume) you have more than 3 points, the system is over-determined so you need to use the left pseudo inverse: $A^+ = (A^T A)^{-1} A^T$. So the answer is: $$ \begin{bmatrix} a \\ b \\ c \end{bmatrix} = (A^T A)^{-1} A^T B $$

And here is some simple Python code with an example:

import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import numpy as np

# These constants are to create random data for the sake of this example
N_POINTS = 10
TARGET_X_SLOPE = 2
TARGET_y_SLOPE = 3
TARGET_OFFSET  = 5
EXTENTS = 5
NOISE = 5

# Create random data.
# In your solution, you would provide your own xs, ys, and zs data.
xs = [np.random.uniform(2*EXTENTS)-EXTENTS for i in range(N_POINTS)]
ys = [np.random.uniform(2*EXTENTS)-EXTENTS for i in range(N_POINTS)]
zs = []
for i in range(N_POINTS):
    zs.append(xs[i]*TARGET_X_SLOPE + \
              ys[i]*TARGET_y_SLOPE + \
              TARGET_OFFSET + np.random.normal(scale=NOISE))

# plot raw data
plt.figure()
ax = plt.subplot(111, projection='3d')
ax.scatter(xs, ys, zs, color='b')

# do fit
tmp_A = []
tmp_b = []
for i in range(len(xs)):
    tmp_A.append([xs[i], ys[i], 1])
    tmp_b.append(zs[i])
b = np.matrix(tmp_b).T
A = np.matrix(tmp_A)

# Manual solution
fit = (A.T * A).I * A.T * b
errors = b - A * fit
residual = np.linalg.norm(errors)

# Or use Scipy
# from scipy.linalg import lstsq
# fit, residual, rnk, s = lstsq(A, b)

print("solution: %f x + %f y + %f = z" % (fit[0], fit[1], fit[2]))
print("errors: \n", errors)
print("residual:", residual)

# plot plane
xlim = ax.get_xlim()
ylim = ax.get_ylim()
X,Y = np.meshgrid(np.arange(xlim[0], xlim[1]),
                  np.arange(ylim[0], ylim[1]))
Z = np.zeros(X.shape)
for r in range(X.shape[0]):
    for c in range(X.shape[1]):
        Z[r,c] = fit[0] * X[r,c] + fit[1] * Y[r,c] + fit[2]
ax.plot_wireframe(X,Y,Z, color='k')

ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_zlabel('z')
plt.show()

Solution 3:

Considering a plane of equation $Ax+By+Cz=0$ and a point of coordinates $(x_i,y_i,z_i)$, the distance is given by $$d_i=\pm\frac{Ax_i+By_i+Cz_i}{\sqrt{A^2+B^2+C^2}}$$ and I suppose that you want to minimize $$F=\sum_{i=1}^n d_i^2=\sum_{i=1}^n\frac{(Ax_i+By_i+Cz_i)^2}{{A^2+B^2+C^2}}$$ Setting $C=1$, we then need to minimize with respect to $A,B$ $$F=\sum_{i=1}^n\frac{(Ax_i+By_i+z_i)^2}{{A^2+B^2+1}}$$ Taking derivatives $$F'_A=\sum _{i=1}^n \left(\frac{2 x_i (A x_i+B y_i+z_i)}{A^2+B^2+1}-\frac{2 A (A x_i+B y_i+z_i)^2}{\left(A^2+B^2+1\right)^2}\right)$$ $$F'_B=\sum _{i=1}^n \left(\frac{2 y_i (A x_i+B y_i+z_i)}{A^2+B^2+1}-\frac{2 B (A x_i+B y_i+z_i)^2}{\left(A^2+B^2+1\right)^2}\right)$$ Since we shall set these derivatives equal to $0$, the equations can be simplified to $$\sum _{i=1}^n \left({ x_i (A x_i+B y_i+z_i)}-\frac{ A (A x_i+B y_i+z_i)^2}{\left(A^2+B^2+1\right)}\right)=0$$ $$\sum _{i=1}^n \left({ y_i (A x_i+B y_i+z_i)}-\frac{ B (A x_i+B y_i+z_i)^2}{\left(A^2+B^2+1\right)}\right)=0$$ whic are nonlinear with respect to the parameters $A,B$; then, good estimates are required since you will probably use Newton-Raphson for polishing the solutions.

These can be obtained making first a multilinear regression (with no intercept in your cas) $$z=\alpha x+\beta y$$ and use $A=-\alpha$ and $B=-\beta$ for starting the iterative process. The values are given by $$A=-\frac{\text{Sxy} \,\text{Syz}-\text{Sxz}\, \text{Syy}}{\text{Sxy}^2-\text{Sxx}\, \text{Syy}}\qquad B=-\frac{\text{Sxy}\, \text{Sxz}-\text{Sxx}\, \text{Syz}}{\text{Sxy}^2-\text{Sxx}\, \text{Syy}}$$

For illustration purposes, let me consider the following data $$\left( \begin{array}{ccc} x & y & z \\ 1 & 1 & 9 \\ 1 & 2 & 14 \\ 1 & 3 & 20 \\ 2 & 1 & 11 \\ 2 & 2 & 17 \\ 2 & 3 & 23 \\ 3 & 1 & 15 \\ 3 & 2 & 20 \\ 3 & 3 & 26 \end{array} \right)$$

The preliminary step gives $z=2.97436 x+5.64103 y$ and solving the rigorous equations converges to $A=-2.97075$, $B=-5.64702$ which are quite close to the estimates (because of small errors).