CN111932628B - 一种位姿确定方法及装置、电子设备、存储介质 - Google Patents
一种位姿确定方法及装置、电子设备、存储介质 Download PDFInfo
- Publication number
- CN111932628B CN111932628B CN202011003842.4A CN202011003842A CN111932628B CN 111932628 B CN111932628 B CN 111932628B CN 202011003842 A CN202011003842 A CN 202011003842A CN 111932628 B CN111932628 B CN 111932628B
- Authority
- CN
- China
- Prior art keywords
- point
- model
- scene
- coordinate point
- current
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 84
- 238000003860 storage Methods 0.000 title claims abstract description 14
- 230000009466 transformation Effects 0.000 claims abstract description 197
- 238000004364 calculation method Methods 0.000 claims abstract description 18
- 239000011159 matrix material Substances 0.000 claims description 274
- 238000013519 translation Methods 0.000 claims description 46
- 238000012545 processing Methods 0.000 claims description 30
- 230000008569 process Effects 0.000 claims description 23
- 239000013598 vector Substances 0.000 claims description 12
- 238000004891 communication Methods 0.000 claims description 7
- 238000012804 iterative process Methods 0.000 claims description 5
- 238000004088 simulation Methods 0.000 claims description 4
- 230000004044 response Effects 0.000 claims description 3
- 238000000605 extraction Methods 0.000 claims description 2
- 238000010586 diagram Methods 0.000 description 11
- 238000004590 computer program Methods 0.000 description 7
- 230000006870 function Effects 0.000 description 6
- 238000004422 calculation algorithm Methods 0.000 description 5
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 description 4
- 238000012935 Averaging Methods 0.000 description 3
- 239000000284 extract Substances 0.000 description 3
- 238000005070 sampling Methods 0.000 description 3
- 230000001131 transforming effect Effects 0.000 description 3
- 230000000694 effects Effects 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 238000000691 measurement method Methods 0.000 description 2
- 238000013528 artificial neural network Methods 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000000354 decomposition reaction Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 239000006185 dispersion Substances 0.000 description 1
- 239000003814 drug Substances 0.000 description 1
- 230000008676 import Effects 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 238000011426 transformation method Methods 0.000 description 1
- 238000012800 visualization Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/70—Determining position or orientation of objects or cameras
- G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T19/00—Manipulating 3D models or images for computer graphics
- G06T19/006—Mixed reality
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2200/00—Indexing scheme for image data processing or generation, in general
- G06T2200/04—Indexing scheme for image data processing or generation, in general involving 3D image data
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10028—Range image; Depth image; 3D point clouds
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Computer Graphics (AREA)
- Computer Hardware Design (AREA)
- General Engineering & Computer Science (AREA)
- Software Systems (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Image Analysis (AREA)
Abstract
本公开实施例公开了一种位姿确定方法及装置、电子设备、存储介质，能够提高确定位姿的效率。该方法包括：根据真实场景图像生成场景点云图，并从场景点云图中提取场景坐标点集合；获取与场景点云图对应的模型点云图，并从模型点云图中并行选取出与场景坐标点集合对应的模型坐标点集合；基于场景坐标点与模型坐标点集合，通过并行迭代计算，求取出场景点云图与模型点云图之间对应的空间变换关系；根据空间变换关系，确定场景点云图与模型点云图之间的相对位姿。
Description
技术领域
本公开涉及软件工程领域，尤其涉及一种位姿确定方法及装置、电子设备、存储介质。
背景技术
目前，随着三维(3D，3-dimensional)视觉的发展，位姿估计在医学，物体建模，机器人学中占据越来越重要的地位。由于现有传感器的限制，大多数应用需要精度极高且由多张图片进行组合才可以得到物体准确的3D位姿结果。尤其在移动机器人场景，能否快速拿到3D图像并且极短时间内进行位姿匹配就成了限制机器人识别精度和运动速度的最重要的一个环节。而现有算法运行的时间成本高，从而降低了确定位姿的效率。
发明内容
本公开实施例期望提供一种位姿确定方法及装置、电子设备、存储介质，能够提高确定位姿的效率。
本公开的技术方案是这样实现的：
本公开实施例提供一种位姿确定方法，所述方法包括：
根据真实场景图像生成场景点云图，并从所述场景点云图中提取场景坐标点集合；
获取与所述场景点云图对应的模型点云图，并从所述模型点云图中并行选取出与所述场景坐标点集合对应的模型坐标点集合；
基于所述场景坐标点与所述模型坐标点集合，通过并行迭代计算，求取出所述场景点云图与所述模型点云图之间对应的空间变换关系；
根据所述空间变换关系，确定所述场景点云图与所述模型点云图之间的相对位姿。
本公开实施例提供一种位姿确定装置，所述位姿确定装置包括：提取单元、选取单元、迭代单元与确定单元，其中，
所述提取单元，用于根据真实场景图像生成场景点云图，并从所述场景点云图中提取场景坐标点集合；
所述选取单元，用于获取与所述场景点云图对应的模型点云图，并从所述模型点云图中并行选取出与所述场景坐标点集合对应的模型坐标点集合；
所述迭代单元，用于基于所述场景坐标点与所述模型坐标点集合，通过并行迭代计算，求取出所述场景点云图与所述模型点云图之间对应的空间变换关系；
所述确定单元，用于根据所述空间变换关系，确定所述场景点云图与所述模型点云图之间的相对位姿。
本公开实施例提供一种电子设备，所述电子设备包括：处理器、存储器以及通信总线，所述存储器通过所述通信总线与所述处理器进行通信，所述存储器存储所述处理器可执行的一个或者多个程序，当所述一个或者多个程序被执行时，所述处理器执行如上述任一种位姿确定方法。
本公开实施例提供一种存储介质，所述存储介质存储有一个或者多个程序，所述一个或者多个程序可被一个或者多个处理器执行，以实现如上述任一种位姿确定方法。
本公开实施例提供了一种位姿确定方法及装置、电子设备、存储介质，该方法包括：根据真实场景图像生成场景点云图，并从场景点云图中提取场景坐标点集合；获取与场景点云图对应的模型点云图，并从模型点云图中并行选取出与场景坐标点集合对应的模型坐标点集合；基于场景坐标点与模型坐标点集合，通过并行迭代计算，求取出场景点云图与模型点云图之间对应的空间变换关系；根据空间变换关系，确定场景点云图与模型点云图之间的相对位姿。采用上述方案，可以通过并行处理的方式，迅速选取出与场景坐标点集合对应的模型坐标点集合，大大提高了模型坐标点集合选取的速度；进一步的，在对通常包含有平移和旋转等多项参数的空间变换关系进行计算时，能够对每个参数进行并行迭代计算，进而根据计算得到的空间变换关系确定场景点云图与模型点云图之间相对位姿，从而提高了确定位姿的效率。
附图说明
图1为本公开实施例提供的一种位姿确定方法的一个可选的流程示意图一；
图2为本公开实施例提供的一种位姿确定方法的一个可选的流程示意图二；
图3为本公开实施例提供的一种位姿确定方法的一个可选的流程示意图三；
图4为本公开实施例提供的一种位姿确定方法的一个可选的流程示意图四；
图5为本公开实施例提供的一种位姿确定方法的一个可选的流程示意图五；
图6为本公开实施例提供的一种位姿确定方法的一个可选的流程示意图六；
图7为本公开实施例提供的一种位姿确定方法的一个可选的流程示意图七；
图8为本公开实施例提供的一种位姿确定方法的一个可选的流程示意图八；
图9为本公开实施例提供的点到曲面的误差度量方法示意图；
图10为本公开实施例提供的一种位姿确定方法的一个可选的流程示意图九；
图11为本公开实施例提供的一种位姿确定装置的结构示意图；
图12为本公开实施例提供的一种电子设备的结构示意图。
具体实施方式
下面将结合本公开实施例中的附图，对本公开实施例中的技术方案进行清楚、完整地描述。
图1为本公开实施例提供的方法的一个可选的流程示意图，将结合图1示出的步骤进行说明。
S101、根据真实场景图像生成场景点云图，并从场景点云图中提取场景坐标点集合。
本公开实施例中，位姿确定装置首先通过图像采集设备对待定位场景进行图像采集，获取待定位场景的真实场景图像。
本公开实施例中，图像采集设备可以是照相机、摄像机、摄像头等具有图像采集功能的设备，本公开实施例不做限定。
本公开实施例中，对于真实场景图像，位姿确定装置可以根据真实场景图像中的像素点生成场景点云图。
本公开实施例中，点云是物体或目标表面特征的海量点的集合，其中，点云中的每个点包括该点的三维坐标以及该点对应的法向量。
本公开实施例中，位姿确定装置可以通过对真实场景图像进行激光测量或摄影测量以得到场景点云图，也可以采用其他生成点云图的方法，具体的根据实际情况进行选择，本公开实施例不做限定。
本公开实施例中，在得到场景点云图之后，位姿确定装置从场景点云图中提取预设数量的坐标点，得到场景坐标点集合。
在一些实施例中，位姿确定装置可以将场景点云图导入图形处理器，通过图形处理器从场景点云图中选取场景坐标点集合。
本公开实施例中，场景坐标点集合中的坐标点为场景坐标点。
本公开实施例中，在场景点云图中，位姿确定装置可以通过随机选取的方式选取预设数量的坐标点；也可以在场景点云图中提取物体角点作为预设数量的坐标点；也可以根据实际定位的需要将场景点云图中指定范围内的坐标点选取为预设数量的坐标点，或者也可以采用均匀采样、随机采样和法矢采样法得到预设数量的坐标点等，具体的根据实际情况进行选择，本公开实施例不做限定。
在一些实施例中，预设数量通常大于3，即位姿确定装置从场景点云图中选取至少三个坐标点构成场景坐标点集合，具体的数量根据场景点云图的点云密度以及实际定位精度的需要进行选择，本公开实施例不做限定。
S102、获取与场景点云图对应的模型点云图，并从模型点云图中并行选取出与场景坐标点集合对应的模型坐标点集合。
本公开实施例中，模型点云图为预存的与场景点云图匹配的参考模板对应的点云图，得到场景坐标点集合之后，位姿确定装置从模型点云图中并行选取出与场景坐标点集合对应的模型坐标点集合。
本公开实施例中，场景点云图中每个场景坐标点所在的坐标系与模型点云图中每个模型坐标点所在的坐标系为两个不同的世界坐标系。其中，模型坐标点为模型坐标点集合中包含的坐标点。
本公开实施例中，为了使得场景点云图与模型点云图之间具有良好的配准初值，避免因为场景点云图与模型点云图之间初始位置匹配不合理，导致算法陷入局部最优，在对场景点云图与模型点云图进行迭代计算求取空间变换关系之前，通常需要先对场景点云图与模型点云图进行初步的图像匹配。因此，位姿确定装置可以预先采集至少一个标准场景的图像作为至少一个参考模板，并根据至少一个参考模板生成至少一个参考模板对应的点云图，存储在电子设备的存储单元中。这样，在获取到场景点云图后，位姿确定装置可以先将场景点云图与预先存储的至少一个参考模板的点云图进行匹配，示例性的，采用神经网络，通过图像特征匹配算法，对场景点云图与预先存储的至少一个参考模板的点云图进行图像匹配，将与场景点云图之间的匹配度大于预设匹配阈值的参考模板的点云图作为模型点云图。
本公开实施例中，图形处理器可以是GPU(Graphics Processing Unit)，用于处理图像和图形相关运算工作的微处理器。
本公开实施例中，图形处理器可以以并行的方式处理数据，并行选取是指在图形处理器中，对于场景点云图中预先选取的所有场景坐标点，同时在模型点云图搜索和确定对应的模型坐标点，进而得到场景坐标点集合对应的模型坐标点集合。
本公开实施例中，根据场景坐标点集合中的每个场景坐标点，位姿确定装置可以采用最近邻点法、法方向最邻近点法，或投影法等方法，从模型点云图中选取出对应的模型坐标点，具体的根据实际情况进行选择，本公开实施例不做限定。
本公开实施例中，通过图形处理器调用模型点云图，可以利用图形处理器的并行处理能力，在模型点云图中同时搜索场景坐标点集合中每个场景坐标点对应的模型坐标点，从而有效地减轻了CPU的负担，并提高模型坐标点集合搜索和选取的速度；也可以通过其他具备并行处理能力的芯片或处理器实现模型坐标点集合的并行选取，具体的根据实际情况进行选择，本公开实施例不做限定。
S103、基于场景坐标点与模型坐标点集合，通过并行迭代计算，求取出场景点云图与模型点云图之间对应的空间变换关系。
本公开实施例中，得到对应点对集合之后，位姿确定装置根据场景坐标点与模型坐标点集合中各自包含的三维坐标点对，对场景点云图和模型点云图之间的空间位置对应关系进行迭代计算，求解出场景点云图与模型点云图之间最准确的位置关系，作为场景点云图与模型点云图之间对应的空间变换关系。
本公开实施例中，由于场景点云图与模型点云图之间的空间位置对应关系在最开始并不明确，因此需要先根据最初得到的场景坐标点与模型坐标点集合，估计出场景点云图与模型点云图之间所符合的初始空间变换关系，再根据初始空间变换关系对场景点云图与模型点云图进行配准，以得到配准误差，并以最小化配准误差为目标，通过多次迭代过程，对初始空间变换关系进行更新，以逐步降低配准误差，直至最后满足迭代终止条件时，得到场景点云图与模型点云图之间对应的空间变换关系。
在一些实施例中，空间变换关系可以是场景点云图与模型点云图之间对应的变换矩阵，变换矩阵用于表征场景点云图与模型点云图之间的位置关系。通过变换矩阵，位姿确定装置可以使场景点云图坐标系下的点云数据通过平移、旋转等变换方法，映射到模型点云图坐标系下，进而能够在同一坐标系中度量场景点云图和模型点云图之间的位置关系，并根据场景点云图和模型点云图之间的位置关系进行位姿确定。
在一些实施例中，位姿确定装置可以基于场景坐标点与模型坐标点集合对预设变换矩阵模型的矩阵参数进行迭代计算，在每次迭代中通过当前次迭代的变换矩阵对场景点云图与模型点云图进行逐步配准，直至配准误差满足迭代终止条件时，得到最终能够准确表征场景点云图与模型点云图之间对应的空间变换关系的变换矩阵。
本公开实施例中，位姿确定装置可以通过迭代最近点(ICP，Iterative ClosetPoint)算法迭代求取出场景点云图与模型点云图之间对应的变换矩阵，也可以使用其他方法，本公开实施例不做限定。
在一些实施例中，位姿确定装置可以在图形处理器中进行空间变换关系的迭代计算，示例性的，对变换矩阵进行每一次迭代求解时，可以通过图形处理器同时计算变换矩阵中每个位置的矩阵参数，达到并行计算的效果；也可以通过其他具备并行处理能力的芯片或处理器实现空间变换关系的并行迭代计算，具体的根据实际情况进行选择，本公开实施例不做限定。
S104、根据空间变换关系，确定场景点云图与模型点云图之间的相对位姿。
本公开实施例中，在得到变换矩阵之后，位姿确定装置可以根据变换矩阵，恢复场景点云图与模型点云图之间的相对位姿。
本公开实施例中，由于变换矩阵表征了场景点云图与模型点云图之间的位置关系，因此位姿确定装置可以通过变换矩阵恢复出场景点云图相对于模型点云图的旋转和平移，从而确定场景点云图与模型点云图之间的相对位姿
本公开实施例中，确定位姿方法可以使用现有技术中的方法，本公开实施例不再赘述。
可以理解的是，本公开实施例中，可以通过并行处理的方式，迅速选取出与场景坐标点集合对应的模型坐标点集合，大大提高了模型坐标点集合选取的速度；进一步的，在计算包含平移和旋转等多项参数的空间变换关系时，能够对每个参数进行并行迭代计算，从而克服了传统算法需要一一串行搜索和选取场景坐标点集合对应的模型坐标点集合以及串行地计算变换矩阵的矩阵参数的计算瓶颈，进而提高了确定位姿的效率。
本公开实施例中，基于图1，S102中从模型点云图中并行选取出与场景坐标点集合对应的模型坐标点集合可以如图2所示，包括S1021-S1022，如下：
S1021、对于场景坐标点集合中的每个场景坐标点，通过图形处理器在模型点云图中进行并行搜索，同步选取出与每个场景坐标点的距离最近的坐标点。
本公开实施例中，对于场景坐标点集合中的一个场景坐标点，位姿确定装置通过图形处理器，从模型点云图中选取出与该场景坐标点之间的距离最近的坐标点。
本公开实施例中，位姿确定装置通过图形处理器，以上述方法同时根据场景坐标点集合中的每个场景坐标点在模型点云图中展开搜索，同步得到与每个场景坐标点的距离最近的坐标点。
S1022、将与每个场景坐标点的距离最近的坐标点，作为每个场景坐标点对应的模型坐标点，从而得到模型坐标点集合。
本公开实施例中，位姿确定装置将与每个场景坐标点的距离最近的坐标点，作为每个场景坐标点对应的模型坐标点，将每个场景坐标点对应的模型坐标点所构成的集合作为模型坐标点集合。
可以理解的是，本公开实施例中，可以利用图形处理器并行处理的特性，对于场景坐标点集合中的每个场景坐标点，在模型点云图中同步搜索并选取出模型坐标点，以用于后续的点云图配准，从而克服了传统方法中一个点找到最近点之后再开始下一个点的串行处理方式，提高了生成模型坐标点集合以及对应点对集合的效率，进而提高了位姿确定的效率。
本公开实施例中，基于图1，S103中在图形处理器中，基于场景坐标点与模型坐标点集合，通过并行迭代计算，求取出场景点云图与模型点云图之间对应的空间变换关系可以如图3所示，包括S1031-S1034，如下：
S1031、在一次迭代过程中，通过图形处理器，基于对应点对集合与预设变换矩阵模型满足的空间对应关系，对预设变换矩阵模型中的矩阵参数进行并行计算，得到当前次变换矩阵；空间对应关系为通过预设变换矩阵模型建立的，对应点对集合中的场景坐标点集合与模型坐标点集合之间的对应关系；对应点对集合为场景坐标点集合中的每个场景坐标点与其在模型坐标点集合中对应的模型坐标点构成的点对集合。
本公开实施例中，在得到对应点对集合之后，位姿确定装置可以基于对应点对集合与预设变换矩阵模型满足的空间对应关系，对预设变换矩阵模型中的矩阵参数进行并行计算，得到当前次变换矩阵。
本公开实施例中，空间对应关系为通过预设变换矩阵模型建立的，对应点对集合中的场景坐标点集合与模型坐标点集合之间的对应关系；预设变换矩阵模型为从场景点云图坐标系到模型点云图坐标系变换的约束关系模型，预设变换矩阵模型中的矩阵参数是未知的，需要通过对应点对集合中每个对应点对的场景坐标点与模型坐标点的三维坐标，根据对应点对集合与预设变换矩阵模型满足的空间对应关系的方程形式，来对未知的矩阵参数进行求解。
本公开实施例中，位姿确定装置将场景坐标点集合中的每个场景坐标点与其在模型坐标点集合中对应的模型坐标点构成的点对集合，作为对应点对集合。
本公开实施例中，对应点对集合由场景坐标点集合与模型坐标点集合之间的对应点对构成，每个对应点对中包含一个场景坐标点和一个与之对应的模型坐标点。
本公开实施例中，预设变换矩阵模型可以是不同坐标系下的刚性变换矩阵，对于三维空间的坐标点，可以构造4乘4的预设变换矩阵模型，对于预设变换矩阵模型中每个位置的矩阵参数，位姿确定装置可以利用图形处理器的并行处理能力，根据对应点对集合中每个场景坐标点的三维坐标值与对应的每个模型坐标点的三维坐标值，对预设变换矩阵模型中每个位置的矩阵参数同时进行并行计算，以同时求得全部矩阵参数的值，将求解出的矩阵参数代入预设变换矩阵模型中得到当前次变换矩阵。
本公开实施例中，当前次变换矩阵表征根据对应点集合求解出的场景点云图坐标系与模型点云图坐标系之间的空间变换关系。
本公开实施例中，预设变换矩阵模型包括旋转矩阵参数与平移矩阵参数，基于图3，S1031中基于对应点对集合与预设变换矩阵模型满足的空间对应关系，对预设变换矩阵模型中的矩阵参数进行并行计算，得到当前次变换矩阵可以如图4所示，包括S201-S203，如下：
S201、对预设变换矩阵模型进行近似处理，得到近似变换矩阵模型；近似处理为将场景坐标系对应于模型坐标系的旋转角度预设为零；场景坐标系为场景坐标点所在的坐标系，模型坐标系为模型坐标点所在的坐标系。
本公开实施例中，平移矩阵模型表征场景点云图坐标系与模型点云图坐标系之间进行对齐时需要平移的单位坐标量，通过公式(1)可以构造平移矩阵参数，如下：
公式(1)中，T(tx,ty,tz)为预设变换矩阵模型中的平移矩阵模型，tx,ty,tz分别为场景点云图坐标系中的x轴、y轴和z轴向模型点云图坐标系对齐时所需平移的单位坐标量。
本公开实施例中，旋转矩阵模型表征场景点云图坐标系与模型点云图坐标系之间进行对齐时需要旋转的角度，通过公式(2)和公式(3)可以构造旋转矩阵模型，如下：
其中，
公式(3)中，R(α,β,γ)为旋转矩阵模型，α,β,γ分别为场景点云图坐标系中的x轴、y轴和z轴向模型点云图坐标系对齐时所需旋转的角度。
本公开实施例中，对应点对集合与预设变换矩阵模型满足的空间对应关系可以如公式(4)所示，如下：
di＝R(α,β,γ)·si+T(tx,ty,tz) (4)
可以看出，本公开实施例中，旋转矩阵模型R是非线性的，导致旋转矩阵参数的求解实质上是一个非线性最小二乘问题。在实际应用中，由于场景点云图与模型点云图已经经过了初配准，因此通常场景点云图与模型点云图之间的相对运动非常小，因此可以在计算旋转矩阵参数r11,r12,r13时，可以将表征场景坐标系对应于模型坐标系的旋转角度的α,β和γ进行近似处理，认为α,β,γ接近于0，因此，基于图4，S201中对预设变换矩阵模型进行近似处理，得到近似变换矩阵模型可以如图5所示，包括S2011-S2012，如下：
S2011、对旋转矩阵模型进行近似处理，得到近似旋转矩阵模型。
本公开实施例中，位姿确定装置对旋转矩阵模型进行近似处理，认为α,β,γ接近于0，则有公式(5)：
sinα＝α，sinβ＝β，sinγ＝γ，cosα＝1，cosβ＝1，cosγ＝1 (5)
将公式(5)代入公式(3)，再代入公式(2)可得公式(6)，如下：
公式(6)中，为经过近似处理后的近似旋转矩阵模型。
S2012、使用近似旋转矩阵模型更新预设变换矩阵模型中的旋转矩阵模型，得到近似变换矩阵模型。
本公开实施例中，在得到近似旋转模型之后，位姿确定装置使用近似旋转模型替代预设变换矩阵模型中的旋转矩阵模型，得到包含近似旋转矩阵模型和平移矩阵模型的近似变换矩阵模型。
S202、将近似变换矩阵模型代入空间对应关系，得到对应点对集合与近似变换矩阵满足的近似空间对应关系。
本公开实施例中，在得到近似旋转矩阵模型之后，就可以将非线性最小二乘问题近似地转换为线性最小二乘问题进行求解。
本公开实施例中，将近似变换矩阵模型中的近似旋转矩阵模型以及平移矩阵模型T(tx,ty,tz)代入公式(4)的空间对应关系中，得到公式(7)：
di＝R(α,β,γ)·si+T(tx,ty,tz) (7)
将公式(7)的两端同时乘以模型坐标点的法向量再进行变形，得到公式(8)
(R(α,β,γ)·si+T(tx,ty,tz))·ni-di·ni＝0 (8)
在公式(8)中，将模型坐标点di的三维坐标、场景坐标点si的三维坐标与模型坐标点的法向量ni的三维向量都以矩阵的方式进行表示，可如公式(8-1)、(8-2)和(8-3)所示：
将公式(8)根据公式(8-1)、(8-2)和(8-3)进行展开，得到公式(9)：
将公式(9)进行整理，可以得到对应点对集合中每一对对应点对之间的近似空间对应关系，如公式(10)所示：
Ax-b＝0 (10)
在公式(10)中，x为近似变换矩阵，其中，A、x和b的具体展开分别如公式(10-1)、(10-2)、(10-3)和(10-4)所示：
x＝(α β γ tx ty tz)T (10-2)
S203、基于对应点对集合与近似空间对应关系，对近似变换矩阵模型中的矩阵参数进行并行计算，得到当前次变换矩阵。
本公开实施例中，位姿确定装置可以将对应点对集合的每对对应点对中的场景坐标点与模型坐标点的坐标值与法向量，根据近似空间对应关系的公式，对近似变换矩阵模型中的矩阵参数进行并行计算，得到当前次变换矩阵。
在本公开的一些实施例中，基于图4，S203中基于对应点对集合与近似空间对应关系，对近似变换矩阵模型中的矩阵参数进行并行计算，得到当前次变换矩阵可以如图6所示，包括S2031-S2035，如下：
S2031、获取对应点对集合中每对对应点对的场景位置信息与模型位置信息；场景位置信息为每对对应点对中场景坐标点的位置信息；模型位置信息为每对对应点对中模型坐标点的位置信息。
本公开实施例中，位姿确定装置获取对应点对集合中每对对应点对的场景位置信息与模型位置信息，其中，场景位置信息为每对对应点对中场景坐标点的位置信息，包含每对对应点对中场景坐标点的三维坐标，模型位置信息为每对对应点对中模型坐标点的位置信息，包含每对对应点对中模型坐标点的三维坐标和法向量。
S2032、将每对对应点对的场景位置信息与模型位置信息分别代入近似空间对应关系中，同步计算得到旋转矩阵参数和平移矩阵参数；旋转矩阵参数为近似变换矩阵中表征旋转角度变换的参数，平移矩阵参数为近似变换矩阵中表征平移变换的参数。
本公开实施例中，位姿确定装置将每对对应点对的场景坐标点的三维坐标与模型坐标点的三维坐标和法向量代入近似空间对应关系的公式(10)中，在图形处理器中对近似空间变化矩阵中的每一个位置的矩阵参数进行同步运算，同时求得旋转矩阵参数和平移矩阵参数。
本公开实施例中，旋转矩阵参数为近似变换矩阵中表征旋转角度变换的参数，对应于公式(10-2)中的α，β和γ；平移矩阵参数为近似变换矩阵中表征平移变换的参数，对应于公式(10-2)中的tx，ty和tz。
本公开实施例中，可以使用求解四元数的方法使用对平移矩阵模型中的平移矩阵参数和旋转矩阵模型中的旋转矩阵参数进行计算，也可以采用其他计算方法，具体的根据实际情况进行选择，本公开实施例不再赘述。
S2033、采用旋转矩阵参数更新近似旋转矩阵模型，得到当前次旋转矩阵。
本公开实施例中，位姿确定装置将计算出的旋转矩阵参数α，β和γ代入到近似旋转矩阵模型中，得到当前次旋转矩阵。
S2034、采用平移矩阵参数更新平移矩阵模型，得到当前次平移矩阵。
本公开实施例中，位姿确定装置将计算出的平移矩阵参数tx，ty和tz代入到平移矩阵模型中，得到当前次平移矩阵。
S2035、将当前次旋转矩阵与当前次平移矩阵相乘，得到当前次变换矩阵。
本公开实施例中，在得到当前次旋转矩阵与当前次平移矩阵之后，位姿确定装置可以根据公式(11)，将当前次旋转矩阵与当前次平移矩阵相乘，得到当前次变换矩阵。
M＝Ti(tx,ty,tz)·Ri(α,β,γ) (11)
公式(11)中，M为当前次变换矩阵，Ti(tx,ty,tz)为当前次旋转矩阵，Ri(α,β,γ)为当前次平移矩阵。
可以理解的是，通过旋转矩阵模型进行近似化处理，可以将非线性最小二乘的问题转换为线性最小二乘问题进行求解，简化了当前次变换矩阵的计算，从而提高了位姿确定的效率。
S1032、根据当前次变换矩阵，对对应点对集合中的每个场景坐标点进行坐标变换，得到每个场景坐标点对应的更新场景坐标点。
本公开实施例中，在根据对应点对集合求解出当前次变换矩阵之后，为了验证当前次变换矩阵的准确度，位姿确定装置根据当前次变换矩阵，对对应点对集合中包含的每个场景坐标点进行坐标变换，得到每个场景坐标点对应的更新场景坐标点。
本公开实施例中，位姿确定装置可以将每个场景坐标点的三维坐标组成的矩阵与当前次变换矩阵相乘，得到每个场景坐标点对应的更新场景坐标点。
本公开实施例中，位姿确定装置根据当前次变换矩阵，对对应点对集合中的每个场景坐标点进行坐标变换，得到每个场景坐标点对应的更新场景坐标点可以如公式(12)所示：
s'i＝M·si (12)
公式(12)中，si为场景坐标点，M为当前次变换矩阵，将场景坐标点si与当前次变换矩阵M代入公式(12)进行相乘可得更新场景坐标点s'i。
S1033、基于每个场景坐标点对应的更新场景坐标点进行下一次迭代，直至第i次迭代满足预设迭代条件时，得到第i次变换矩阵；其中，i为大于等于1的正整数。
本公开实施例中，位姿确定装置可以基于每个场景坐标点对应的更新场景坐标点对场景点云图与模型点云图进行配准，基于配准误差进行下一次迭代，直至第i次迭代满足预设迭代条件时，得到第i次变换矩阵，其中i为大于等于1的正整数。
本公开实施例中，基于图4，S1033可以如图7所示，通过S501-S504实现，如下：
S501、将每个场景坐标点对应的更新场景坐标点与每个场景坐标点对应的模型坐标点作为一对当前次对应点对，从而得到当前次对应点对集合。
本公开实施例中，对于一个场景坐标点对应的更新场景坐标点，位姿确定装置将该更新场景坐标点与该场景坐标点对应的模型坐标点所构成的新的对应点对，作为一个当前次对应点对。
本公开实施例中，位姿确定装置对于将每个场景坐标点对应的更新场景坐标点与每个场景坐标点对应的模型坐标点进行同样的处理，从而得到当前次对应点对集合，其中，当前次对应点对集合中的每对当前次对应点对包含一个更新场景坐标点和一个对应的模型坐标点。
在一些实施例中，对应点对集合为{pi,qi}，其中，每个场景坐标点pi对应的更新坐标点为p'i，每个场景坐标点pi对应的模型坐标点为qi，则位姿确定装置将{p'i,qi}作为当前次对应点对集合。
S502、计算当前次对应点对集合中每对当前次对应点对中的点之间的距离，得到当前次距离集合。
本公开实施例中，当前次对应点对集合中的每对当前次对应点对包含了一个更新场景坐标点与对应的模型坐标点，位姿确定装置可以根据每对当前次对应点对中更新场景坐标点的三维坐标值与对应的模型坐标点的三维坐标值及法向量，计算当前次对应点对集合中每对当前次对应点对中的点之间的距离，从而将每对当前次对应点对中的点之间的距离构成的集合作为当前次距离集合。
本公开实施例中，位姿确定装置可以通过多种度量方式，计算当前次对应点对集合中每对当前次对应点对中的点之间的距离。示例性的，位姿确定装置可以在每对当前次对应点对中，计算更新场景坐标点到对应的模型坐标点的点对点(point-to-point)的距离，也可以计算更新场景坐标点到对应的模型坐标点的切平面(point-to-plane)的距离，也可以计算更新场景坐标点的切平面到对应的模型坐标点的切平面(plane-to-plane)的距离，具体的根据实际情况进行选择，本公开实施例不作限定。
在一些实施例中，基于图7，S502中计算当前次对应点对集合中每对当前次对应点对中的点之间的距离，得到当前次距离集合具体可以如图8所示，包括S301-S302，如下：
S301、计算每对当前次对应点对中的更新场景坐标点到每对当前次对应点对中模型坐标点所在的切平面的距离，得到每对当前次对应点对的当前次距离。
本公开实施例中，对于计算更新场景坐标点与模型坐标点之间的距离，可以采用点到曲面(Point to Plane)的误差度量方法，示例性的，如图9所示，当前次对应点对集合中包含当前次对应点对{(s'1，d1)，(s'2，d2)，(s'3，d3)}，其中，更新场景坐标点s'1位于源曲面source surface上，模型坐标点d1位于目标曲面destination surface上。则对于更新场景坐标点s'1，计算s'1到d1所在曲面位置的切平面tangent plane的距离l1，作为当前次对应点对s'1与d1之间的距离，对于当前次对应点对(s'2，d2)，(s'3，d3)，以同样的方法计算得到l2和l3，将l1、l2和l3分别作为当前次对应点对(s'1，d1)，(s'2，d2)，(s'3，d3)对应的当前次距离。
本公开实施例中，S301中计算每对当前次对应点对中的更新场景坐标点到每对当前次对应点对中模型坐标点所在的切平面的距离具体可以如公式(13)所示：
l＝(s'i-di)·ni (13)
公式(13)中di为模型坐标点，ni为模型坐标点对应的法向量，在通过公式(12)得到更新场景坐标点s'i之后，将更新场景坐标点s'i与对应的模型坐标点的差值di乘以模型坐标点的法向量ni，可以计算出更新场景坐标点s'i到模型坐标点di所在的切平面之间的距离l。
S302、将每对当前次对应点对的当前次距离构成的集合，作为当前次距离集合。
本公开实施例中，位姿确定装置得到每对当前次对应点对的当前次距离之后，将每对当前次对应点对的当前次距离构成的集合作为当前次距离集合。
在一些实施例中，l1、l2和l3分别为当前次对应点对集合中每对当前次对应点对(s'1，d1)，(s'2，d2)，(s'3，d3)对应的当前次距离，则{l1，l2，l3}为当前次距离集合。
S503、基于当前次距离集合，得到场景坐标点集合到模型坐标点集合之间的当前次距离误差。
本公开实施例中，在得到当前次距离集合之后，位姿确定装置基于当前次距离集合中的全部当前次距离，对场景坐标点和模型坐标点之间的距离进行评估，得到场景坐标点集合到模型坐标点集合之间的当前次距离误差。
在一些实施例中，位姿确定装置可以对当前次距离集合进行均值化处理来得到当前次距离误差，均值化方法可以是取全部当前次距离的平均值，也可以是取全部当前次距离的平方和，以对当前次距离的变异或离差进行度量，或者是其他均值化方法，本公开实施例不做限定。
在一些实施例中，位姿确定装置也可以根据实际应用的需要，通过其他方式基于当前次距离集合得到当前次距离误差，示例性的，取最大值作为当前次距离误差等等，具体的根据实际情况进行选择，本公开实施例不做限定。
本公开实施例中，基于图7，S503中基于当前次距离集合，得到场景坐标点集合到模型坐标点集合之间的当前次距离误差具体可以包括S401，如下：
S401、对当前次距离集合中的每个当前次距离的平方进行求和，将求和结果作为当前次距离误差。
本公开实施例中，可以根据公式(14)得到当前次距离误差，如下：
公式(14)中，e为当前次距离集合对应的当前次距离误差。
S504、响应于当前次距离误差不满足预设迭代条件的情况，基于每个场景坐标点对应的更新场景坐标点更新当前次对应点对集合，得到下一次对应点对集合，基于下一次对应点对集合进行下一次迭代，直至第i次迭代满足预设迭代条件时，得到第i次变换矩阵。
本公开实施例中，在得到当前次距离误差之后，位姿确定装置可以根据当前次距离误差判断是否满足预设迭代条件，若当前次距离误差不满足预设迭代条件，则基于每个场景坐标点对应的更新场景坐标点更新当前次对应点对集合，得到下一次对应点对集合，之后下一次更新对应点对集合进行下一次迭代，直至第i次迭代的距离误差满足预设迭代条件时，得到第i次变换矩阵。
本公开实施例中，预设迭代条件可以是预设的当前次距离误差的阈值，也可以设置迭代次数上限，在迭代次数达到上限时判断满足预设迭代条件，具体的根据实际情况进行选择，本公开实施例不做限定。
本公开实施例中，当当前次距离误差不满足预设迭代条件时，说明根据对应点对集合计算出的当前次变换矩阵误差较大，不能准确的表征场景坐标点与模型坐标点之间的关系，因此，位姿确定装置可以在基于当前次变化矩阵所得到的每个场景坐标点对应的更新场景坐标点上，在模型点云图中再选取出每个更新场景坐标点对应的坐标点，作为更新模型坐标点，将每个更新场景坐标点与对应的更新模型坐标点作为下一次对应点对集合，基于下一次对应点对集合开始下一次迭代，直至第i次迭代对应的第i次距离误差满足预设迭代条件时，停止迭代过程，得到第i次变换矩阵。
本公开实施例中，由于更新场景坐标点是场景坐标点经过当前次变换矩阵的变换得来的，即更新场景坐标点构成的更新场景坐标点集合与模型点云图的配准度更高，因此位姿确定装置基于每个场景坐标点对应的更新场景坐标点再进行模型点云图中模型坐标点集合的选取以及进行下一次迭代，可以逐步将更新场景坐标点集合向模型坐标点集合进行配准，减小两者之间的当前次距离误差，因此，当第i次迭代计算得到的第i次距离误差就是全部i次迭代过程中得到的最小距离误差，位姿确定装置根据第i迭代计算得到预设矩阵模型的矩阵参数得到第i次变换矩阵。
在一些实施例中，位姿确定装置也可以在每一次迭代过程中，以最小化每次迭代过程中的当前次距离误差为迭代目标，在全部i次迭代过程结束之后，在得到的全部i次距离误差中找出最小距离误差。
本公开实施例中，位姿确定装置也可以使用公式(15)作为优化函数，在每次迭代过程中对下一次迭代的距离误差进行优化，最终在满足预设迭代条件的第i次迭代中得到全部i次迭代过程中最小的距离误差：
公式(15)中，Mopt为使得i次距离误差达到最小值的最优变换矩阵，也是迭代过程最终要求得的变换矩阵。
本公开实施例中，由于当前次变换矩阵模型是对预设变换矩阵模型进行的近似化处理后计算得到的，因此使用公式(10)对公式(15)进行变形，可以将公式(15)中对Mopt的求解转换为公式(16)中对xopt进行求解，将xopt作为迭代过程最终要求得的变换矩阵：
xopt＝argminx|Ax-b|2 (16)
对公式(16)进行伪逆变换，得到公式(17)，如下：
xopt＝A+b (17)
对公式(17)进行SVD分解，即可求得场景点云图与所述模型点云图之间对应的变换矩阵xopt。
S1034、将第i次变换矩阵作为空间变换关系。
本公开实施例中，最小的第i次距离误差表征第i次迭代过程中所求解出的第i次变换矩阵最符合场景点云图与模型点云图之间的空间位置变换关系，因此将迭代过程中得到的第i次距离误差对应的第i次矩阵作为空间变换关系。
可以理解的是，本公开实施例中，在每次迭代过程中，通过图形处理器的并行处理的物理特征，对预设变换矩阵模型中每个位置的矩阵参数同时进行并行计算，避免了对预设变换矩阵模型中每个位置的矩阵参数进行一一计算，加快了当前次变换矩阵的求解，可以快速得到场景坐标点集合与模型坐标点集合之间的空间变换关系，从而最终提高了确定位姿的效率。
本公开实施例中，基于图1，S104之后，还可以如图10所示，包括S105，如下：
S105、基于相对位姿，进行场景定位、三维建模和/或虚拟现实仿真过程。
本公开实施例中，位姿确定装置确定出场景点云图与模型点云图之间的相对位姿，可以基于相对位姿进行多种图形视觉化处理，示例性的，位姿确定装置可以通过相对位姿获得场景点云图相对于模型点云图的旋转和平移，进而确定场景点云图中每个场景坐标点在模型点云图坐标系中的位置，最终实现场景定位。
在一些实施例中，位姿确定装置还可以基于确定的相对位置实现三维建模与虚拟现实仿真等过程，具体的对相对位姿的应用过程不在本公开实施例中限定。
本公开实施例提供了一种位姿确定装置，对应于一种位姿确定方法，图11为本公开实施例提供的一种位姿确定装置的结构示意图，如图11所示，该位姿确定装置1包括：提取单元100、选取单元200、迭代单元300与确定单元400，其中，
所述提取单元100，用于根据真实场景图像生成场景点云图，并从所述场景点云图中提取场景坐标点集合；
所述选取单元200，用于获取与所述场景点云图对应的模型点云图，并从所述模型点云图中并行选取出与所述场景坐标点集合对应的模型坐标点集合；
所述迭代单元300，用于基于所述场景坐标点与所述模型坐标点集合，通过并行迭代计算，求取出所述场景点云图与所述模型点云图之间对应的空间变换关系；
所述确定单元400，用于根据所述空间变换关系，确定所述场景点云图与所述模型点云图之间的相对位姿。
在一些实施例中，迭代单元300，还用于在一次迭代过程中，通过图形处理器，基于对应点对集合与预设变换矩阵模型满足的空间对应关系，对所述预设变换矩阵模型中的矩阵参数进行并行计算，得到当前次变换矩阵；所述空间对应关系为通过所述预设变换矩阵模型建立的，所述对应点对集合中的场景坐标点集合与模型坐标点集合之间的对应关系；所述对应点对集合为所述场景坐标点集合中的每个场景坐标点与其在所述模型坐标点集合中对应的模型坐标点构成的点对集合；根据所述当前次变换矩阵，对所述对应点对集合中的每个场景坐标点进行坐标变换，得到所述每个场景坐标点对应的更新场景坐标点；基于所述每个场景坐标点对应的更新场景坐标点进行下一次迭代，直至第i次迭代满足预设迭代条件时，得到第i次变换矩阵；其中，i为大于等于1的正整数；将所述第i次变换矩阵作为所述空间变换关系。
在一些实施例中，所述迭代单元300，还用于将所述每个场景坐标点对应的更新场景坐标点与所述每个场景坐标点对应的模型坐标点作为一对当前次对应点对，从而得到当前次对应点对集合；计算所述当前次对应点对集合中每对当前次对应点对中的点之间的距离，得到当前次距离集合；基于所述当前次距离集合，得到所述场景坐标点集合到所述模型坐标点集合之间的当前次距离误差；响应于所述当前次距离误差不满足预设迭代条件的情况，基于所述每个场景坐标点对应的更新场景坐标点更新所述当前次对应点对集合，得到下一次对应点对集合，基于所述下一次对应点对集合进行下一次迭代，直至第i次迭代满足预设迭代条件时，得到所述第i次变换矩阵。
在一些实施例中，所述选取单元200，还用于对于所述场景坐标点集合中的每个场景坐标点，通过图形处理器在所述模型点云图中进行并行搜索，同步选取出与所述每个场景坐标点的距离最近的坐标点；
将与所述每个场景坐标点的距离最近的坐标点，作为所述每个场景坐标点对应的模型坐标点，从而得到所述模型坐标点集合。
在一些实施例中，所述迭代单元300，还用于计算所述每对当前次对应点对中的更新场景坐标点到所述每对当前次对应点对中模型坐标点所在的切平面的距离，得到所述每对当前次对应点对的当前次距离；将所述每对当前次对应点对的当前次距离构成的集合，作为所述当前次距离集合。
在一些实施例中，所述迭代单元300，还用于对所述当前次距离集合中的每个当前次距离的平方进行求和，将求和结果作为所述当前次距离误差。
在一些实施例中，所述迭代单元300，还用于对所述预设变换矩阵模型进行近似处理，得到近似变换矩阵模型；所述近似处理为将场景坐标系对应于模型坐标系的旋转角度预设为零；所述场景坐标系为所述场景坐标点所在的坐标系，所述模型坐标系为所述模型坐标点所在的坐标系；将所述近似变换矩阵模型代入所述空间对应关系，得到所述对应点对集合与所述近似变换矩阵满足的近似空间对应关系；基于所述对应点对集合与所述近似空间对应关系，对所述近似变换矩阵模型中的矩阵参数进行并行计算，得到所述当前次变换矩阵。
在一些实施例中，所述预设变换矩阵模型包括旋转矩阵模型与平移矩阵模型，所述迭代单元300，还用于对所述旋转矩阵模型进行近似处理，得到近似旋转矩阵模型；使用所述近似旋转矩阵模型更新所述预设变换矩阵模型中的旋转矩阵模型，得到近似变换矩阵模型。
在一些实施例中，所述迭代单元300，还用于获取所述对应点对集合中每对对应点对的场景位置信息与模型位置信息；所述场景位置信息为所述每对对应点对中场景坐标点的位置信息；所述模型位置信息为所述每对对应点对中模型坐标点的位置信息；将所述每对对应点对的场景位置信息与模型位置信息分别代入所述近似空间对应关系中，同步计算得到旋转矩阵参数和平移矩阵参数；所述旋转矩阵参数为所述近似变换矩阵中表征旋转角度变换的参数，所述平移矩阵参数为所述近似变换矩阵中表征平移变换的参数；采用所述旋转矩阵参数更新所述旋转矩阵模型，得到当前次旋转矩阵；采用所述平移矩阵参数更新所述平移矩阵模型，得到当前次平移矩阵；将所述当前次旋转矩阵与所述当前次平移矩阵相乘，得到所述当前次变换矩阵。
在一些实施例中，所述迭代单元300，还用于若所述当前次距离误差不满足预设迭代条件，通过所述图形处理器，从所述模型点云图中并行选取出与所述每个场景坐标点的更新场景坐标点对应的更新模型坐标点，将每个更新场景坐标点与每个更新模型坐标点构成的集合作为所述下一次对应点对集合，基于所述下一次对应点对集合进行下一次迭代，得到下一次迭代对应的下一次距离误差，直至第i次迭代对应的第i次距离误差满足预设迭代条件时，得到所述第i次变换矩阵。
在一些实施例中，所述迭代单元300，还用于对于一对当前次对应点对，将该当前次对应点对中的更新场景坐标点与该当前次对应点对中的模型坐标点的差值乘以所述模型坐标点的法向量，得到该当前次对应点对之间的距离，从而得到所述每对当前次对应点对的所述当前次距离。
在一些实施例中，所述位姿确定装置1还包括定位单元，所述定位单元，用于基于所述相对位姿，进行场景定位、三维建模和/或虚拟现实仿真过程。
需要说明的是，在实际应用中，上述提取单元100、选取单元200、迭代单元300与确定单元400可由位于电子设备2上的处理器54实现，具体为中央处理器(CentralProcessing Unit，CPU)、微处理器(Microprocessor Unit，MPU)、数字信号处理器(DigitalSignal Processing，DSP)或现场可编程门阵列(Field Programmable Gate Array，FPGA)等实现。
需要说明的是，以上装置实施例的描述，与上述方法实施例的描述是类似的，具有同方法实施例相似的有益效果。对于本发明装置实施例中未披露的技术细节，请参照本发明方法实施例的描述而理解。
本公开实施例提供一种电子设备，对应于一种位姿确定方法；图12为本公开实施例提供的一种电子设备的结构示意图，如图12所示，该电子设备2包括：处理器54、存储器55和通信总线56，存储器55通过通信总线56与处理器54进行通信，存储器55存储处理器54可执行的一个或者多个程序，当一个或者多个程序被执行时，通过处理器54执行如前述实施例的任意一种位姿确定方法。
本公开实施例提供了一种计算机可读存储介质，计算机可读存储介质存储有一个或者多个程序，一个或者多个程序可被一个或者多个处理器54执行，程序被处理器54执行时实现如本公开实施例的位姿确定方法。
本领域内的技术人员应明白，本公开的实施例可提供为方法、系统、或计算机程序产品。因此，本公开可采用硬件实施例、软件实施例、或结合软件和硬件方面的实施例的形式。而且，本公开可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器和光学存储器等)上实施的计算机程序产品的形式。
本公开是参照根据本公开实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
以上所述，仅为本公开的较佳实施例而已，并非用于限定本公开的保护范围。
Claims (14)
1.一种位姿确定方法，其特征在于，包括：
根据真实场景图像生成场景点云图，并从所述场景点云图中提取场景坐标点集合；
获取与所述场景点云图对应的模型点云图，并从所述模型点云图中并行选取出与所述场景坐标点集合对应的模型坐标点集合；
基于所述场景坐标点集合与所述模型坐标点集合，通过并行迭代计算，求取出所述场景点云图与所述模型点云图之间对应的空间变换关系；
根据所述空间变换关系，确定所述场景点云图与所述模型点云图之间的相对位姿；
所述基于所述场景坐标点集合与所述模型坐标点集合，通过并行迭代计算，求取出所述场景点云图与所述模型点云图之间对应的空间变换关系，包括：
在一次迭代过程中，通过图形处理器，基于对应点对集合与预设变换矩阵模型满足的空间对应关系，所述预设变换矩阵的场景坐标系对应于旋转角度为零的模型坐标系；对所述预设变换矩阵模型中的矩阵参数进行并行计算，得到当前次变换矩阵；所述空间对应关系为通过所述预设变换矩阵模型建立的，所述对应点对集合中的场景坐标点集合与模型坐标点集合之间的对应关系；所述对应点对集合为所述场景坐标点集合中的每个场景坐标点与其在所述模型坐标点集合中对应的模型坐标点构成的点对集合；
根据所述当前次变换矩阵，对所述对应点对集合中的每个场景坐标点进行坐标变换，得到所述每个场景坐标点对应的更新场景坐标点；
基于所述每个场景坐标点对应的更新场景坐标点进行下一次迭代，直至第i次迭代满足预设迭代条件时，得到第i次变换矩阵；其中，i为大于等于1的正整数；
将所述第i次变换矩阵作为所述空间变换关系。
2.根据权利要求1所述的方法，其特征在于，所述基于所述每个场景坐标点对应的更新场景坐标点进行下一次迭代，直至第i次迭代满足预设迭代条件时，将第i次变换矩阵作为所述空间变换关系，包括：
将所述每个场景坐标点对应的更新场景坐标点与所述每个场景坐标点对应的模型坐标点作为一对当前次对应点对，从而得到当前次对应点对集合；
计算所述当前次对应点对集合中每对当前次对应点对中的点之间的距离，得到当前次距离集合；
基于所述当前次距离集合，得到所述场景坐标点集合到所述模型坐标点集合之间的当前次距离误差；
响应于所述当前次距离误差不满足预设迭代条件的情况，基于所述每个场景坐标点对应的更新场景坐标点更新所述当前次对应点对集合，得到下一次对应点对集合，基于所述下一次对应点对集合进行下一次迭代，直至第i次迭代满足预设迭代条件时，得到所述第i次变换矩阵。
3.根据权利要求1所述的方法，其特征在于，所述从所述模型点云图中并行选取出与所述场景坐标点集合对应的模型坐标点集合，包括：
对于所述场景坐标点集合中的每个场景坐标点，通过图形处理器在所述模型点云图中进行并行搜索，同步选取出与所述每个场景坐标点的距离最近的坐标点；
将与所述每个场景坐标点的距离最近的坐标点，作为所述每个场景坐标点对应的模型坐标点，从而得到所述模型坐标点集合。
4.根据权利要求2所述的方法，其特征在于，所述计算所述当前次对应点对集合中每对当前次对应点对中的点之间的距离，得到当前次距离集合，包括：
计算所述每对当前次对应点对中的更新场景坐标点到所述每对当前次对应点对中模型坐标点所在的切平面的距离，得到所述每对当前次对应点对的当前次距离；
将所述每对当前次对应点对的当前次距离构成的集合，作为所述当前次距离集合。
5.根据权利要求2或4所述的方法，所述基于所述当前次距离集合，得到所述场景坐标点集合到所述模型坐标点集合之间的当前次距离误差，包括：
对所述当前次距离集合中的每个当前次距离的平方进行求和，将求和结果作为所述当前次距离误差。
6.根据权利要求1所述的方法，其特征在于，所述基于所述对应点对集合与预设变换矩阵模型满足的空间对应关系，对所述预设变换矩阵模型中的矩阵参数进行并行计算，得到当前次变换矩阵，包括：
对所述预设变换矩阵模型进行近似处理，得到近似变换矩阵模型；所述近似处理为将场景坐标系对应于模型坐标系的旋转角度预设为零；所述场景坐标系为所述场景坐标点所在的坐标系，所述模型坐标系为所述模型坐标点所在的坐标系；
将所述近似变换矩阵模型代入所述空间对应关系，得到所述对应点对集合与所述近似变换矩阵满足的近似空间对应关系；
基于所述对应点对集合与所述近似空间对应关系，对所述近似变换矩阵模型中的矩阵参数进行并行计算，得到所述当前次变换矩阵。
7.根据权利要求6所述的方法，其特征在于，所述预设变换矩阵模型包括：旋转矩阵模型与平移矩阵模型，所述对所述预设变换矩阵模型进行近似处理，得到近似变换矩阵模型，包括：
对所述旋转矩阵模型进行近似处理，得到近似旋转矩阵模型；
使用所述近似旋转矩阵模型更新所述预设变换矩阵模型中的旋转矩阵模型，得到近似变换矩阵模型。
8.根据权利要求6所述的方法，其特征在于，所述基于所述对应点对集合与所述近似空间对应关系，对所述近似变换矩阵模型中的矩阵参数进行并行计算，得到当前次变换矩阵，包括：
获取所述对应点对集合中每对对应点对的场景位置信息与模型位置信息；所述场景位置信息为所述每对对应点对中场景坐标点的位置信息；所述模型位置信息为所述每对对应点对中模型坐标点的位置信息；
将所述每对对应点对的场景位置信息与模型位置信息分别代入所述近似空间对应关系中，同步计算得到旋转矩阵参数和平移矩阵参数；所述旋转矩阵参数为所述近似变换矩阵中表征旋转角度变换的参数，所述平移矩阵参数为所述近似变换矩阵中表征平移变换的参数；
采用所述旋转矩阵参数更新旋转矩阵模型，得到当前次旋转矩阵；
采用所述平移矩阵参数更新平移矩阵模型，得到当前次平移矩阵；
将所述当前次旋转矩阵与所述当前次平移矩阵相乘，得到所述当前次变换矩阵。
9.根据权利要求2所述的方法，其特征在于，所述响应于所述当前次距离误差不满足预设迭代条件的情况，基于所述每个场景坐标点的更新场景坐标点更新所述当前次对应点对集合，得到下一次对应点对集合，基于所述下一次对应点对集合进行下一次迭代，直至第i次迭代满足预设迭代条件时，得到所述第i次变换矩阵，包括：
若所述当前次距离误差不满足预设迭代条件，通过所述图形处理器，从所述模型点云图中并行选取出与所述每个场景坐标点的更新场景坐标点对应的更新模型坐标点，将每个更新场景坐标点与每个更新模型坐标点构成的集合作为所述下一次对应点对集合，基于所述下一次对应点对集合进行下一次迭代，得到下一次迭代对应的下一次距离误差，直至第i次迭代对应的第i次距离误差满足预设迭代条件时，得到所述第i次变换矩阵。
10.根据权利要求4所述的方法，其特征在于，所述计算所述每对当前次对应点对中的更新场景坐标点到所述每对当前次对应点对中模型坐标点所在的切平面的距离，得到所述每对当前次对应点对的当前次距离，包括：
对于一对当前次对应点对，将该当前次对应点对中的更新场景坐标点与该当前次对应点对中的模型坐标点的差值乘以所述模型坐标点的法向量，得到该当前次对应点对之间的距离，从而得到所述每对当前次对应点对的所述当前次距离。
11.根据权利要求1-10任一项所述的方法，其特征在于，所述根据所述空间变换关系，确定所述场景点云图与所述模型点云图之间的相对位姿之后，所述方法还包括：
基于所述相对位姿，进行场景定位、三维建模和/或虚拟现实仿真过程。
12.一种位姿确定装置，其特征在于，所述位姿确定装置包括：提取单元、选取单元、迭代单元与确定单元，其中，
所述提取单元，用于根据真实场景图像生成场景点云图，并从所述场景点云图中提取场景坐标点集合；
所述选取单元，用于获取与所述场景点云图对应的模型点云图，并从所述模型点云图中并行选取出与所述场景坐标点集合对应的模型坐标点集合；
所述迭代单元，用于基于所述场景坐标点集合与所述模型坐标点集合，通过并行迭代计算，求取出所述场景点云图与所述模型点云图之间对应的空间变换关系；还用于在一次迭代过程中，通过图形处理器，基于对应点对集合与预设变换矩阵模型满足的空间对应关系，所述预设变换矩阵的场景坐标系对应于旋转角度为零的模型坐标系；对所述预设变换矩阵模型中的矩阵参数进行并行计算，得到当前次变换矩阵；所述空间对应关系为通过所述预设变换矩阵模型建立的，所述对应点对集合中的场景坐标点集合与模型坐标点集合之间的对应关系；所述对应点对集合为所述场景坐标点集合中的每个场景坐标点与其在所述模型坐标点集合中对应的模型坐标点构成的点对集合；根据所述当前次变换矩阵，对所述对应点对集合中的每个场景坐标点进行坐标变换，得到所述每个场景坐标点对应的更新场景坐标点；基于所述每个场景坐标点对应的更新场景坐标点进行下一次迭代，直至第i次迭代满足预设迭代条件时，得到第i次变换矩阵；其中，i为大于等于1的正整数；将所述第i次变换矩阵作为所述空间变换关系；
所述确定单元，用于根据所述空间变换关系，确定所述场景点云图与所述模型点云图之间的相对位姿。
13.一种电子设备，其特征在于，所述电子设备包括：处理器、存储器以及通信总线，所述存储器通过所述通信总线与所述处理器进行通信，所述存储器存储所述处理器可执行的一个或者多个程序，当所述一个或者多个程序被执行时，所述处理器执行如权利要求1-11任一项所述的方法。
14.一种存储介质，其特征在于，所述存储介质存储有一个或者多个程序，所述一个或者多个程序可被一个或者多个处理器执行，以实现如权利要求1-11任一项所述的方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011003842.4A CN111932628B (zh) | 2020-09-22 | 2020-09-22 | 一种位姿确定方法及装置、电子设备、存储介质 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011003842.4A CN111932628B (zh) | 2020-09-22 | 2020-09-22 | 一种位姿确定方法及装置、电子设备、存储介质 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN111932628A CN111932628A (zh) | 2020-11-13 |
| CN111932628B true CN111932628B (zh) | 2025-02-21 |
Family
ID=73334055
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202011003842.4A Active CN111932628B (zh) | 2020-09-22 | 2020-09-22 | 一种位姿确定方法及装置、电子设备、存储介质 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111932628B (zh) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112561998B (zh) * | 2020-12-16 | 2024-02-20 | 国网江苏省电力有限公司检修分公司 | 一种基于三维点云配准的机器人定位和自主充电方法 |
| CN114241316A (zh) * | 2021-12-22 | 2022-03-25 | 杭州萤石软件有限公司 | 一种物体空间信息标签的生成方法、装置、存储介质和电子设备 |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106803270A (zh) * | 2017-01-13 | 2017-06-06 | 西北工业大学深圳研究院 | 无人机平台基于单目slam的多关键帧协同地面目标定位方法 |
| CN107292965A (zh) * | 2017-08-03 | 2017-10-24 | 北京航空航天大学青岛研究院 | 一种基于深度图像数据流的虚实遮挡处理方法 |
Family Cites Families (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2018189510A (ja) * | 2017-05-08 | 2018-11-29 | 株式会社マイクロ・テクニカ | ３次元物体の位置および姿勢を推定する方法および装置 |
| CN109297510B (zh) * | 2018-09-27 | 2021-01-01 | 百度在线网络技术（北京）有限公司 | 相对位姿标定方法、装置、设备及介质 |
| JP7159033B2 (ja) * | 2018-12-21 | 2022-10-24 | 株式会社日立製作所 | ３次元位置・姿勢認識装置及び方法 |
| CN111091062B (zh) * | 2019-11-21 | 2023-07-04 | 东南大学 | 一种基于3d视觉聚类和匹配的机器人乱序目标分拣方法 |
| CN111598946B (zh) * | 2020-03-16 | 2023-03-21 | 华南理工大学 | 一种物体位姿测量方法、装置及存储介质 |
| CN111474932B (zh) * | 2020-04-23 | 2021-05-11 | 大连理工大学 | 一种集成情景经验的移动机器人建图与导航方法 |
-
2020
- 2020-09-22 CN CN202011003842.4A patent/CN111932628B/zh active Active
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106803270A (zh) * | 2017-01-13 | 2017-06-06 | 西北工业大学深圳研究院 | 无人机平台基于单目slam的多关键帧协同地面目标定位方法 |
| CN107292965A (zh) * | 2017-08-03 | 2017-10-24 | 北京航空航天大学青岛研究院 | 一种基于深度图像数据流的虚实遮挡处理方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN111932628A (zh) | 2020-11-13 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN106940704B (zh) | 一种基于栅格地图的定位方法及装置 | |
| CN107358629B (zh) | 一种基于目标识别的室内建图与定位方法 | |
| CN107679537B (zh) | 一种基于轮廓点orb特征匹配的无纹理空间目标姿态估计算法 | |
| CN108764048A (zh) | 人脸关键点检测方法及装置 | |
| CN112651944B (zh) | 基于cad模型的3c部件高精度六维位姿估计方法及系统 | |
| CN113781622B (zh) | 三维模型纹理贴图的转换方法、装置、设备以及介质 | |
| US20230169677A1 (en) | Pose Estimation Method and Apparatus | |
| CN111754579A (zh) | 多目相机外参确定方法及装置 | |
| CN113361365B (zh) | 定位方法和装置、设备及存储介质 | |
| CN111932628B (zh) | 一种位姿确定方法及装置、电子设备、存储介质 | |
| JP2023502819A (ja) | 視覚位置決め方法、関連モデルの訓練方法及び関連装置並びに機器 | |
| CN112967340A (zh) | 同时定位和地图构建方法、装置、电子设备及存储介质 | |
| CN113393524A (zh) | 一种结合深度学习和轮廓点云重建的目标位姿估计方法 | |
| JP2019109747A (ja) | 位置姿勢推定装置、位置姿勢推定方法及びプログラム | |
| JP4921847B2 (ja) | 対象物の三次元位置推定装置 | |
| CN106408654B (zh) | 一种三维地图的创建方法及系统 | |
| CN113671523A (zh) | 机器人定位方法、装置、存储介质及机器人 | |
| JP2778430B2 (ja) | 視覚に基く三次元位置および姿勢の認識方法ならびに視覚に基く三次元位置および姿勢の認識装置 | |
| CN115131433B (zh) | 一种非合作目标位姿的处理方法、装置及电子设备 | |
| CN116817920A (zh) | 无需三维地图模型的平面移动机器人视觉定位方法及装置 | |
| CN110135340A (zh) | 基于点云的3d手部姿态估计方法 | |
| CN112907669A (zh) | 基于共面特征点的相机位姿测量方法及装置 | |
| CN119205855B (zh) | 点云图像的配准方法、装置、电子设备和存储介质 | |
| JP5991730B2 (ja) | 計測装置、計測方法及びプログラム | |
| JP2018200175A (ja) | 情報処理装置、情報処理方法及びプログラム |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |