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]])

Answer 2

You can simply do everything in matrices is one option.

If you add your points as row vectors to a matrix X, and y is a vector, then the parameters vector beta for the least squares solution are:

import numpy as np

beta = np.linalg.inv(X.T.dot(X)).dot(X.T.dot(y))

but there's an easier way, if we want to do projections: QR decomposition gives us an orthonormal projection matrix, as Q.T, and Q is itself the matrix of orthonormal basis vectors. So, we can first form QR, then get beta, then use Q.T to project the points.

QR:

Q, R = np.linalg.qr(X)

beta:

# use R to solve for beta
# R is upper triangular, so can use triangular solver:
beta = scipy.solve_triangular(R, Q.T.dot(y))

So now we have beta, and we can project the points using Q.T very simply:

X_proj = Q.T.dot(X)

Thats it!

If you want more information and graphical piccies and stuff, I made a whole bunch of notes, whilst doing something similar, at: https://github.com/hughperkins/selfstudy-IBP/blob/9dedfbb93f4320ac1bfef60db089ae0dba5e79f6/test_bases.ipynb

(Edit: note that if you want to add a bias term, so the best-fit doesnt have to pass through the origin, you can simply add an additional column, with all-1s, to X, which acts as the bias term/feature)