CN117788529B - 三维平面点云粗配准方法、***、介质及设备 - Google Patents

Abstract

一种三维平面点云粗配准方法、***、介质及设备，涉及机器人技术领域。所述方法包括：获取第一平面的第一法向量，以及第二平面的第二法向量，所述第一平面和所述第二平面是需要进行配准的两个不同三维平面；根据所述第一法向量以及所述第二法向量计算目标旋转矩阵；基于所述目标旋转矩阵将所述第一平面进行旋转变换，得到旋转后的第一平面，以使所述旋转后的第一平面与所述第二平面平行；根据所述旋转后的第一平面的平面点云参数计算目标平移向量；将所述旋转后的第一平面按照所述第一法向量的方向平移所述目标平移向量，以使所述第一平面和所述第二平面重合。实施本申请提供的技术方案，可以快速进行三维平面的粗配准。

Description

三维平面点云粗配准方法、***、介质及设备

技术领域

本申请涉及机器人技术领域，具体涉及一种三维平面点云粗配准方法、***、介质及设备。

背景技术

随着科技的进步，机器人技术在近些年得到快速发展。并且随着处理器性能增强和各种消费级传感器的出现，移动机器人已经成功进军到物流、仓储、服务等行业。点云配准一直是移动机器人领域所重点关注的课题，例如有些机器人定位与建图、语义识别等技术会需要点云配准作为基础。点云配准的主要目的是将两个点云数据合并到同一坐标系下，通常分为粗配准和细配准，粗配准是利用点云特征进行快速初步配准，为后续的细配准提供一个大致的初始位姿。

传统的迭代最近点算法（ICP）在点云粗配准中具有精度高的特点，因此广泛应用于移动机器人建图与定位。而对于某些纯平面粗配准问题，如园区环境扫描到地面的平面粗配准，使用ICP进行平面粗配准容易出现发散问题，且陷入局部优化无法取得全局最优解，导致平面的粗配准的效率较低，从而影响机器人的定位与建图效果。

发明内容

本申请提供了一种三维平面点云粗配准方法、***、介质及设备，可以快速进行三维平面的粗配准，提高平面的粗配准效率，从而有效提高机器人的定位与建图效果。

第一方面，本申请提供了一种三维平面点云粗配准方法，所述方法包括：

获取需要进行粗配准的第一平面的第一法向量，以及第二平面的第二法向量；

根据所述第一法向量以及所述第二法向量计算目标旋转矩阵；

基于所述目标旋转矩阵将所述第一平面进行旋转变换，得到旋转后的第一平面；

根据所述旋转后的第一平面的平面点云参数计算目标平移向量；

基于所述第一法向量的方向和所述目标平移向量，平移所述旋转后的第一平面。

通过采用上述技术方案，提取两个平面点云的法向量信息，并构建旋转矩阵与平移向量实现两平面的配准，无需现有技术ICP算法中建立点与点的最近邻关系进行迭代配准，也避免了因平面特征单一导致的易发散和陷入局部最优的问题，通过合理利用两平面点云的法向量信息，实现了平面点云的快速旋转对齐和平移重合，达到准确、高效的平面点云粗配准，提高了平面的粗配准效率，从而提高机器人的定位与建图效果。

可选的，所述获取需要进行粗配准的第一平面的第一法向量，以及第二平面的第二法向量，包括：获取第一平面的第一点云数据和第二平面的第二点云数据；基于随机采样一致性算法计算所述第一点云数据在欧式坐标系下的第一平面方程，并确定所述第一平面方程对应的第一法向量；基于所述随机采样一致性算法计算所述第二点云数据在欧式坐标系下的第二平面方程，并确定所述第二平面方程对应的第二法向量。

通过采用上述技术方案，随机采样一致性算法分别拟合计算两平面点云数据的平面模型，以获取两法向量，该算法通过迭代排除误差较大的离群点，可有效抑制离群点对平面拟合的干扰，确保法向量准确表示平面法线方向。

可选的，所述根据所述第一法向量以及所述第二法向量计算目标旋转矩阵，包括：根据所述第一法向量计算第一X轴旋转角和第一Y轴旋转角；根据所述第二法向量计算第二X轴旋转角和第二Y轴旋转角；基于所述第一X轴旋转角、所述第一Y轴旋转角、所述第二X轴旋转角以及所述第二Y轴旋转角，得到目标旋转矩阵。

通过采用上述技术方案，通过解析两法向量在坐标轴方向上的投影分量，分别计算两平面需要旋转的角度，然后基于这四个旋转角构建目标旋转矩阵。相比直接使用法向量构建变换，该方法充分利用了法向量在各轴上的分量信息，可准确提取两平面点云所需的旋转变换参数，有利于构建精确表达两平面旋转关系的目标矩阵。

