如何通过拟合平面找到lidar点云数据intensity导数

source: chatgpt

我是知识的搬运工，尝试弄懂chat老师给我写的代码，我的思考历程是这样的

我跟它说生成一个关于intensity的导数，这是它生成的结果：

def compute_gradient(neighbors_pts, neighbors_int):
    '''
    Fit a plane I(x, y, z) = a*x + b*y + c*z + d
    Coefficent a b c is the gradient along three direction
    '''
    A = np.c_[neighbors_pts, np.ones(neighbors_pts.shape[0])]
    b = neighbors_int
    coeffs,_,_,_ = np.linalg.lstsq(A, b, rcond=None)
    gradient = coeffs[:3]
    return gradient

它的导数是local zone内导数，所以用kdtree寻找了求导点附近的临近点。

附加一个小知识这里的kdtree是确定需要找的临近点的个数k，然后计算距离寻找最近的k个点。

然后取所有邻居点的坐标，每行加了最后为1的列，ok不明白为什么，但是接着向下看，b是临近点对应的intensity， 它用coeffs, _, _, _ = np.linalg.lstsq(A, b, rcond=None)，也就是找到 A×coefficient=b，其实是拟合了一个I(x, y, z) = a*x + b*y + c*z + d的平面，然后它直接取gradient是coeffient。

一开始完全不懂，但是如果我们已经有这个平面方程，对x求导就是coefficient a，对y求导就是coefficient b，常数项在求导的时候直接被抹去了，但是在拟合平面的时候是我们需要的，所以它在点坐标A加了1列1, 对应的就是在求导数时不见的常数项，但是拟合平面的时候十分重要的常数。

kdtree = o3d.geometry.KDTreeFlann(downpcd)
for i in range(len(points)):
    [_,idx,_] = kdtree.search_knn_vector_3d(downpcd.points[i], k)
    neighbors_pts = points[idx,:]
    neighbors_int = intensity[idx]

    gradient = compute_gradient(neighbors_pts, neighbors_int)
    gradients.append(gradient)