计算点云每个点与Z轴的垂直度（附open3d python代码）

用于计算每个点的垂直度
此函数通过查找由周围的点，然后求出此法线和正z轴之间的角度来得到垂直度。
法向量的z分量必须是正值，因此如果是负值，则乘以-1，确保生成的比率为正。
程序使用以下公式：
角度=arccos[（xa*xb+ya*yb+za*zb）/(√（xa^2+ya^2+za^2）*√（xb^2+yb^2+zb^2）]
其中（xa、ya、za）=（法向量的x、y和z分量）和（xb，yb，zb）=（0，0，1）（z轴的矢量）。

 
# coding:utf-8
import open3d as o3d
import numpy as np
from numpy.linalg import norm
from numpy.linalg import svd
 
 
def planeFit(points):
    """
    p, n = planeFit(points)
    Given an array, points, of shape (d,...)
    representing points in d-dimensional space,
    fit an d-dimensional plane to the points.
    Return a point, p, on the plane (the point-cloud centroid),
    and the normal, n.
    """
    points = np.transpose(points)
    points = np.reshape(points, (