title: Fit points to a plane algorithms, how to iterpret results?
body: <p><strong>Update</strong>: <em>I have modified the Optimize and Eigen and Solve methods to reflect changes.  All now return the "same" vector allowing for machine precision.  <strong>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.</strong>.</em></p>
  
  <p><strong>Thanks</strong> <em>Alexander Kramer, for explaining why I take a slice, only alowed to select one correct answer</em></p>
  
  <p>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.  </p>
  
  <p>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.  </p>
  
  <p>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?</p>
  
  <p>Here is the <strong><em>Updated output</em></strong> of the code:</p>
  
  <pre><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]
  </code></pre>
  
  <p>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. </p>
  
  <pre><code>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)
  </code></pre>