可选的，所述基于所述第一X轴旋转角、所述第一Y轴旋转角、所述第二X轴旋转角以及所述第二Y轴旋转角，得到目标旋转矩阵，包括：将所述第一平面绕世界欧式坐标系的X轴方向旋转所述第一X轴旋转角，得到第一旋转矩阵，将所述第一旋转矩阵绕所述世界欧式坐标系的Y轴方向旋转所述第一Y轴旋转角，得到第二旋转矩阵；将所述第二平面绕所述世界欧式坐标系的X轴方向旋转所述第二X轴旋转角，得到第三旋转矩阵，将所述第三旋转矩阵绕所述世界欧式坐标系的Y轴方向旋转所述第二Y轴旋转角，得到第四旋转矩阵；基于所述第一旋转矩阵、所述第二旋转矩阵、所述第三旋转矩阵以及所述第四旋转矩阵，得到目标旋转矩阵。

通过采用上述技术方案，通过分步计算四个表示两平面旋转的单轴旋转矩阵，并将它们组合获得目标旋转矩阵。该方法充分利用四个旋转角，分别构建平面在X轴和Y轴方向的旋转矩阵，然后将两平面的旋转矩阵合成目标矩阵，能够准确表达两平面在不同坐标轴上的旋转关系，有助于构建精确的目标旋转矩阵。

可选的，所述基于所述第一旋转矩阵、所述第二旋转矩阵、所述第三旋转矩阵以及所述第四旋转矩阵，得到目标旋转矩阵，包括：将所述第一旋转矩阵与所述第二旋转矩阵相乘，得到所述第一平面对应的第一对合矩阵；将所述第三旋转矩阵与所述第四旋转矩阵相乘，得到所述第二平面对应的第二对合矩阵；将所述第一对合矩阵和所述第二对合矩阵相乘，得到目标旋转矩阵。

通过采用上述技术方案，通过分别计算两平面在X轴和Y轴旋转矩阵的矩阵乘积，获得第一对合矩阵和第二对合矩阵，然后将两对合矩阵相乘，可有效构建出融合两平面旋转信息的目标旋转矩阵。该具体实现充分发挥矩阵运算的优势，将表示单轴旋转的矩阵进行层层合成，可高效计算准确表达两平面配准关系的目标矩阵。

可选的，所述根据所述旋转后的第一平面的平面点云参数计算目标平移向量，包括：根据所述旋转后的第一平面的平面点云参数以及所述第二平面的平面点云参数，计算所述第一平面与所述第二平面之间的目标距离；计算所述目标距离在所述第二法向量上的投影向量，将所述投影向量作为目标平移向量。

通过采用上述技术方案，通过计算两平面之间的目标距离及其在第二法向量方向上的投影，获得目标平移向量。该方法充分利用两平面旋转调整后的参数，优化计算两平面间的相对位移关系，可直接确定出将第一平面准确平移到第二平面位置的平移向量，可以更加精确提取两平面实际需要移动的距离，有利于构建精确表达两平面间位置关系的目标平移向量。

可选的，所述根据所述旋转后的第一平面的平面点云参数计算目标平移向量之前，还包括：获取旋转后的第一平面的点云数据；基于最小二乘法和所述旋转后的第一平面的点云数据，计算所述旋转后的第一平面的平面点云参数。

通过采用上述技术方案，在计算目标平移向量之前，首先提取旋转调整后的第一平面点云，并通过最小二乘法拟合计算描述该平面模型的平面点云参数，最小二乘法可有效抑制数据噪声影响，确保计算所得平面参数准确可靠，且可以进一步减少计算量。

在本申请的第二方面提供了一种三维平面点云粗配准***，所述***包括：

法向量获取模块，用于获取需要进行粗配准的第一平面的第一法向量，以及第二平面的第二法向量；

旋转矩阵计算模块，用于根据所述第一法向量以及所述第二法向量计算目标旋转矩阵；

旋转变换模块，用于基于所述目标旋转矩阵将所述第一平面进行旋转变换，得到旋转后的第一平面；

平移向量计算模块，用于根据所述旋转后的第一平面的平面点云参数计算目标平移向量；

平移变换模块，用于基于所述第一法向量的方向和所述目标平移向量，平移所述旋转后的第一平面。

在本申请的第三方面提供了一种计算机存储介质，所述计算机存储介质存储有多条指令，所述指令适于由处理器加载并执行上述的方法步骤。

在本申请的第四方面提供了一种电子设备，包括：处理器和存储器；其中，所述存储器存储有计算机程序，所述计算机程序适于由所述处理器加载并执行上述的方法步骤。

综上所述，本申请实施例中提供的一个或多个技术方案，至少具有如下技术效果或优点：

1、本申请提取两个平面点云的法向量信息，并构建旋转矩阵与平移向量实现两平面的配准，无需现有技术ICP算法中建立点与点的最近邻关系进行迭代配准，也避免了因平面特征单一导致的易发散和陷入局部最优的问题，通过合理利用两平面点云的法向量信息，实现了平面点云的快速旋转对齐和平移重合，达到准确、高效的平面点云粗配准，提高了平面的粗配准效率，从而提高机器人的定位与建图效果；

