python计算点到面的距离

已知M个点，记P∈R^(M*3)，目的是求M个点到平面Z= aX + bY + c的距离

解法1: 使用平面的法向量来求解                                    
平面Z= aX + bY + c的法向量为[a,b,-1];

解法2： 使用系数[a,b,c]来求解

• code1

# 加入normal就为法向量[a,b,-1]
def point_plane_dist(normals,pcd):
	max_dist = 0.4
    a1 = normals[0] # a
    a2 = normals[1] # b
    a3 = normals[2] # -1
    normals = normals.reshape(3,1)
    FM = np.sqrt(a1**2 + a2**2+a3**2)
    dists = abs(np.dot(normals,A)+a3)/FM

	# 求点集中于平面距离小于阈值的点集的索引
    ground_idx = (dists <= max_dist).ravel()
    ground_points = pcd[ground_idx]
    pre_cloud = pcd[np.logical_not(ground_idx)]
    # 输出地面背景点和前景点
    return ground_points, pre_cloud

• code2

def point_plane_dist(normals,pcd):
    max_dist = 0.4
    a1 = normals[0]
    a2 = normals[1]
    a3 = normals[2]
    FM = np.sqrt(a1**2 + a2**2+1)
    A = np.array([a1,a2,-1]).reshape(3,1)
    dists = abs(np.dot(pcd,A)+a3)/FM

    ground_idx = (dists <= max_dist).ravel()
    ground_points = pcd[ground_idx]
    pre_cloud = pcd[np.logical_not(ground_idx)]
    return ground_points, pre_cloud

• code2（c++版本代码）

#include<iostream>
#include<vector>

using namespace std;

vector<float> mat(vector<vector<float>> P, vector<float> N)
{
	float c = N[2];
	N[2] = -1;
	float FM = sqrt(pow(N[0], 2.0) + pow(N[1], 2.0) + 1);
	vector<float> res(P.size(), 0);
	for (int i = 0; i < P.size(); i++)
	{
		for (int j = 0; j < N.size(); j++)
			res[i] += P[i][j] * N[j];
		res[i] += c;
		res[i] = res[i] / FM;
	}
	return res;
}

int main()
{
	// 平面Z = aX + bY + c
	vector<float> normals(3, 0);//法向量[a,b,c]
	int N;
	cin >> N;
	vector<vector<float>> points(N, vector<float>(3, 0));
	vector<float> dist = mat(points, normals);
	return 0;

}