How to estimate local tangent plane for 3d points?

Question

Let's say I have a set of points,

R = [[x1, y1, z1],[x2, y2, z2],...,[xn, yn, zn]]

For each point (p) in R, I have identified a local neighborhood with radius (r) and height (2r) using scipy.cKDTree

import numpy as np
import scipy.spatial

R = np.array(R)
r = 1 # search radius

xy = R[:,0:2] # make array of ONLY xy
tree = scipy.spatial.cKDTree(xy)

for p in range(len(R)):
    2d_nbr_indices = tree.query_ball_point(xy[p], r) # indices w/in xy neighborhood
    2d_nbr_array = R[2d_nbr_indices] # 3d array of 2d neighbors
    z = R[p][1] # get z value
    zMin = z - r
    zMax = z + r
    # Create boolean array to filter 3d array
    hgt_filter = np.any([2d_nbr_array[:, 2] >= zMin,
                         2d_nbr_array[:, 2] <= zMax], axis=0)
    3d_nbr_array = 2d_nbr_array[hgt_filter] # points in xyz neighborhood

I would like to calculate an orthogonal regression plane for each neighborhood, determine the distance (orthogonal) from each point to the plane, and calculate normal vector of plane. Does anyone have any advice on how to go about this in python?

EDIT: I found an odr user guide. It appears to handle 3d points. Any advice on it's implementation and use is welcome. I also found this similar question.

EDIT: I should mention that the data may contain vertical or near vertical surfaces, so an implicit model is necessary. I found this example in the scipy codebook, but only with xy data.

Answer 1

This a general example of how to fit a 3d surface to a point cloud with scipy.odr. Hope it helps.

from scipy.odr import ODR, Model, Data
import numpy as np
import matplotlib.pyplot as plt

from mpl_toolkits.mplot3d import Axes3D

def func(beta,data):
    x,y = data
    a,b,c = beta
    return a*x+b*y+c

N = 20
x = np.random.randn(N)
y = np.random.randn(N)
z = func([-3,-1,2],[x,y])+np.random.normal(size=N)


data = Data([x,y],z)
model = Model(func)
odr = ODR(data, model, [0.0,0.0,0.0])
odr.set_job(fit_type = 0)
res = odr.run()

Y,X = np.mgrid[y.min():y.max():20j,x.min():x.max():20j]
Z = func(res.beta, [X,Y])
f = plt.figure()
pl = f.add_subplot(111,projection='3d')
pl.scatter3D(x,y,z)
pl.plot_surface(X,Y,Z,alpha=0.4)