2、本申请随机采样一致性算法分别拟合计算两平面点云数据的平面模型，以获取两法向量，该算法通过迭代排除误差较大的离群点，可有效抑制离群点对平面拟合的干扰，确保法向量准确表示平面法线方向；

3、本申请通过解析两法向量在坐标轴方向上的投影分量，分别计算两平面需要旋转的角度，然后基于这四个旋转角构建目标旋转矩阵。相比直接使用法向量构建变换，该方法充分利用了法向量在各轴上的分量信息，可准确提取两平面点云所需的旋转变换参数，有利于构建精确表达两平面旋转关系的目标矩阵。

附图说明

图1是本申请实施例提供的一种三维平面点云粗配准方法的流程示意图；

图2是本申请实施例提供的一种三维平面点云粗配准***模块示意图；

图3是本申请实施例提供的一种电子设备的结构示意图。

附图标记说明：300、电子设备；301、处理器；302、通信总线；303、用户接口；304、网络接口；305、存储器。

具体实施方式

为了使本领域的技术人员更好地理解本说明书中的技术方案，下面将结合本说明书实施例中的附图，对本说明书实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本申请一部分实施例，而不是全部的实施例。

在本申请实施例的描述中，“例如”或者“举例来说”等词用于表示作例子、例证或说明。本申请实施例中被描述为“例如”或者“举例来说”的任何实施例或设计方案不应被解释为比其他实施例或设计方案更优选或更具优势。确切而言，使用“例如”或者“举例来说”等词旨在以具体方式呈现相关概念。

在本申请实施例的描述中，术语“多个”的含义是指两个或两个以上。例如，多个***是指两个或两个以上的***，多个屏幕终端是指两个或两个以上的屏幕终端。此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个该特征。术语“包括”、“包含”、“具有”及它们的变形都意味着“包括但不限于”，除非是以其他方式另外特别强调。

下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清除、完整的描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。

请参考图1，特提出了一种三维平面点云粗配准方法的流程示意图，该方法可以依赖于计算机程序实现，可依赖于单片机实现，也可运行于一种三维平面点云粗配准***上，该计算机程序可集成在点云匹配智能终端中，也可作为独立的工具类应用运行，具体的，该方法包括步骤10至步骤50，上述步骤如下：

步骤10：获取需要进行粗配准的第一平面的第一法向量，以及第二平面的第二法向量。

本申请实施例可以应用在机器人平面粗配准的场景，比如通常在办公室、生产车间、物流仓库等室内场景和园区、城市公路等室外场景环境均存在大量平面，可以对两个平面进行点云粗配准，来作为后续机器人定位的初始化位姿，具体应用场景在此不做限定。

在本申请实施例中，第一平面是指需要进行旋转和平移变换的平面点云，通过变换调整第一平面的位置和姿态，实现与第二平面的粗配准。第二平面是指不需要进行旋转和平移变换的平面点云，作为参考的目标平面点云。

具体的，为实现两不同三维平面点云的配准，需要获取各自平面法向量，以构建后续的旋转和平移变换。本实施例使用3D激光雷达获取第一平面点云数据和第二平面点云数据。然后对两平面点云数据分别采用随机采样一致性算法，通过迭代随机采样拟合平面模型，即可计算代表各平面法向量方向的法线向量，取得两平面法向量后，可构建旋转矩阵将第一平面法向量旋转调整与第二平面法向量平行，再计算平移距离实现两平面点云配准。本实施例通过3D激光雷达获取高精度平面点云数据，运用随机采样一致性算法抑制噪声点对法向量计算的影响，有效获取两平面点云的法向量，为后续变换奠定基础，实现两不同平面点云的准确粗配准，其中第一平面和第二平面是需要进行粗配准的两个不同三维平面，第一平面和第二平面之间可以是任意角度，只要两法向量不平行即可计算粗配准。

在上述实施例的基础上，作为一种可选的实施例，获取需要进行粗配准的第一平面的第一法向量，以及第二平面的第二法向量这一步骤，还可以包括以下步骤：

步骤101：获取第一平面的第一点云数据和第二平面的第二点云数据。

具体的，为了后续计算两平面点云的法向量实现粗配准，需要先获取包含两平面信息的高质量点云数据。本实施例可以使用机械旋转式三维激光雷达对室内环境进行多站位扫描并点云拼接，得到整体点云。然后通过平面分割算法提取点云中位于不同方向两个平面作为第一平面点云数据和第二平面点云数据。其中，第一点云数据可以来源于针对第一平面进行的三维扫描获得的点云，比如对墙面进行的扫描获得的点云。第二点云数据可以来源于针对第二平面进行的三维扫描获得的点云，比如对地面进行的扫描获得的点云。第一点云数据和第二点云数据分别包含两平面的三维点分布信息。

