三维空间点线面解析

最新推荐文章于 2022-10-08 10:53:24 发布

原创

最新推荐文章于 2022-10-08 10:53:24 发布 · 3k 阅读

9 ·

CC 4.0 BY-SA版权

文章标签：

#线性代数 #算法 #点面

本文详细探讨了三维空间中点、线、面的表示方法，并介绍了如何计算它们之间的关系，如向量夹角、平行与垂直判断、共面性、距离计算以及交点查找。此外，还提供了相应的Python函数实现，适用于室内场景重建等领域的几何计算。

简介

之前写了3篇室内场景重建的博客, 只是简单介绍了一下方法, 并没有对点线面的具体计算做讨论.

这篇补个坑, 用作交流学习之用.

基元类型及表示

本篇将重点讨论, 三维空间内, 如下三个基元(Primitive)的代数和几何关系

点(Point), 用一个1x3的行向量 $[x, y, z]$ 表示, 分别表示点的三个坐标
线(Line), 用一个2x3的矩阵 $x_0, y_0, z_0], [A, B, C]]$ , 表示, 其中 $x_0, y_0, z_0]$ 为该直线上的一个点, $[A, B, C]$ 为该直线的方向(不区分正负方向). 该直线的方程如下:
$\large \frac{x-x_0}{A} = \frac{y-y_0}{B} = \frac{z-z_0}{C}$
面(Plane), 用一个1x4的行向量 $[A, B, C, - 1]$ 表示, 其中 $[A, B, C]$ 为该平面的法向量(不区分正负方向). 该平面的方程如下:

本篇指的所有的面, 都是指矩形, 不是不规则的平面. 毕竟室内场景中, 绝大部分的面都是矩形.
$\large Ax + By + Cz -1 = 0$

本篇思想和原则

在计算关系的时候, 将上述三种基元统一表示成同格式的向量来计算
尽可能多使用numpy封装的API, 避免for循环. 一方面是考虑到代码的美观, 另一方面是numpy经过底层加速, 速度会比for循环快不少.

关系求解

至此我们定义了3种基元的表示, 但是可以看到这三种表示是不统一的.

因此我们在计算关系的时候, 采用的最重要的思路就是将他们统一表示成同格式的向量来计算

向量的夹角

为了方便后续计算, 我们先定义求解任意两个向量夹角的函数cosine_between_vectors和arccosine两个函数

两个向量夹角公式各位应该特别熟悉, 我就贴一下不做进一步的证明了
$\large cosine(\alpha, \beta) = \frac{\alpha * \beta}{||\alpha||*||\beta||}$

def arccosine(cos, dtype='degree'):
    """
    返回一个角度
    @param cos:
    @param dtype:
    @return:        np.float
    """
    assert dtype in ['rad', 'degree'], 'Wrong value of param {dtype}'
    angle = np.arccos(cos)
    if 'degree' == dtype:
        return np.degrees(angle)
    return angle


def cosine_between_vectors(vec_1, vec_2):
    """
    两个向量之间的夹角余弦
    @param vec_1:
    @param vec_2:
    @return:             float
    """

    if not type(vec_1) == np.array:
        vec_1 = np.array(vec_1)
    assert 1 == len(vec_1.shape), 'Wrong shape of param {vec_1}'	# 向量1必须是1xM的形状

    if not type(vec_2) == np.array:
        vec_2 = np.array(vec_2)
    assert 1 == len(vec_2.shape), 'Wrong shape of param {vec_2}'	# 向量2必须是1xM的形状

    assert vec_1.shape[0] == vec_2.shape[0], '{vec_1} is not the same shape with {vec_2}'

    normal_1_len = np.linalg.norm(vec_1)	# 向量1的模
    normal_2_len = np.linalg.norm(vec_2)	# 向量2的模

    return vec_1.dot(vec_2) / (normal_1_len * normal_2_len)

向量平行或垂直

有了上面求向量夹角的函数, 可以判断两个向量是否平行或垂直.

注意:

