Fit points to a plane algorithms, how to iterpret results?

Question

Update: I have modified the Optimize and Eigen and Solve methods to reflect changes. All now return the "same" vector allowing for machine precision. I am still stumped on the Eigen method. Specifically How/Why I select slice of the eigenvector does not make sense. It was just trial and error till the normal matched the other solutions. If anyone can correct/explain what I really should do, or why what I have done works I would appreciate it..

Thanks Alexander Kramer, for explaining why I take a slice, only alowed to select one correct answer

I have a depth image. I want to calculate a crude surface normal for a pixel in the depth image. I consider the surrounding pixels, in the simplest case a 3x3 matrix, and fit a plane to these point, and calculate the normal unit vector to this plane.

Sounds easy, but thought best to verify the plane fitting algorithms first. Searching SO and various other sites I see methods using least squares, singlualar value decomposition, eigenvectors/values etc.

Although I don't fully understand the maths I have been able to get the various fragments/example to work. The problem I am having, is that I am getting different answers for each method. I was expecting the various answers would be similar (not exact), but they seem significantly different. Perhaps some methods are not suited to my data, but not sure why I am getting different results. Any ideas why?

Here is the Updated output of the code:

LTSQ:   [ -8.10792259e-17   7.07106781e-01  -7.07106781e-01]
SVD:    [ 0.                0.70710678      -0.70710678]
Eigen:  [ 0.                0.70710678      -0.70710678]
Solve:  [ 0.                0.70710678       0.70710678]
Optim:  [ -1.56069661e-09   7.07106781e-01   7.07106782e-01]

The following code implements five different methods to calculate the surface normal of a plane. The algorithms/code were sourced from various forums on the internet.

import numpy as np
import scipy.optimize

def fitPLaneLTSQ(XYZ):
    # Fits a plane to a point cloud, 
    # Where Z = aX + bY + c        ----Eqn #1
    # Rearanging Eqn1: aX + bY -Z +c =0
    # Gives normal (a,b,-1)
    # Normal = (a,b,-1)
    [rows,cols] = XYZ.shape
    G = np.ones((rows,3))
    G[:,0] = XYZ[:,0]  #X
    G[:,1] = XYZ[:,1]  #Y
    Z = XYZ[:,2]
    (a,b,c),resid,rank,s = np.linalg.lstsq(G,Z) 
    normal = (a,b,-1)
    nn = np.linalg.norm(normal)
    normal = normal / nn
    return normal


def fitPlaneSVD(XYZ):
    [rows,cols] = XYZ.shape
    # Set up constraint equations of the form  AB = 0,
    # where B is a column vector of the plane coefficients
    # in the form b(1)*X + b(2)*Y +b(3)*Z + b(4) = 0.
    p = (np.ones((rows,1)))
    AB = np.hstack([XYZ,p])
    [u, d, v] = np.linalg.svd(AB,0)        
    B = v[3,:];                    # Solution is last column of v.
    nn = np.linalg.norm(B[0:3])
    B = B / nn
    return B[0:3]


def fitPlaneEigen(XYZ):
    # Works, in this case but don't understand!
    average=sum(XYZ)/XYZ.shape[0]
    covariant=np.cov(XYZ - average)
    eigenvalues,eigenvectors = np.linalg.eig(covariant)
    want_max = eigenvectors[:,eigenvalues.argmax()]
    (c,a,b) = want_max[3:6]    # Do not understand! Why 3:6? Why (c,a,b)?
    normal = np.array([a,b,c])
    nn = np.linalg.norm(normal)
    return normal / nn  

def fitPlaneSolve(XYZ):
    X = XYZ[:,0]
    Y = XYZ[:,1]
    Z = XYZ[:,2] 
    npts = len(X)
    A = np.array([ [sum(X*X), sum(X*Y), sum(X)],
                   [sum(X*Y), sum(Y*Y), sum(Y)],
                   [sum(X),   sum(Y), npts] ])
    B = np.array([ [sum(X*Z), sum(Y*Z), sum(Z)] ])
    normal = np.linalg.solve(A,B.T)
    nn = np.linalg.norm(normal)
    normal = normal / nn
    return normal.ravel()