步骤102：基于随机采样一致性算法计算第一点云数据在欧式坐标系下的第一平面方程，并确定第一平面方程对应的第一法向量。

具体的，为构建平面点云之间的变换，需要计算各平面法向量。本实施例对已获取的第一平面点云数据，采用随机采样一致性算法求解其平面模型，具体是：随机从第一平面点云中抽取3个非共线点，构建过这3点的平面方程；然后计算剩余所有点到此平面的距离，保留距离最小的一批内点，舍弃误差较大的离群点；基于内点不断迭代求解平面模型，逐步提高精度。最终可获得第一平面在欧式坐标系下的精确第一平面方程，比如第一平面方程可以为：，并由此推算出代表平面法向量方向的第一法向量，比如第一法向量为：。该方法有效抑制了离群点对平面法向量计算的干扰，保证第一法向量的准确性。

步骤103：基于随机采样一致性算法计算第二点云数据在欧式坐标系下的第二平面方程，并确定第二平面方程对应的第二法向量。

具体的，对已获取的第二平面点云数据，采用随机采样一致性算法求解其平面模型，具体是：随机从第二平面点云中抽取3个非共线点，构建过这3点的平面方程；然后计算剩余所有点到此平面的距离，保留距离最小的一批内点，舍弃误差较大的离群点；基于内点不断迭代求解平面模型，逐步提高精度。最终可获得第二平面在欧式坐标系下的精确第二平面方程，比如第二平面方程可以为：，并由此推算出代表平面法向量方向的第二法向量，比如第二法向量为：。需要说明的是，第一法向量和第二法向量均为单位法向量。

步骤20：根据第一法向量以及第二法向量计算目标旋转矩阵。

具体的，为实现两平面点云的粗配准，需要构建旋转矩阵将第一平面点云旋转调整与第二平面重合。本实施例通过计算两个单位化法向量之间的夹角求得旋转轴，然后根据旋转角计算旋转矩阵，作为目标旋转矩阵。具体计算方法是：计算第一法向量和第二法向量的叉乘得到旋转轴；计算两个单位向量的点乘获取旋转角；根据旋转轴和旋转角构建目标旋转矩阵，该目标旋转矩阵可以将第一平面法向量方向旋转调整与第二平面法向量方向平行。

在上述实施例的基础上，作为一种可选的实施例，根据第一法向量以及第二法向量计算目标旋转矩阵，还可以包括以下步骤：

步骤201：根据第一法向量计算第一X轴旋转角和第一Y轴旋转角。

步骤202：根据第二法向量计算第二X轴旋转角和第二Y轴旋转角。

具体的，首先解析出第一法向量在三维笛卡尔坐标系下的分量值，然后根据其在XY平面上的投影与X轴正方向的夹角计算第一X轴旋转角，再根据其与Z轴正方向的夹角计算第一Y轴旋转角。这样可以获得沿水平和垂直方向上对第一平面点云进行旋转调整所需的角度值。同理，可以根据第二法向量计算第二X轴旋转角和第二Y轴旋转角。

示例性地，第一X轴旋转角为：，第一Y轴旋转角为：/>。第二X轴旋转角为：/>，第二Y轴旋转角为：/>。

步骤203：基于第一X轴旋转角、第一Y轴旋转角、第二X轴旋转角以及第二Y轴旋转角，得到目标旋转矩阵。

具体的，在计算第一平面点云所需的第一X轴旋转角、第一Y轴旋转角及第二平面点云对应的第二X轴旋转角和第二Y轴旋转角后，本实施例将这四个旋转角度代入坐标旋转矩阵的公式，运算构建出一个目标旋转矩阵。该目标矩阵包含了沿横向和纵向两个方向上，将第一平面点云旋转到与第二平面平行所需的旋转变换信息。应用该旋转矩阵对第一平面点云进行坐标变换，可以准确实现第一平面点云向第二平面点云方向的旋转调整。最终达到两平面配准的目。

在上述实施例的基础上，作为一种可选的实施例，基于第一X轴旋转角、第一Y轴旋转角、第二X轴旋转角以及第二Y轴旋转角，得到目标旋转矩阵，还可以包括以下步骤：

步骤2031：将第一平面绕世界欧式坐标系的X轴方向旋转第一X轴旋转角，得到第一旋转矩阵，将第一旋转矩阵绕世界欧式坐标系的Y轴方向旋转第一Y轴旋转角，得到第二旋转矩阵。

步骤2032：将第二平面绕世界欧式坐标系的X轴方向旋转第二X轴旋转角，得到第三旋转矩阵，将第三旋转矩阵绕世界欧式坐标系的Y轴方向旋转第二Y轴旋转角，得到第四旋转矩阵。