由于本篇中忽略了向量的正负方向, 因此即使夹角为180°的两个向量, 依然认为是平行的
实际应用中, 激光扫描得到的点会有误差, 在编程判断的时候需要体现出来这个误差

因此思路应该是, 给定两个向量 $vec_1, vec_2$ , 需要计算两者的夹角余弦的绝对值, 如果绝对值非常接近1, 说明两者平行(再次强调本文所有方向都忽略正反), 如果非常接近0, 说明两者垂直.

这里我们定义两个阈值COSINE_PARALLEL_THRESHOLD和COSINE_VERTICAL_THRESHOLD.

那么进一步封装, 本篇处理的不仅仅是向量, 而是3类基元(点, 线, 面). 除了点不存在平行垂直的概念, 剩下的向量, 直线, 平面都可以两两组合判断平行或者垂直. 一共有向量和向量, 向量和直线, 向量和平面, 直线和直线, 直线和平面, 平面和平面六种组合, 其中除了向量和向量可以直接做计算, 剩下的情况都需要做一点处理才能计算.

做处理的思路也很简单, 将参与计算的两个基元, 转化成两个向量做计算.

直线

原则上直线也是一种向量, 因此这种情况和上面向量和向量的流程是一样的, 唯一需要注意的是, 向量是一个行向量, 而本篇中的直线是一个矩阵, 不能直接将行向量和矩阵做处理, 应该将直线的方向向量, 也就是矩阵的第二行拿出来, 这就转化成了两个向量

平面

同样, 考虑将平面转化成一个向量参与计算, 那么首先想到的应该是平面的法向量

如果这个向量和平面平行, 那么这个向量应该垂直于平面的法向量
反过来, 如果向量和平面垂直, 那么向量和法向量应该平行.

这里唯一需要注意的是, 涉及到平面的垂直和平行的关系会变化.

那么我们很容易得到下面的is_parallel()和is_vertical()两个函数:

COSINE_PARALLEL_THRESHOLD = 0.999       # 两条向量夹角的cos值的绝对值超过这个值即认为这两条向量平行
COSINE_VERTICAL_THRESHOLD = 0.001       # 两条向量夹角的cos值的绝对值小于这个值即认为这两条向量垂直

def is_parallel(primitive_1, primitive_2, dtype=None):
    """
    根据两个基元的类型, 判断两个基元是否平行
    @param primitive_1:
    @param primitive_2:
    @param dtype:       表示基元1和基元2的类型, 由以下表示
                            - 'v' :     表示向量, 由向量的方向参数[x, y, z]表示
                            - 'l' :     表示直线, 由直线的方程参数[[x0, y0, z0], [A, B, C]]表示
                            - 'p' :     表示平面, 由平面的方程参数[A, B, C, -1]表示
                        可按下两两组合
                            组合\意义        基元1         基元2
                            - 'vv' :        向量          向量
                            - 'vl' :        向量          直线
                            - 'vp' :        向量          平面
                            - 'll' :        直线          直线
                            - 'lp' :        直线          平面
                            - 'pp' :        平面          平面
    @return:            bool
    """
    if not type(primitive_1) == np.array:
        primitive_1 = np.array(primitive_1)

    if not type(primitive_2) == np.array:
        primitive_2 = np.array(primitive_2)

    assert dtype in ['vv', 'vl', 'vp', 'll', 'lp', 'pp'], 'wrong value of param {dtype}'

    if 'vv' == dtype:
        cos = cosine_between_vectors(primitive_1, primitive_2)
        return np.abs(cos) > COSINE_PARALLEL_THRESHOLD
    elif 'vl' == dtype:
        vec = primitive_2[1]
        cos = cosine_between_vectors(primitive_1, vec)
        return np.abs(cos) > COSINE_PARALLEL_THRESHOLD
    elif 'vp' == dtype:
        """
        由于平面的法向量垂直于平面, 所以若向量与平面平行, 向量应该垂直于法向量
        """
        normal_plane = normal_of_plane(primitive_2)
        cos = cosine_between_vectors(primitive_1, normal_plane)
        return np.abs(cos) < COSINE_VERTICAL_THRESHOLD
    elif 'll' == dtype:
        vec_1, vec_2 = primitive_1[1], primitive_2[1]
        cos = cosine_between_vectors(vec_1, vec_2)
        return np.abs(cos) > COSINE_PARALLEL_THRESHOLD
    elif 'lp' == dtype:
        """
        由于平面的法向量垂直于平面, 所以若线与平面平行, 线应该垂直于法向量
        """
        vec = primitive_1[1]
        normal_plane = normal_of_plane(primitive_2)
        cos = cosine_between_vectors(vec, normal_plane)
        return np.abs(cos) < COSINE_VERTICAL_THRESHOLD
    elif 'pp' == dtype:
        """
        由于平面的法向量垂直于平面, 所以若两个平面平行, 则两条法线也平行
        """
        normal_plane_1 = normal_of_plane(primitive_1)
        normal_plane_2 = normal_of_plane(primitive_2)
        cos = cosine_between_vectors(normal_plane_1, normal_plane_2)
        return np.abs(cos) > COSINE_PARALLEL_THRESHOLD