def fitPlaneOptimize(XYZ):
    def residiuals(parameter,f,x,y):
        return [(f[i] - model(parameter,x[i],y[i])) for i in range(len(f))]


    def model(parameter, x, y):
        a, b, c = parameter
        return a*x + b*y + c

    X = XYZ[:,0]
    Y = XYZ[:,1]
    Z = XYZ[:,2]
    p0 = [1., 1.,1.] # initial guess
    result = scipy.optimize.leastsq(residiuals, p0, args=(Z,X,Y))[0]
    normal = result[0:3]
    nn = np.linalg.norm(normal)
    normal = normal / nn
    return normal


if __name__=="__main__":
    XYZ = np.array([
        [0,0,1],
        [0,1,2],
        [0,2,3],
        [1,0,1],
        [1,1,2],
        [1,2,3],
        [2,0,1],
        [2,1,2],
        [2,2,3]
        ])
    print "Solve: ", fitPlaneSolve(XYZ)
    print "Optim: ",fitPlaneOptimize(XYZ)
    print "SVD:   ",fitPlaneSVD(XYZ)
    print "LTSQ:  ",fitPLaneLTSQ(XYZ)
    print "Eigen: ",fitPlaneEigen(XYZ)

Cupronickel

Sorry to be dense. Is XYZ a 2d map of z-values, as in z(x,y) = XYZ[x,y], or a list of points in the form xi, yi, zi = XYZ[i]? My guess would be the list of points based on your code examples, but in your intro you say you have an image, which suggests the map of z-values. – Barone

I 3x3 image patch where the intensities represent the depth value. I transform this to points. Where the x,y are the indices of the corresponding z value. This is because as I understand the algorithms need x,y,z. – Cupronickel

@Michael: In your SVD code, you are using B = v[3,:]; is this correct? When I compile it returns an error saying there is no index 3. I am using 3d vectors, but your data is different? Thanks. – Neighborly

My real data is different, essentially I have a 2D array/map of z-values. In the above code, I simplified it to 9 vectors (so 9x3 array). It has been a while since I have looked at this code. I just cut-n-paste the above and it runs as expected. Hope that helps. – Cupronickel

I'm using fitPlaneSVD and had to change B = v[3,:]; for B = np.array(v[3,:])[0]. First one returns a matrix not a vector – Sostenuto

Answer 1

Optimize

The normal vector of a plane a*x + b*y +c*z = 0, equals (a,b,c)

The optimize method finds a values for a and b such that a*x+b*y~z (~ denotes approximates) It omits to use the value of c in the calculation at all. I don't have numpy installed on this machine but I expect that changing the model to (a*x+b*y)/c should fix this method. It will not give the same result for all data-sets. This method will always assume a plane that goes through the origin.

SVD and LTSQ

produce the same results. (The difference is about the size of machine precision).

Eigen

The wrong eigenvector is chosen. The eigenvector corresponding to the greatest eigenvalue (lambda = 1.50) is x=[0, sqrt(2)/2, sqrt(2)/2] just as in the SVD and LTSQ.

Solve

I have no clue how this is supposed to work.

Kristykristyn

Thanks. I will tweak optimize and eigen. As for "solve" (I probable could have given it a better name) is meant to a "ordinary least squares" take form this SO post: https://mcmap.net/q/400469/-3d-least-squares-plane – Cupronickel

In optimize, changed model to $a*x + b*y + c$ and works. I do not understand how to select the "correct" eigenvector but through some strange array manipulation I end up with the same normal. Now that they are working, I will just use SVD as it seems to preferred in most forums on plane fitting. Thanks again. – Cupronickel

for anyone else with this issue, np.linalg.solve() was also giving me a wrong sign in solution – Apropos

Answer 2

The normal vector of the plane in Eigen solution is the eigenvector for smallest eigenvalue. Some Eigen implementations sort the eigenvalues and eigenvectors some others don't. So in some implementations it's sufficient to take first (or last) eigenvector for normal. In other implementations you have to sort them first. On the other hand the majority of SVD implementations provide sorted values so it's simple first (or last) vector.

Rheims