具体的，将第一平面的第一法向量旋转变换到（0，0，1）方向上，也就是根据计算的第一X轴旋转角，代入二维旋转矩阵公式，将第一平面点云绕世界坐标系X轴方向旋转第一X轴旋转角，得到代表X轴旋转变换的第一旋转矩阵。然后再根据第一Y轴旋转角，将第一旋转矩阵绕Y轴方向旋转第一Y轴旋转角，计算代表Y轴旋转变换的第二旋转矩阵。同理，将第二平面的第二法向量旋转变换到（0，0，1）方向上，也就是再根据第二X轴旋转角，计算将第二平面点云绕世界坐标系X轴方向旋转的第三旋转矩阵，然后依据第二Y轴旋转角，计算将第三旋转矩阵继续绕Y轴旋转得到第四旋转矩阵。

需要说明的是，从几何上来讲，无论先绕世界欧式坐标系的X轴方向旋转再绕Y轴旋转或者先绕Y轴旋转再绕X轴旋转的结果是相同的，在此不做限定。

步骤2033：基于第一旋转矩阵、第二旋转矩阵、第三旋转矩阵以及第四旋转矩阵，得到目标旋转矩阵。

具体的，为实现两平面点云精确配准，需要构建包含两平面旋转信息的目标旋转矩阵。在计算第一平面和第二平面分别对应的两对旋转矩阵后，本实施例将代表第一平面调整的第一旋转矩阵和第二旋转矩阵相乘，得到第一平面对应的第一对合矩阵。同时将代表第二平面对应的第三旋转矩阵和第四旋转矩阵相乘，得到第二平面对应的第二对合矩阵。然后再将两对合矩阵相乘，即可以得到第一平面旋转变换的目标旋转矩阵。这样目标矩阵包含了使两平面点云方向一致所需的复合旋转信息，使用该矩阵转换第一平面点云坐标，就可精确实现两平面点云的粗配准。

示例性地，先将第一平面的第一法线向量旋转变换到（0，0，1）方向上，也就是将第一平面绕世界欧式坐标系的轴方向旋转第一X轴旋转角，得到对应的第一旋转矩阵，再将第一旋转矩阵绕世界欧式坐标系的Y轴方向旋转第一Y轴旋转角，得到对应的第二旋转矩阵/>，将该第一旋转矩阵与第二旋转矩阵相乘，得到第一平面对应的第一对合矩阵：/>。由于的逆矩阵，即为对合矩阵。同理，也将第三旋转矩阵与第四旋转矩阵相乘，得到第二平面对应的第二对合矩阵：/>，/>同样为对合矩阵。则将第一平面旋转变换到与第二平面平行的目标旋转矩阵为：/>，则

步骤30：基于目标旋转矩阵将第一平面进行旋转变换，得到旋转后的第一平面。

具体的，在构建出目标旋转矩阵后，为实现两平面精确配准，需要使用该目标旋转矩阵对第一平面点云进行坐标变换。本实施例直接将目标旋转矩阵和第一平面每个点的坐标相乘，进行矩阵运算，即可得到经过方向调整的新第一平面点云坐标，这样通过矩阵旋转变换，第一平面点云就与第二平面点云方向平行。

步骤40：根据旋转后的第一平面的平面点云参数计算目标平移向量。

具体的，在实现两平面配准的方向对齐后，还需计算平移向量完成位置对齐。本实施例基于经旋转调整后的第一平面平面点云，计算两平面的目标距离。然后计算目标距离在第二法向量上的投影向量，该投影向量即为第一平面相对第二平面所需的目标平移向量，这样就可以获得将旋转后的第一平面点云移动到与第二平面重合位置的平移变换量。

在上述实施例的基础上，作为一种可选的实施例，根据旋转后的第一平面的平面点云参数计算目标平移向量这一步骤，还可以包括以下步骤：

步骤401：根据旋转后的第一平面的平面点云参数以及第二平面的平面点云参数，计算第一平面与第二平面之间的目标距离。

具体的，为计算实现两平面配准的平移向量，需先确定旋转调整后的第一平面参数和第二平面的平面点云参数，平面点云参数是表示平面点云数学模型的一组参数。本实施例通过提取经目标旋转矩阵变换后的第一平面点云数据，作为旋转后的第一平面点云，然后基于最小二乘法，利用该旋转后平面点云数据拟合计算平面参数，包括法向量和质心坐标等，通过最小二乘法的计算可以进一步减少计算量。

进一步的，结合上述实施例可知，第一平面点云粗匹配到第二平面点云的变换矩阵为：，例如第一平面点云在旋转后的平面参数为：/>；则有/>=/>，其中/>写成分块函数的形式，则上式为：/>，其中/>，根据分块矩阵的性质可知：/>=/>，带入上式可得到/>，由此可以得到第一平面与第二平面之间的目标距离为：/>。

步骤402：计算目标距离在第二法向量上的投影向量，将投影向量作为目标平移向量。