def is_vertical(primitive_1, primitive_2, dtype):
    """
    根据两个基元的类型, 判断两个基元是否垂直
    @param primitive_1:
    @param primitive_2:
    @param dtype:       表示基元1和基元2的类型, 由以下表示
                            - 'v' :     表示向量, 由向量的方向参数[x, y, z]表示
                            - 'l' :     表示直线, 由直线的方程参数[[x0, y0, z0], [A, B, C]]表示
                            - 'p' :     表示平面, 由平面的方程参数[A, B, C, -1]表示
                        可按下两两组合
                            组合\意义        基元1         基元2
                            - 'vv' :        向量          向量
                            - 'vl' :        向量          直线
                            - 'vp' :        向量          平面
                            - 'll' :        直线          直线
                            - 'lp' :        直线          平面
                            - 'pp' :        平面          平面
    @return:            bool
    """

    if not type(primitive_1) == np.array:
        primitive_1 = np.array(primitive_1)

    if not type(primitive_2) == np.array:
        primitive_2 = np.array(primitive_2)

    assert dtype in ['vv', 'vl', 'vp', 'll', 'lp', 'pp'], 'wrong value of param {dtype}'

    if 'vv' == dtype:
        cos = cosine_between_vectors(primitive_1, primitive_2)
        return np.abs(cos) < COSINE_VERTICAL_THRESHOLD
    elif 'vl' == dtype:
        vec = primitive_2[1]
        cos = cosine_between_vectors(primitive_1, vec)
        return np.abs(cos) < COSINE_VERTICAL_THRESHOLD
    elif 'vp' == dtype:
        """
        由于平面的法向量垂直于平面, 所以若向量与平面垂直, 向量应该平行于法向量
        """
        normal_plane = normal_of_plane(primitive_2)
        cos = cosine_between_vectors(primitive_1, normal_plane)
        return np.abs(cos) > COSINE_PARALLEL_THRESHOLD
    elif 'll' == dtype:
        vec_1, vec_2 = primitive_1[1], primitive_2[1]
        cos = cosine_between_vectors(vec_1, vec_2)
        return np.abs(cos) < COSINE_VERTICAL_THRESHOLD
    elif 'lp' == dtype:
        """
        由于平面的法向量垂直于平面, 所以若线与平面垂直, 线应该平行于法向量
        """
        vec = primitive_1[1]
        normal_plane = normal_of_plane(primitive_2)
        cos = cosine_between_vectors(vec, normal_plane)
        return np.abs(cos) > COSINE_PARALLEL_THRESHOLD
    elif 'pp' == dtype:
        """
        由于平面的法向量垂直于平面, 所以若两个平面垂直, 则两条法线也垂直
        """
        normal_plane_1 = normal_of_plane(primitive_1)
        normal_plane_2 = normal_of_plane(primitive_2)
        cos = cosine_between_vectors(normal_plane_1, normal_plane_2)
        return np.abs(cos) < COSINE_VERTICAL_THRESHOLD