3D Vision--将点云投影至平面

写在前面

1、内容：
如何将空间点投影至一个给定平面
2、环境：
open3d
2、转载请注明出处：
https://blog.csdn.net/qq_41102371/article/details/121482108

几何原理

空间点云投影到一个平面的问题，实际上就是一个点到平面的投影点问题
平面的一般方程为
$$Ax+By+Cz+D=0$$
其法向量为
$$\vec{n}=(A,B,C)$$
现已知平面$\alpha :(A,B,C,D)$以及平面外的一点$P_0(x_0,y_0,z_0)$，求$P_0$到平面的投影点坐标

(图片来自同济高数第七版下册P29)
过$P_0$作垂线$N{P}_0$垂直于平面$\alpha$，$N(x,y,z)$即为$P_0$在$\alpha$上的投影点，$P_1$是平面上的任意一点。
$\because \overrightarrow{N{P}_0}\perp \alpha$
$\therefore \overrightarrow{N{P}_0}\parallel \vec{n}$
根据直线的点向式方程：
$$\frac{x_0-x}{A}=\frac{y_0-y}{B}=\frac{z_0-z}{C}=t$$
则可以得到直线$N{P}_0$的参数方程：
$$\{\begin{array}{l}x=x_0-At\\ y=y_0-Bt\\ z=z_0-Ct\end{array}$$
因为点$N(x,y,z)$在平面$\alpha$上，因此：
$$Ax+By+Cz+D=0$$
$$A(x_0-At)+B(y_0-Bt)+C(z_0-Ct)+D=0$$
$\therefore$ $$t=\frac{A{x}_0+B{y}_0+C{z}_0+D}{A^2+B^2+C^2}$$
将t带入$N{P}_0$的参数方程：
$$\begin{array}{rlr}x& =& x_0-A\frac{A{x}_0+B{y}_0+C{z}_0+D}{A^2+B^2+C^2}\\ & & \\ y& =& y_0-B\frac{A{x}_0+B{y}_0+C{z}_0+D}{A^2+B^2+C^2}\\ & & \\ z& =& z_0-C\frac{A{x}_0+B{y}_0+C{z}_0+D}{A^2+B^2+C^2}\end{array}$$

右侧完全已知，因此，只要知道任意空间点$P_0$的坐标以及给定平面$\alpha$，就能计算出$P_0$在$\alpha$上的投影点坐标$N(x,y,z)$

python代码

def project_points2plane(src_npy, tgt_plane_coefficients):
    """
    Args:
        src_npy: numpy form of a point cloud
        tgt_plane_coefficients: a given plane to project

    Returns:
        numpy form of points that projected to the given plane: n x 3

	# @Author  : Carlos_Lee
	# @Blog    ：https://blog.csdn.net/qq_41102371/article/details/121482108
	# References:
	# https://www.cnblogs.com/nobodyzhou/p/6145030.html
	# https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_plane
    """
    project_plane = copy.deepcopy(src_npy)
    a, b, c, d = tgt_plane_coefficients
    x_i = project_plane[:, 0]
    y_i = project_plane[:, 1]
    z_i = project_plane[:, 2]

    t = (a * x_i + b * y_i + c * z_i) / (a * a + b * b + c * c)
    project_plane[:, 0] = x_i - a * t
    project_plane[:, 1] = y_i - b * t
    project_plane[:, 2] = z_i - c * t
    '''
    if we project a group of points to a plane, 
    some points will be projected to a same point on the plane, resulting in duplicate points,
    we need to remove them
    '''
    # to remove the repeat points
    project_plane = np.unique(project_plane, axis=0)
    return project_plane

完整测试代码在GitHub：
https://github.com/Noel-Gallagher-Highflyingbirds/geometry

References

计算点在平面上的投影坐标: https://www.cnblogs.com/nobodyzhou/p/6145030.html
https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_plane
高等数学（第七版）（下册）．高等教育出版社
https://zhuanlan.zhihu.com/p/267722955

完