具体的，由于第二平面的第二法向量为，则目标距离在该第二法向量分别向欧式坐标系的X，Y，Z轴进行投影，可得投影向量为：，该投影向量即为目标平移向量。该目标目标平移向量也可以表示为：/>，其中/>。

步骤50：基于第一法向量的方向和目标平移向量，平移旋转后的第一平面。

具体的，在计算实现两平面配准的目标平移向量后，需对旋转调整后的第一平面点云应用该目标平移向量，完成两平面位置对齐。本实施例直接将目标平移向量按照第一平面法向量方向添加到第一平面每个点坐标上，进行向量运算，也就是将旋转后的第一平面按照第一法向量的方向平移该目标平移向量。这样就将旋转后的第一平面点云精确平移到第二平面所在位置，实现两平面重合，从而实现第一平面和第二平面的快速粗配准。

请参见图2，为本申请实施例提供的一种三维平面点云粗配准***模块示意图，该三维平面点云粗配准***可以包括：法向量获取模块、旋转矩阵计算模块、旋转变换模块、平移向量计算模块以及平移变换模块，其中：

法向量获取模块，用于获取需要进行粗配准的第一平面的第一法向量，以及第二平面的第二法向量；

旋转矩阵计算模块，用于根据所述第一法向量以及所述第二法向量计算目标旋转矩阵；

旋转变换模块，用于基于所述目标旋转矩阵将所述第一平面进行旋转变换，得到旋转后的第一平面；

平移向量计算模块，用于根据所述旋转后的第一平面的平面点云参数计算目标平移向量；

平移变换模块，用于基于所述第一法向量的方向和所述目标平移向量，平移所述旋转后的第一平面。

可选的，所述法向量获取模块，还用于获取第一平面的第一点云数据和第二平面的第二点云数据；基于随机采样一致性算法计算所述第一点云数据在欧式坐标系下的第一平面方程，并确定所述第一平面方程对应的第一法向量；基于所述随机采样一致性算法计算所述第二点云数据在欧式坐标系下的第二平面方程，并确定所述第二平面方程对应的第二法向量。

可选的，所述旋转矩阵计算模块，还用于根据所述第一法向量计算第一X轴旋转角和第一Y轴旋转角；根据所述第二法向量计算第二X轴旋转角和第二Y轴旋转角；基于所述第一X轴旋转角、所述第一Y轴旋转角、所述第二X轴旋转角以及所述第二Y轴旋转角，得到目标旋转矩阵。

可选的，所述旋转矩阵计算模块，还用于将所述第一平面绕世界欧式坐标系的X轴方向旋转所述第一X轴旋转角，得到第一旋转矩阵，将所述第一旋转矩阵绕所述世界欧式坐标系的Y轴方向旋转所述第一Y轴旋转角，得到第二旋转矩阵；将所述第二平面绕所述世界欧式坐标系的X轴方向旋转所述第二X轴旋转角，得到第三旋转矩阵，将所述第三旋转矩阵绕所述世界欧式坐标系的Y轴方向旋转所述第二Y轴旋转角，得到第四旋转矩阵；基于所述第一旋转矩阵、所述第二旋转矩阵、所述第三旋转矩阵以及所述第四旋转矩阵，得到目标旋转矩阵。

可选的，所述旋转矩阵计算模块，还用于将所述第一旋转矩阵与所述第二旋转矩阵相乘，得到所述第一平面对应的第一对合矩阵；将所述第三旋转矩阵与所述第四旋转矩阵相乘，得到所述第二平面对应的第二对合矩阵；将所述第一对合矩阵和所述第二对合矩阵相乘，得到目标旋转矩阵。

可选的，所述平移向量计算模块，还用于根据所述旋转后的第一平面的平面点云参数以及所述第二平面的平面点云参数，计算所述第一平面与所述第二平面之间的目标距离；计算所述目标距离在所述第二法向量上的投影向量，将所述投影向量作为目标平移向量。

可选的，一种三维平面点云粗配准***还包括点云参数计算模块，所述点云参数计算模块，还用于获取旋转后的第一平面的点云数据；基于最小二乘法和所述旋转后的第一平面的点云数据，计算所述旋转后的第一平面的平面点云参数。

需要说明的是：上述实施例提供的***在实现其功能时，仅以上述各功能模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能模块完成，即将设备的内部结构划分成不同的功能模块，以完成以上描述的全部或者部分功能。另外，上述实施例提供的***和方法实施例属于同一构思，其具体实现过程详见方法实施例，这里不再赘述。

本申请实施例还提供了一种计算机存储介质，计算机存储介质可以存储有多条指令，指令适于由处理器加载并执行上述实施例的一种三维平面点云粗配准方法，具体执行过程可以参见上述实施例的具体说明，在此不进行赘述。

请参照图3本申请还公开一种电子设备。图3是本申请实施例的公开的一种电子设备的结构示意图。该电子设备300可以包括：至少一个处理器301，至少一个网络接口304，用户接口303，存储器305，至少一个通信总线302。

