python - orthogonal projection with numpy

Question

I have a list of 3D-points for which I calculate a plane by numpy.linalg.lstsq - method. But Now I want to do a orthogonal projection for each point into this plane, but I can't find my mistake:

from numpy.linalg import lstsq

def VecProduct(vek1, vek2):
    return (vek1[0]*vek2[0] + vek1[1]*vek2[1] + vek1[2]*vek2[2])

def CalcPlane(x, y, z):
    # x, y and z are given in lists
    n = len(x)
    sum_x = sum_y = sum_z = sum_xx = sum_yy = sum_xy = sum_xz = sum_yz = 0
    for i in range(n):
        sum_x += x[i] 
        sum_y += y[i]
        sum_z += z[i]
        sum_xx += x[i]*x[i]
        sum_yy += y[i]*y[i]
        sum_xy += x[i]*y[i]
        sum_xz += x[i]*z[i]
        sum_yz += y[i]*z[i]

    M = ([sum_xx, sum_xy, sum_x], [sum_xy, sum_yy, sum_y], [sum_x, sum_y, n])
    b = (sum_xz, sum_yz, sum_z)

    a,b,c = lstsq(M, b)[0]

    '''
    z = a*x + b*y + c
    a*x = z - b*y - c
    x = -(b/a)*y + (1/a)*z - c/a 
    '''

    r0 = [-c/a, 
          0, 
          0]

    u = [-b/a,
         1,
         0]

    v = [1/a,
         0,
         1]

    xn = []
    yn = []
    zn = []

    # orthogonalize u and v with Gram-Schmidt to get u and w

    uu = VecProduct(u, u)
    vu = VecProduct(v, u)
    fak0 = vu/uu
    erg0 = [val*fak0 for val in u]
    w = [v[0]-erg0[0], 
        v[1]-erg0[1], 
        v[2]-erg0[2]]
    ww = VecProduct(w, w)

    # P_new = ((x*u)/(u*u))*u + ((x*w)/(w*w))*w
    for i in range(len(x)):
        xu = VecProduct([x[i], y[i], z[i]], u)
        xw = VecProduct([x[i], y[i], z[i]], w)
        fak1 = xu/uu
        fak2 = xw/ww
        erg1 = [val*fak1 for val in u]
        erg2 = [val*fak2 for val in w]
        erg = [erg1[0]+erg2[0], erg1[1]+erg2[1], erg1[2]+erg2[2]]
        erg[0] += r0[0] 
        xn.append(erg[0])
        yn.append(erg[1])
        zn.append(erg[2])

    return (xn,yn,zn)

This returns me a list of points which are all in a plane, but when I display them, they are not at the positions they should be. I believe there is already a certain built-in method to solve this problem, but I couldn't find any =(

Answer 1

You are doing a very poor use of np.lstsq, since you are feeding it a precomputed 3x3 matrix, instead of letting it do the job. I would do it like this:

import numpy as np

def calc_plane(x, y, z):
    a = np.column_stack((x, y, np.ones_like(x)))
    return np.linalg.lstsq(a, z)[0]

>>> x = np.random.rand(1000)
>>> y = np.random.rand(1000)
>>> z = 4*x + 5*y + 7 + np.random.rand(1000)*.1
>>> calc_plane(x, y, z)
array([ 3.99795126,  5.00233364,  7.05007326])

It is actually more convenient to use a formula for your plane that doesn't depend on the coefficient of z not being zero, i.e. use a*x + b*y + c*z = 1. You can similarly compute a, b and c doing:

def calc_plane_bis(x, y, z):
    a = np.column_stack((x, y, z))
    return np.linalg.lstsq(a, np.ones_like(x))[0]
>>> calc_plane_bis(x, y, z)
array([-0.56732299, -0.70949543,  0.14185393])

To project points onto a plane, using my alternative equation, the vector (a, b, c) is perpendicular to the plane. It is easy to check that the point (a, b, c) / (a**2+b**2+c**2) is on the plane, so projection can be done by referencing all points to that point on the plane, projecting the points onto the normal vector, subtract that projection from the points, then referencing them back to the origin. You could do that as follows:

def project_points(x, y, z, a, b, c):
    """
    Projects the points with coordinates x, y, z onto the plane
    defined by a*x + b*y + c*z = 1
    """
    vector_norm = a*a + b*b + c*c
    normal_vector = np.array([a, b, c]) / np.sqrt(vector_norm)
    point_in_plane = np.array([a, b, c]) / vector_norm

    points = np.column_stack((x, y, z))
    points_from_point_in_plane = points - point_in_plane
    proj_onto_normal_vector = np.dot(points_from_point_in_plane,
                                     normal_vector)
    proj_onto_plane = (points_from_point_in_plane -
                       proj_onto_normal_vector[:, None]*normal_vector)

    return point_in_plane + proj_onto_plane

So now you can do something like:

>>> project_points(x, y, z, *calc_plane_bis(x, y, z))
array([[  0.13138012,   0.76009389,  11.37555123],
       [  0.71096929,   0.68711773,  13.32843506],
       [  0.14889398,   0.74404116,  11.36534936],
       ..., 
       [  0.85975642,   0.4827624 ,  12.90197969],
       [  0.48364383,   0.2963717 ,  10.46636903],
       [  0.81596472,   0.45273681,  12.57679188]])