其中，通信总线302用于实现这些组件之间的连接通信。

其中，用户接口303可以包括显示屏（Display）、摄像头（Camera），可选用户接口303还可以包括标准的有线接口、无线接口。

其中，网络接口304可选的可以包括标准的有线接口、无线接口（如WI-FI接口）。

其中，处理器301可以包括一个或者多个处理核心。处理器301利用各种接口和线路连接整个服务器内的各个部分，通过运行或执行存储在存储器305内的指令、程序、代码集或指令集，以及调用存储在存储器305内的数据，执行服务器的各种功能和处理数据。可选的，处理器301可以采用数字信号处理（Digital Signal Processing，DSP）、现场可编程门阵列（Field-Programmable Gate Array，FPGA）、可编程逻辑阵列（Programmable LogicArray，PLA）中的至少一种硬件形式来实现。处理器301可集成中央处理器（CentralProcessing Unit，CPU）、图像处理器（Graphics Processing Unit，GPU）和调制解调器等中的一种或几种的组合。其中，CPU主要处理操作***、用户界面和应用程序等；GPU用于负责显示屏所需要显示的内容的渲染和绘制；调制解调器用于处理无线通信。可以理解的是，上述调制解调器也可以不集成到处理器301中，单独通过一块芯片进行实现。

其中，存储器305可以包括随机存储器（Random Access Memory，RAM），也可以包括只读存储器（Read-Only Memory）。可选的，该存储器305包括非瞬时性计算机可读介质（non-transitory computer-readable storage medium）。存储器305可用于存储指令、程序、代码、代码集或指令集。存储器305可包括存储程序区和存储数据区，其中，存储程序区可存储用于实现操作***的指令、用于至少一个功能的指令（比如触控功能、声音播放功能、图像播放功能等）、用于实现上述各个方法实施例的指令等；存储数据区可存储上面各个方法实施例中涉及的数据等。存储器305可选的还可以是至少一个位于远离前述处理器301的存储装置。参照图3，作为一种计算机存储介质的存储器305中可以包括操作***、网络通信模块、用户接口模块以及一种三维平面点云粗配准方法的应用程序。

在图3所示的电子设备300中，用户接口303主要用于为用户提供输入的接口，获取用户输入的数据；而处理器301可以用于调用存储器305中存储一种三维平面点云粗配准方法的应用程序，当由一个或多个处理器301执行时，使得电子设备300执行如上述实施例中一个或多个所述的方法。需要说明的是，对于前述的各方法实施例，为了简单描述，故将其都表述为一系列的动作组合，但是本领域技术人员应该知悉，本申请并不受所描述的动作顺序的限制，因为依据本申请，某些步骤可以采用其他顺序或者同时进行。其次，本领域技术人员也应该知悉，说明书中所描述的实施例均属于优选实施例，所涉及的动作和模块并不一定是本申请所必需的。

在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述的部分，可以参见其他实施例的相关描述。

在本申请所提供的几种实施方式中，应该理解到，所披露的装置，可通过其他的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个***，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些服务接口，装置或单元的间接耦合或通信连接，可以是电性或其他的形式。

作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

另外，在本申请各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储器中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储器中，包括若干指令用以使得一台计算机设备（可为个人计算机、服务器或者网络设备等）执行本申请各个实施例方法的全部或部分步骤。而前述的存储器包括：U盘、移动硬盘、磁碟或者光盘等各种可以存储程序代码的介质。

以上所述者，仅为本公开的示例性实施例，不能以此限定本公开的范围。即但凡依本公开教导所作的等效变化与修饰，皆仍属本公开涵盖的范围内。本领域技术人员在考虑说明书及实践真理的公开后，将容易想到本公开的其他实施方案。

本申请旨在涵盖本公开的任何变型、用途或者适应性变化，这些变型、用途或者适应性变化遵循本公开的一般性原理并包括本公开未记载的本技术领域中的公知常识或惯用技术手段。说明书和实施例仅被视为示例性的，本公开的范围和精神由权利要求限定。

Claims (9)

1.一种三维平面点云粗配准方法，其特征在于，所述方法包括：

获取需要进行粗配准的第一平面的第一法向量，以及第二平面的第二法向量；

根据所述第一法向量以及所述第二法向量计算目标旋转矩阵；

基于所述目标旋转矩阵将所述第一平面进行旋转变换，得到旋转后的第一平面；

根据所述旋转后的第一平面的平面点云参数以及所述第二平面的平面点云参数，计算所述第一平面与所述第二平面之间的目标距离；所述旋转后的第一平面的平面点云参数为：,则有/>，其中，写成分块函数的形式，则上式为：，其中，，根据分块矩阵的性质可知：，带入上式可得到/>，由此得到第一平面与第二平面之间的目标距离为：/>，第二平面的第二法向量为/>，则目标距离在所述第二法向量分别向欧式坐标系的X，Y，Z轴进行投影，可得投影向量为：/>，所述投影向量即为目标平移向量，所述目标平移向量表示为：/>，其中：/>；基于所述第一法向量的方向和所述目标平移向量，平移所述旋转后的第一平面。

2.根据权利要求1所述的三维平面点云粗配准方法，其特征在于，所述获取需要进行粗配准的第一平面的第一法向量，以及第二平面的第二法向量，包括：

获取第一平面的第一点云数据和第二平面的第二点云数据；

基于随机采样一致性算法计算所述第一点云数据在欧式坐标系下的第一平面方程，并确定所述第一平面方程对应的第一法向量；

基于所述随机采样一致性算法计算所述第二点云数据在欧式坐标系下的第二平面方程，并确定所述第二平面方程对应的第二法向量。

3.根据权利要求1所述的三维平面点云粗配准方法，其特征在于，所述根据所述第一法向量以及所述第二法向量计算目标旋转矩阵，包括：

根据所述第一法向量计算第一X轴旋转角和第一Y轴旋转角；

根据所述第二法向量计算第二X轴旋转角和第二Y轴旋转角；

基于所述第一X轴旋转角、所述第一Y轴旋转角、所述第二X轴旋转角以及所述第二Y轴旋转角，得到目标旋转矩阵。

4.根据权利要求3所述的三维平面点云粗配准方法，其特征在于，所述基于所述第一X轴旋转角、所述第一Y轴旋转角、所述第二X轴旋转角以及所述第二Y轴旋转角，得到目标旋转矩阵，包括：

将所述第一平面绕世界欧式坐标系的X轴方向旋转所述第一X轴旋转角，得到第一旋转矩阵，将所述第一旋转矩阵绕所述世界欧式坐标系的Y轴方向旋转所述第一Y轴旋转角，得到第二旋转矩阵；

将所述第二平面绕所述世界欧式坐标系的X轴方向旋转所述第二X轴旋转角，得到第三旋转矩阵，将所述第三旋转矩阵绕所述世界欧式坐标系的Y轴方向旋转所述第二Y轴旋转角，得到第四旋转矩阵；

基于所述第一旋转矩阵、所述第二旋转矩阵、所述第三旋转矩阵以及所述第四旋转矩阵，得到目标旋转矩阵。

5.根据权利要求4所述的三维平面点云粗配准方法，其特征在于，所述基于所述第一旋转矩阵、所述第二旋转矩阵、所述第三旋转矩阵以及所述第四旋转矩阵，得到目标旋转矩阵，包括：

将所述第一旋转矩阵与所述第二旋转矩阵相乘，得到所述第一平面对应的第一对合矩阵；

将所述第三旋转矩阵与所述第四旋转矩阵相乘，得到所述第二平面对应的第二对合矩阵；

将所述第一对合矩阵和所述第二对合矩阵相乘，得到目标旋转矩阵。

6.根据权利要求1所述的三维平面点云粗配准方法，其特征在于，所述根据所述旋转后的第一平面的平面点云参数计算目标平移向量之前，还包括：

获取旋转后的第一平面的点云数据；

基于最小二乘法和所述旋转后的第一平面的点云数据，计算所述旋转后的第一平面的平面点云参数。

7.一种三维平面点云粗配准***，其特征在于，所述***包括：

法向量获取模块，用于获取需要进行粗配准的第一平面的第一法向量，以及第二平面的第二法向量；

旋转矩阵计算模块，用于根据所述第一法向量以及所述第二法向量计算目标旋转矩阵；

旋转变换模块，用于基于所述目标旋转矩阵将所述第一平面进行旋转变换，得到旋转后的第一平面；

平移向量计算模块，用于根据所述旋转后的第一平面的平面点云参数以及所述第二平面的平面点云参数，计算所述第一平面与所述第二平面之间的目标距离；所述旋转后的第一平面的平面点云参数为：,则有/>，其中，/>写成分块函数的形式，则上式为：，其中，，根据分块矩阵的性质可知：，带入上式可得到/>，由此得到第一平面与第二平面之间的目标距离为：/>，第二平面的第二法向量为/>，则目标距离在所述第二法向量分别向欧式坐标系的X，Y，Z轴进行投影，可得投影向量为：/>，所述投影向量即为目标平移向量，所述目标平移向量表示为：，其中：/>；

平移变换模块，用于基于所述第一法向量的方向和所述目标平移向量，平移所述旋转后的第一平面。

8.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有多条指令，所述指令适用于由处理器加载并执行如权利要求1-6任意一项所述的方法。

9.一种电子设备，其特征在于，包括处理器、存储器、用户接口及网络接口，所述存储器用于存储指令，所述用户接口和所述网络接口用于给其他设备通信，所述处理器用于执行所述存储器中存储的指令，以使所述电子设备执行如权利要求1-6任意一项所述的方法。