定位方法及装置、设备、存储介质与流程

本申请实施例涉及电子技术，涉及但不限于定位方法及装置、设备、存储介质。

背景技术：

基于图像信息进行定位的相关技术中，目前主要是通过识别摄像头采集到的图像中的人物和固定物体，以确定所述人物的位置。该方案将所述固定物体与预先构建的室内地图进行匹配，从而确定所述固定物体在室内的对应位置；然后根据所述固定物体的位置确定所述人物在室内的位置；其中，确定所述人物的位置的整体思路为，通过图像识别的方法识别图像中的固定物体，并根据所述固定物体与图像中的人物之间的相对位置关系、和所述固定物体在室内的位置，确定所述人物的位置。

然而，这种定位方法主要根据图像中的人物和固定物体之间的相对位置关系进行定位，这样，在实现定位时就要求图像中必须有可以识别出的人物和固定物体，否则定位就会失效，所以这种定位方法的鲁棒性较差。

技术实现要素：

本申请实施例提供定位方法及装置、设备、存储介质。本申请实施例的技术方案是这样实现的：

第一方面，本申请实施例提供一种定位方法，所述方法包括：根据图像采集模组的内参矩阵和所述图像采集模组采集的深度图像中多个像素点的深度值，将所述多个像素点的像素坐标转换为相机坐标；将每一所述像素点的相机坐标与预先构建的点云地图中多个体素的目标世界坐标进行匹配，得出所述图像采集模组的位姿信息；其中，每一所述体素的目标世界坐标是根据多个样本图像对，更新每一所述体素的初始世界坐标而获得的，所述样本图像对包括二维样本图像和深度样本图像。

第二方面，本申请实施例提供一种定位装置，包括：坐标转换模块，配置为根据图像采集模组的内参矩阵和所述图像采集模组采集的深度图像中多个像素点的深度值，将所述多个像素点的像素坐标转换为相机坐标；定位模块，配置为将每一所述像素点的相机坐标与预先构建的点云地图中多个体素的目标世界坐标进行匹配，得出所述图像采集模组的位姿信息；其中，每一所述体素的目标世界坐标是根据多个样本图像对，更新每一所述体素的初始世界坐标而获得的，所述样本图像对包括二维样本图像和深度样本图像。

第三方面，本申请实施例提供一种电子设备，包括存储器和处理器，所述存储器存储有可在处理器上运行的计算机程序，所述处理器执行所述程序时实现所述定位方法中的步骤。

第四方面，本申请实施例提供一种计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现所述定位方法中的步骤。

本申请实施例中，根据图像采集模组采集的深度图像中多个像素点的相机坐标与预先构建的点云地图中多个体素的目标世界坐标进行匹配，即可确定图像采集模组的位姿信息；如此，在对所述图像采集模组进行定位时，其定位方法不依赖于所述深度图像中必须有固定物体和待定位的人物，所以能够获得较好的鲁棒性。

附图说明

图1为本申请实施例定位方法的实现流程示意图；

图2为本申请实施例点云地图的构建过程示意图；

图3为本申请实施例对特定物理空间进行量化的实现示意图；

图4a为本申请实施例定位装置的组成结构示意图；

图4b为本申请实施例另一定位装置的组成结构示意图；

图5为本申请实施例电子设备的一种硬件实体示意图。

具体实施方式

为使本申请实施例的目的、技术方案和优点更加清楚，下面将结合本申请实施例中的附图，对本申请的具体技术方案做进一步详细描述。以下实施例用于说明本申请，但不用来限制本申请的范围。

除非另有定义，本文所使用的所有的技术和科学术语与属于本申请的技术领域的技术人员通常理解的含义相同。本文中所使用的术语只是为了描述本申请实施例的目的，不是旨在限制本申请。

在以下的描述中，涉及到“一些实施例”，其描述了所有可能实施例的子集，但是可以理解，“一些实施例”可以是所有可能实施例的相同子集或不同子集，并且可以在不冲突的情况下相互结合。

需要指出，本申请实施例所涉及的术语“第一\第二\第三”仅仅是是区别不同的对象，不代表针对对象的特定排序，可以理解地，“第一\第二\第三”在允许的情况下可以互换特定的顺序或先后次序，以使这里描述的本申请实施例能够以除了在这里图示或描述的以外的顺序实施。

本申请实施例提供一种定位方法，所述方法可以应用于电子设备，所述电子设备可以是手机、平板电脑、笔记本电脑、台式计算机、服务器、机器人、无人机等具有信息处理能力的设备。所述定位方法所实现的功能可以通过所述电子设备中的处理器调用程序代码来实现，当然程序代码可以保存在计算机存储介质中，可见，所述电子设备至少包括处理器和存储介质。

图1为本申请实施例定位方法的实现流程示意图，如图1所示，所述方法至少包括以下步骤s101至步骤s102：

步骤s101，根据图像采集模组的内参矩阵和所述图像采集模组采集的深度图像中多个像素点的深度值，将所述多个像素点的像素坐标转换为相机坐标。

所述图像采集模组一般包括第一摄像头和第二摄像头；其中，第一摄像头用于采集拍摄场景的二维图像，例如通过单目摄像头采集拍摄场景的红、绿、蓝(red、green、blue，rgb)图像；第二摄像头用于采集拍摄场景的深度图像，第二摄像头可以为双目相机、结构光相机或者飞行时间(time-of-flight，tof)相机等三维视觉传感器。需要说明的是，所述电子设备可以包括所述图像采集模组，也就是说，所述图像采集模组被安装在所述电子设备中，例如，所述电子设备为具有第一摄像头和第二摄像头的智能手机；当然，在其他实施例中，所述电子设备也可以不包括所述图像采集模组，所述图像采集模组可以将自身的内参矩阵和采集的深度图像发送给所述电子设备。

在实现时，如果已知图像采集模组的焦距f和图像中心点的像素坐标(u0,v0)，如下式(1)所示，图像采集模组的内参矩阵f为：

式中，表示焦距f在相机坐标系的x轴上的焦距分量；表示焦距f在相机坐标系的y轴上的焦距分量。

深度图像又被称为距离图像，是指将图像采集模组到拍摄场景中物体表面各点的距离作为像素值的图像，也就是说，深度图像中像素点的像素值为在三维视觉传感器的视野中，物体表面上的某个点到该传感器镜面的距离，一般将该像素值称为深度值。因此，在已知像素点j的深度值zc的情况下，可以根据如下公式(2)，将像素点j的像素坐标(u,v)转换为相机坐标(xc,yc,zc)：

需要说明的是，由于相机坐标系的z轴为镜头的光轴，因此像素点j的深度值即为该像素点的相机坐标的z轴坐标值zc。

步骤s102，将每一所述像素点的相机坐标与预先构建的点云地图中多个体素的目标世界坐标进行匹配，得出所述图像采集模组的位姿信息。

一般来说，所述体素的目标世界坐标为所述体素的某一个点(例如中心点)的世界坐标。在点云地图中，每一所述体素的目标世界坐标是所述电子设备根据多个包括二维样本图像和深度样本图像的样本图像对，更新每一所述体素的初始世界坐标而获得的。点云地图的构建过程可以包括如下实施例中的步骤s501至步骤s503。

在实现时，所述位姿信息包括世界坐标；或者，所述位姿信息包括世界坐标和所述图像采集模组在所述点云地图中的朝向。所述世界坐标可以是二维坐标，也可以是三维坐标。可以理解地，通过步骤s102得出所述图像采集模组在所述点云地图中的朝向，如此使得定位方法能够适用于更多的应用场景。例如，根据机器人的当前朝向，指示机器人执行下一个动作。

在本申请实施例中，根据图像采集模组采集的深度图像中多个像素点的相机坐标与预先构建的点云地图中多个体素的目标世界坐标进行匹配，即可确定图像采集模组的位姿信息；如此，在对所述图像采集模组进行定位时，其定位方法不依赖于所述深度图像中必须有固定物体和待定位的人物，所以能够获得较好的鲁棒性。

本申请实施例再提供一种定位方法，所述方法至少包括以下步骤s201至步骤s203：

步骤s201，根据图像采集模组的内参矩阵和所述图像采集模组采集的深度图像中多个像素点的深度值，将所述多个像素点的像素坐标转换为相机坐标。

步骤s202，根据迭代策略，将每一所述像素点的相机坐标与预先构建的点云地图中多个体素的目标世界坐标进行匹配，得出相机坐标系相对于世界坐标系的目标变换关系。

在本申请实施例中，点云地图中包括体素的目标世界坐标，但不包括体素的图像特征；如此，可以大大降低点云地图的数据量，从而节约点云地图在电子设备中的存储空间。

在点云地图不包括体素的图像特征的情况下，即，在已知图像采集模组采集的深度图像中每一像素点的相机坐标和点云地图中多个体素的目标世界坐标的前提下，通过迭代策略，尝试寻找相机坐标系相对于世界坐标系的目标变换关系，即可实现对图像采集模组的定位。针对目标变换关系的寻找，例如，通过如下实施例步骤s302至步骤s306、或者步骤s402至步骤s409，迭代地寻找与每一像素点最邻近(即最匹配)的体素，从而得到目标变换关系。

步骤s203，根据所述目标变换关系，确定所述图像采集模组的位姿信息。

在本申请实施例提供的定位方法中，无需图像采集模组采集二维图像，更无需从二维图像中提取每一像素点的图像特征，而是仅仅根据内参矩阵和深度图像中每一像素点的深度值，将与所述像素点对应的像素坐标转换为相机坐标然后，通过迭代策略，将每一像素点的相机坐标与多个体素的目标世界坐标进行匹配，即可实现对所述图像采集模组的精确定位；如此，可以降低定位方法的实现复杂度，从而更加高效地实现定位，满足实时定位的需求。

本申请实施例再提供一种定位方法，所述方法至少包括以下步骤s301至步骤s307：

步骤s301，根据图像采集模组的内参矩阵和所述图像采集模组采集的深度图像中多个像素点的深度值，将所述多个像素点的像素坐标转换为相机坐标。

步骤s302，从预先构建的点云地图中的多个体素中选取与每一所述像素点匹配的初始目标体素。

在实现时，可以设置相机坐标系相对于世界坐标系的初始变换关系；然后，根据像素点的相机坐标和初始变换关系，将该像素点与所述多个体素进行匹配，从而从所述多个体素中选取与该像素点匹配的初始目标体素。在一个示例中，可以通过如下实施例中的步骤s402至步骤s404，选取初始目标体素。

实际上，通过步骤s302，只是为了选取与像素点可能匹配的体素，所述初始目标体素可能不是与所述像素点真正匹配的对象；因此，需要通过如下步骤s303至步骤s306，进一步确定所述初始目标体素是否是与所述像素点真正匹配的对象。

步骤s303，根据每一所述像素点的相机坐标和对应的初始目标体素的目标世界坐标，确定所述相机坐标系相对于所述世界坐标系的第一变换关系。

在实现时，可以根据每一所述像素点的相机坐标和对应的初始目标体素的目标世界坐标，构建误差函数；然后，通过最小二乘法求解当前最优的第一变换关系。例如，包括n个像素点的相机坐标的集合表示为p＝{p1,p2,...,pi,...,pn}，像素点的相机坐标用pi来表示，与所述n个像素点匹配的初始目标体素的目标世界坐标的集合表示为q＝{q1,q2,...,qi,...,qn}，初始目标体素的目标世界坐标用qi来表示，那么，可以列出如下式(3)：

式中，e(r,t)为误差函数，r和t分别为待求解的第一变换关系中的旋转矩阵和平移向量。那么，可以通过最小二乘法求解式(1)中r和t的最优解。

步骤s304，根据所述第一变换关系、每一所述像素点的相机坐标和对应的初始目标体素的目标世界坐标，确定匹配误差。

这里，所述匹配误差指的是整体匹配误差，即所有像素点的匹配误差。在获得最优解，即第一变换关系之后，即可根据第一变换关系，将每一所述像素点的相机坐标转换为对应的第二世界坐标。如果在步骤s302中选取的初始目标体素和所述像素点在实际物理空间中表示的同一个位置点或者是两个相近的位置点，那么所述像素点的第二世界坐标应该是与对应的初始目标体素的目标世界坐标相同或相近的。反之，如果两者表示的不是同一位置点，也不是两个相近的位置点，那么，所述像素点的第二世界坐标与对应的初始目标体素的目标世界坐标不同，也不相近。基于此，可以通过如下步骤s406和步骤s407确定所述匹配误差，从而基于所述匹配误差和预设阈值，确定所述初始目标体素是否是与所述像素点真正匹配的点，进而确定目标变换关系。

步骤s305，如果所述匹配误差大于预设阈值，返回步骤s302，重新选取初始目标体素，并重新确定匹配误差。

可以理解地，如果所述匹配误差大于预设阈值，说明当前选取的初始目标体素并不是与所述像素点匹配的体素，两者指代的不是物理空间中同一位置点或者相近的位置点。此时，还需要返回步骤s302，重新选取初始目标体素，并基于重新选取的初始目标体素，重新执行步骤s303至步骤s304，以重新确定匹配误差，直至重新确定的匹配误差小于所述预设阈值时，认为当前迭代中选取的初始目标体素是与所述像素点真正匹配的点，此时可以将当前迭代获得的第一变换关系确定为目标变换关系。

反之，在其他实施例中，如果所述匹配误差小于或等于所述预设阈值，则根据当前迭代获得的第二变换关系确定图像采集模组在点云地图中的位姿信息。

步骤s306，将重新确定的匹配误差小于等于所述预设阈值时的第一变换关系，确定为目标变换关系。

步骤s307，根据所述目标变换关系，确定所述图像采集模组的位姿信息。

本申请实施例再提供一种定位方法，所述方法至少包括以下步骤s401至步骤s410：

步骤s401，根据图像采集模组的内参矩阵和所述图像采集模组采集的深度图像中多个像素点的深度值，将所述多个像素点的像素坐标转换为相机坐标。

步骤s402，获取相机坐标系相对于世界坐标系的第二变换关系。

在实现时，可以将第二变换关系中的旋转矩阵和平移向量设置一个初始值，也就是，设置第二变换关系的初始值。

步骤s403，根据所述第二变换关系和所述深度图像中第j个像素点的相机坐标，确定所述第j个像素点的第一世界坐标，j为大于0的整数。

步骤s404，将每一所述像素点的第一世界坐标与所述多个体素的目标世界坐标进行匹配，得出对应的初始目标体素。

在实现时，可以确定所述像素点的第一世界坐标与每一所述体素的目标世界坐标之间的距离，然后将距离所述像素点最近的体素确定为所述初始目标体素，或者将距离小于或等于距离阈值的体素确定为所述初始目标体素。在实现时，可以确定所述像素点的第一世界坐标与所述体素的目标世界坐标之间的欧氏距离，将所述欧式距离作为所述像素点与所述体素之间的距离。

步骤s405，根据每一所述像素点的相机坐标和对应的初始目标体素的目标世界坐标，确定所述相机坐标系相对于所述世界坐标系的第一变换关系。

步骤s406，根据所述第一变换关系和所述深度图像中第j个像素点的相机坐标，确定所述第j个像素点的第二世界坐标，j为大于0的整数。

步骤s407，根据每一所述像素点的第二世界坐标和对应的初始目标体素的目标世界坐标，确定所述匹配误差。

在实现时，可以先确定每一所述像素点的第二世界坐标与对应的初始目标体素的目标世界坐标之间的距离(例如欧式距离)；然后根据每一所述距离，确定所述匹配误差。

这里，可以将多个像素点与匹配的初始目标体素之间的平均距离，确定为所述匹配误差。例如，包括n个像素点的第二世界坐标的集合表示为p′＝{p′1,p′2,...,p′i,...,p′n}，像素点的第二世界坐标用p′i表示，与所述n个像素点匹配的初始目标体素的目标世界坐标的集合表示为q＝{q1,q2,...,qi,...,qn}，初始目标体素的目标世界坐标用qi表示，那么，通过如下公式(4)，可以求取所述匹配误差d：

式中||p′i-qi||²表示，所述像素点与匹配的初始目标体素之间的欧式距离。

步骤s408，如果所述匹配误差大于预设阈值，将所述第一变换关系作为所述第二变换关系，重新选取初始目标体素，并重新确定匹配误差，直至重新确定的匹配误差小于所述预设阈值为止。

可以理解地，如果所述匹配误差大于预设阈值，说明获取的第二变换关系是不符合实际的。换句话说，得出的初始目标体素不是真正与所述像素点匹配的对象，此时，可以将当前确定的第一变换关系作为所述第二变换关系，然后重新执行步骤s403至步骤s407，直至匹配误差小于所述阈值为止，执行步骤s409。

步骤s409，将重新确定的匹配误差小于等于所述预设阈值时的第一变换关系，确定为目标变换关系。

步骤s410，根据所述目标变换关系，确定所述图像采集模组的位姿信息。

本申请实施例提供的定位方法依赖于预先构建的点云地图，已经被预先构建好的点云地图，通常被存储在所述电子设备中或者存储在其他电子设备中，所述电子设备在实施所述定位方法时只需加载所述点云地图即可；其中，所述点云地图的构建过程如图2所示，至少包括以下步骤s501至步骤s503：

步骤s501，对特定物理空间的尺寸进行量化处理，得到多个体素的初始世界坐标。

可以理解地，所述特定物理空间指的是点云地图所覆盖的物理场景，例如，特定物理空间为某栋大楼、某块物理区域等。体素实际上是该特定物理空间中的最小单位。如图3所示，将特定物理空间看作一个具有一定尺寸的立方体301，然后以体素302为单位，对该立方体进行网格划分，得到多个体素；以世界坐标系为参考坐标系，确定每一体素的初始世界坐标系。举例来说，特定物理空间的尺寸为512×512×512m³，体素的尺寸为1×1×1m³，那么以1×1×1m³大小的体素为单位，对512×512×512m³大小的物理空间进行量化处理，可以得到512×512×512个体素的初始世界坐标。需要说明的是，所述量化处理包括量化特定物理空间的尺寸和确定每一体素的初始世界坐标。

步骤s502，根据所述图像采集模组在所述特定物理空间中采集的多个样本图像对，对每一所述体素的初始世界坐标进行更新，得到每一所述体素的目标世界坐标，所述样本图像对包括二维样本图像和深度样本图像。

二维样本图像是指不包含深度信息的平面图像，例如所述二维样本图像为rgb图像，在实现时，可以通过图像采集模组中的第一摄像头采集二维样本图像。深度样本图像是指仅包含深度信息的图像，在实现时，可以通过图像采集模组中的第二摄像头采集深度样本图像。所述电子设备可以通过如下实施例的步骤s602至步骤s604实现步骤s502。

步骤s503，根据每一所述体素的目标世界坐标，构建所述点云地图。也就是说，所述点云地图中包括每一所述体素的目标世界坐标。

可以理解地，图像采集模组在不同时刻或者不同位置采集样本图像时，其拍摄场景是有重叠区域的，也就是说，不同的样本图像中包括相同的拍摄物体，这使得在基于这些样本图像构建点云地图时，引入了大量的冗余信息，物理空间中的同一位置点可能被多个像素点以相同或相近的世界坐标表示在点云地图中，这样就大大增加了点云地图的数据量，影响了定位速度，并且对于获得高精度的定位结果也是不利的。

因此，在本申请实施例中，以体素的形式构建点云地图，即，通过采集的多个样本图像对，对每一所述体素的初始世界坐标进行更新，从而得到包括每一所述体素的目标世界坐标的点云地图。这种构建点云地图的方式，相当于将所述体素所涵盖的所有像素点的世界坐标融合为一个世界坐标值，如此，就解决了物理空间中的同一位置点被多个像素点以相同或相近的世界坐标表示在点云地图中所带来的上述问题，去除了大量的冗余信息，这对于定位速度的提高是非常有利的；另外，可以通过控制体素的尺寸，来获得更高的定位精度。理论上来讲，体素的尺寸越小，获得的定位精度越高。

本申请实施例再提供一种点云地图的构建过程，该过程至少包括以下步骤s601至步骤s605：

步骤s601，对特定物理空间的尺寸进行量化处理，得到多个体素的初始世界坐标。

步骤s602，控制所述图像采集模组按照预设帧率采集样本图像对。

在实现时，图像采集模组可以边移动边采集样本图像对。例如，可以通过具有图像采集模组的机器人实现样本图像对的采集。

步骤s603，根据所述图像采集模组在当前时刻采集的第一样本图像对和在历史时刻采集的第二样本图像对，更新每一所述体素的初始世界坐标。

所述电子设备可以通过如下实施例的步骤s703至步骤s705实现步骤s603。

步骤s604，根据所述第一样本图像对和所述图像采集模组在下一时刻采集的第三样本图像对，继续更新每一所述体素的当前世界坐标，直到样本图像采集结束时，将每一所述体素的当前世界坐标作为与所述体素对应的目标世界坐标。

可以理解地，在步骤s604中，所述继续更新每一所述体素的当前世界坐标，更新的是通过步骤s603更新得到的每一所述体素的目标世界坐标。实际上，通过步骤s603和步骤s604，所述电子设备可以根据图像采集模组当前时刻采集的样本图像对和历史时刻采集的样本图像对，实时地更新每一所述体素的当前世界坐标，直到图像采集模组的图像采集任务结束为止，将当前更新得到的每一体素的目标世界坐标作为与所述体素对应的目标世界坐标。

步骤s605，根据每一所述体素的目标世界坐标，构建所述点云地图。

在本申请实施例中，边采集样本图像对，边利用采集的样本图像对更新每一所述体素的当前世界坐标。也就是，所述电子设备不断地利用图像采集模组在当前时刻采集的样本图像对和在历史时刻(例如前一时刻)采集的样本图像对，更新每一所述体素的当前世界坐标。由于前后时刻获得的两张样本图像，具有较多的重叠区域，这样，所述电子设备就无需从多个样本图像对中找出重叠区域最多的两个样本图像对，然后在基于这两个样本图像对更新每一所述体素的当前世界坐标；如此，能够大大提高地图构建的效率。

本申请实施例再提供一种点云地图的构建过程，该过程至少包括以下步骤s701至步骤s707：

步骤s701，对特定物理空间的尺寸进行量化处理，得到多个体素的初始世界坐标。

步骤s702，控制所述图像采集模组按照预设帧率采集样本图像对。

步骤s703，根据所述图像采集模组在当前时刻采集的第一样本图像对和在历史时刻采集的第二样本图像对，确定每一所述体素的当前相机坐标。

在实现时，所述电子设备可以根据所述第一样本图像对和所述第二样本图像对，确定相机坐标系相对于世界坐标系的当前变换关系；然后根据所述当前变换关系，将每一所述体素的初始世界坐标转换为当前相机坐标。

在一个示例中，所述电子设备可以根据第一样本图像对中二维样本图像的像素点的图像特征、第一样本图像对中深度样本图像的像素点的深度值、以及第二样本图像对中二维样本图像的像素点的图像特征和第二样本图像对中深度样本图像的像素点的深度值，确定所述当前变换关系。基于此，根据如下公式(5)将所述体素的初始世界坐标转换为当前相机坐标。

式中，(xc,yc,zc)表示的是相机坐标，变换关系包括旋转矩阵r和平移向量t，(xw,yw,zw)表示的是世界坐标。

步骤s704，从所述第一样本图像对的深度样本图像中，获取与每一所述体素的当前像素坐标对应的深度值。

在实现时，所述电子设备可以根据所述图像采集模组的内参矩阵，将每一所述体素的当前相机坐标转换为当前像素坐标；从所述第一样本图像对的深度样本图像中，获取与每一所述体素的当前像素坐标对应的深度值。

步骤s705，根据每一所述体素的当前相机坐标和与每一所述体素的当前像素坐标对应的深度值，更新与所述体素对应的初始世界坐标。

在实现时，所述电子设备可以获取每一所述体素到物体表面的历史距离；将每一所述体素的当前相机坐标的z轴坐标值、与每一所述体素的当前像素坐标对应的深度值和每一所述体素到物体表面的历史距离，输入至与所述体素对应的距离模型中，以更新所述历史距离，得到目标距离；将每一所述体素到物体表面的目标距离，更新为与所述体素对应的初始世界坐标中的z轴坐标值，以实现对与所述体素对应的初始世界坐标进行更新。

在一个示例中，所述体素对应的距离模型如下式(6)所示：

式中，wt(x,y,z)表示在当前时刻t所述体素的权重；wt-1表示在前一时刻t-1所述体素的权重；maxweight为在前一时刻t-1所有体素中的最大权重；dt(u,v)表示与所述体素的当前像素坐标对应的深度值；zc表示所述体素的当前相机坐标的z轴坐标值；maxtruncation和mintruncation分别表示截断范围的最大值和最小值；dt-1表示在前一时刻t-1确定的所述体素到物体表面的距离(即所述历史距离的一种示例)，而dt则是当前待求的所述体素到物体表面的目标距离。

这样将所述体素的当前相机坐标的z轴坐标值zc、所述体素的当前像素坐标对应的深度值dt(u,v)和所述体素到物体表面的历史距离输入至公式(6)所示的距离模型中，即可得到所述体素到物体表面的目标距离。

可以理解地，由于在更新所述体素对应的初始世界坐标时，考虑了所述体素到物体表面的历史距离，如此，使得更新后的所述体素的初始世界坐标更加平滑，从而使得最终获得的所述体素的目标世界坐标更加精确，进而在定位阶段，能够提高定位精度。

步骤s706，根据所述第一样本图像对和所述图像采集模组在下一时刻采集的第三样本图像对，继续更新每一所述体素的当前世界坐标，直到样本图像采集结束时，将所述体素的当前世界坐标作为所述目标世界坐标。

可以理解地，所述电子设备通过执行类似于步骤s703至步骤s705，以继续更新每一所述体素的当前世界坐标。

步骤s707，根据每一所述体素的目标世界坐标，构建所述点云地图。

本申请实施例再提供一种点云地图的构建过程，该过程至少包括以下步骤s801至步骤s811：

步骤s801，对特定物理空间的尺寸进行量化处理，得到多个体素的初始世界坐标。

步骤s802，控制所述图像采集模组按照预设帧率采集样本图像对。

步骤s803，根据所述图像采集模组在当前时刻采集的第一样本图像对和在历史时刻采集的第二样本图像对，确定相机坐标系相对于世界坐标系的当前变换关系。

步骤s804，根据所述当前变换关系，将每一所述体素的初始世界坐标转换为当前相机坐标。

步骤s805，根据所述图像采集模组的内参矩阵，将每一所述体素的当前相机坐标转换为当前像素坐标。

步骤s806，从所述第一样本图像对的深度样本图像中，获取与每一所述体素的当前像素坐标对应的深度值。

步骤s807，获取每一所述体素到物体表面的历史距离；

步骤s808，将每一所述体素的当前相机坐标的z轴坐标值、与每一所述体素的当前像素坐标对应的深度值和每一所述体素到物体表面的历史距离，输入至与所述体素对应的距离模型中，以更新所述历史距离，得到目标距离。

步骤s809，将每一所述体素的目标距离，更新为与所述体素对应的初始世界坐标中的z轴坐标值，以实现对与所述体素对应的初始世界坐标进行更新。

步骤s810，根据所述第一样本图像对和所述图像采集模组在下一时刻采集的第三样本图像对，继续更新每一所述体素的当前世界坐标，直到样本图像采集结束时，将所述体素的当前世界坐标作为所述目标世界坐标。

可以理解地，所述电子设备通过执行类似于步骤s803至步骤s810，以继续更新每一所述体素的当前世界坐标。

步骤s811，根据每一所述体素的目标世界坐标，构建所述点云地图。

通过视觉信息可以建立室内环境地图，协助用户快速定位到自身位置和周边环境。针对于视觉技术，在相关技术中，通过识别摄像头采集到的图像中的人物和背景，以确定人物位置的方法。该方案将背景与预先测定的建筑物室内地图匹配，确定背景在室内的对应位置，然后根据背景的位置确认人物在室内的位置。在确定人物位置的具体算法流程上该方案的整体思路如下，通过图像识别的方法识别图像背景中固定位置物体，并根据固定位置物体的相对位置关系，确定某一时刻人物的位置。该方案的核心技术点是：1、通过视觉图像进行室内环境建图；2、图像匹配；3、在图像中识别出人物和物体。

然而，上述相关技术存在以下缺陷：1、该技术仅考虑了视觉图像的二维图像特征，定位后无法获得人物的姿态信息，且在定位精度方面较低；2、该技术可以针对图像中的人物或固定物体进行定位，但是前提是图像中必须有可以识别出的人物或固定物体，否则定位就会失效，定位的可靠性较低；3、该技术无法应对光线变换场景的定位需求，比如白天黑夜不同情况下的定位，定位鲁棒性较差。

基于此，下面将说明本申请实施例在一个实际的应用场景中的示例性应用。

本申请实施例实现了一种基于稠密点云的室内环境重建与定位技术，可以帮助用户创建稠密点云形式的室内地图(即所述点云地图的一种示例)，并且实时定位用户位置。该方案可以针对室内场景，提取图像特征进行视觉追踪和运动估计，并构建稠密地图；定位过程不依赖于外部基站设备，定位精度高，鲁棒性强。该方案包含两个主要部分：构建地图和视觉定位。

在本申请实施例中，构建地图部分主要是通过图像采集模组中的单目摄像头采集rgb图像信息，提取图像特征进行视觉追踪，同时利用图像采集模组中的三维视觉传感器(例如tof、结构光等)，采集深度信息构建稠密点云地图(即所述点云地图的一种示例)，具体的技术步骤至少包括以下步骤s11至步骤s17：

步骤s11，利用单目摄像头，以固定帧率进行rgb图像采集；

步骤s12，利用三维视觉传感器，以固定帧率进行深度图像采集；

步骤s13，将rgb图像和深度图像进行对齐，包括时间戳对齐和像素对齐；

步骤s14，采集过程中实时提取rgb图像中的特征信息和深度图像中的深度信息，以对图像采集模组进行视觉追踪和运动估计，确定相机坐标系相对于世界坐标系的当前变换关系；

步骤s15，通过深度图像和相机的内参矩阵得到稠密点云，所述稠密点云中包括每一像素点的相机坐标；

步骤s16，利用tsdf算法以体素的形式对稠密点云进行融合；

步骤s17，存储融合后的稠密点云，并序列化存储到本地作为稠密点云地图。

针对步骤s12中的利用三维视觉传感器进行深度图像采集，这里给出如下解释。深度图像又被称为距离图像，是指从图像采集模组到拍摄场景中各点的距离作为像素值的图像。深度图像直观反映了事物可见表面的几何形状。在深度数据流所提供的图像帧中，每一个像素点代表的是在三维视觉传感器的视野中，该特定的坐标处物体到摄像头平面的距离。

针对步骤s15中提到的通过深度图像和相机的内参矩阵得到稠密点云，这里给出如下解释。可以通过如下的公式(7)将像素点(u,v)转换到相机坐标(xc,yc,zc)：

式中(u0,v0)是图像的中心坐标点，zc表示相机坐标的z轴值，也就是该像素点对应的深度值；相机坐标和世界坐标下的同一物体具有相同的深度，即zc＝zw。

针对步骤s16中的利用tsdf算法以体素的形式对稠密点云进行融合，这里给出如下技术步骤s161至步骤s164：

步骤s161，首先获取体素在全局坐标系下的坐标v^g(x,y,z)(即所述体素的目标世界坐标)，然后根据运动追踪得到的变换矩阵(即步骤s14输出的当前变换关系)将其从全局坐标转换为相机坐标v(x,y,z)；

步骤s162，根据相机的内参矩阵将相机坐标v(x,y,z)转换为图像坐标，得到一个图像坐标(u,v)；

步骤s163，如果第l帧深度图像在图像坐标(u,v)处的深度值d(u,v)不为0，则比较d(u,v)与体素的相机坐标v(x,y,z)中z的大小，如果d(u,v)＜z，说明此体素距离相机更远，在融合表面的内部；否则，说明此体素距离相机更近，在融合表面的外部；

步骤s164，根据步骤s163的结果更新此体素中距离dl和权重值wl，更新公式如下式(8)所示：

式中，wi(x,y,z)为当前帧全局数据立方体中体素的权重，wi-1(x,y,z)为上一帧全局数据立方体中体素的权重，maxweight为上一帧全局数据立方体中所有体素的权重中的最大权重，可以设定为1，di(x,y,z)当前全局数据立方体中体素到物体表面的距离，di-1(x,y,z)为上一帧全局数据立方体重体素到物体表面的距离，di(x,y,z)为根据当前帧深度数据计算得到的全局数据立方体中体素到物体表面的距离，z表示体素在相机坐标系下的z轴坐标，di(u,v)表示当前帧深度图像在像素点(u,v)处的深度值，[mintruncation,maxtruncation]为截断范围，其会影响到重建结果的精细程度。

基于步骤s11至步骤s17可以构建出一张基于稠密点云的稠密点云地图(即所述点云地图的一种示例)，该地图以二进制格式存储该稠密点云地图到本地，在视觉定位过程中，该地图将被加载使用。

在本申请实施例中，视觉定位部分通过使用三维视觉传感器，采集深度信息并将其转换为点云，再通过迭代最近点算法算法(iterativeclosestpoint，icp)匹配稠密点云地图，得到当前相机在地图中的位姿，以达到定位目的，具体的技术步骤至少可以包括步骤s21至步骤s24：

步骤s21，加载构建好的稠密点云地图；

步骤s22，利用三维视觉传感器进行深度图像采集，得到待处理的深度图像；

步骤s23，通过深度图像和相机的内参矩阵得到当前点云，所述当前点云中包括所述深度图像的像素点的相机坐标；

步骤s24，通过icp算法匹配当前点云和稠密点云地图，得到当前相机在地图中的精确位姿。

针对步骤s23中的通过深度图像和相机的内参矩阵得到当前点云的方法，可参考步骤s15。

针对步骤s24中的通过icp算法匹配当前点云和稠密点云地图，得到当前相机在地图中的精确位姿，这里给出如下解释。icp算法本质上是基于最小二乘法的最优配准方法。该算法重复进行选择对应关系点对，计算最优刚体变换，知道满足正确配准的收敛精度要求。icp算法的基本原理是：分别在待匹配的目标点云p和源点云q中，按照一定的约束条件，找到最邻近的点(pi,qi)，然后计算出最优的旋转r和平移t，使得误差函数最小，误差函数e(r,t)如公式(9)：

式中，n为邻近点对的数量，pi为目标点云p中的一点，qi为源点云q中与pi对应的最近点，r为旋转矩阵，t为平移向量。算法包括以下步骤s241至步骤s246：

步骤s241，在当前点云p中取点集pi∈p；

步骤s242，找出稠密点云地图q中的对应点集qi∈q，使得||qi-pi||＝min；

步骤s243，计算旋转矩阵r和平移矩阵t，使得误差函数最小；

步骤s244，对pi使用步骤s243求得的旋转矩阵r和平移矩阵t进行旋转和平移变换，得到新的对应点集pi'＝{pi'＝rpi+t,pi∈p}；

步骤s245，计算pi'与对应点集qi的平均距离

步骤s246，若d小于给定阈值dth或者大于预设的迭代次数，则停止迭代计算，算法输出当前的旋转矩阵r和平移矩阵t；否则跳回到步骤s242。

在本申请实施例中，基于步骤s21至步骤s24可以通过三维视觉传感器提供的深度信息，在预定义的稠密点云地图中达成定位目的，得到图像采集模组在地图坐标系下的位置和姿态。该定位结果精度较高，不需要依赖外部基站设备，抗环境干扰性强，鲁棒性强。

本申请实施例提供的定位方法，能够获得以下技术效果：1、利用三维视觉传感器可以得到深度信息，从而利用深度信息实现地图构建和定位，如此定位精度不会受到光照变换情况下的影响，定位方法的鲁棒性较高；2、在定位结果上可以同时提供位置和姿态，相对于其他室内定位方法提高了定位准确度；3、定位方法不需要引入物体识别等错误率较高的算法，定位成功率高，鲁棒性强；4、构建的地图形式为稠密点云地图，不需要存储环境的rgb信息，因此地图的私密性较好。

在本申请实施例中，利用三维视觉传感器采集深度信息构建稠密点云地图，并结合高精度高鲁棒性的点云匹配算法的进行室内环境定位。在地图构建上，本申请实施例通过使用三维视觉传感器采集深度图像信息，以稠密点云的形式存储为稠密点云地图。在定位方法上，本申请实施例采用icp算法匹配当前点云和地图稠密点云，精确地计算出当前自身位置和姿态。两者结合形成了一套高精度、高鲁棒性的室内定位方法。

基于前述的实施例，本申请实施例提供一种定位装置，该装置包括所包括的各模块、以及各模块所包括的各单元，可以通过电子设备中的处理器来实现；当然也可通过具体的逻辑电路实现；在实施的过程中，处理器可以为中央处理器(cpu)、微处理器(mpu)、数字信号处理器(dsp)或现场可编程门阵列(fpga)等。

图4a为本申请实施例定位装置的组成结构示意图，如图4a所示，所述装置400包括坐标转换模块401和定位模块402，其中：

坐标转换模块401，配置为根据图像采集模组的内参矩阵和所述图像采集模组采集的深度图像中多个像素点的深度值，将所述多个像素点的像素坐标转换为相机坐标；

定位模块402，配置为将每一所述像素点的相机坐标与预先构建的点云地图中多个体素的目标世界坐标进行匹配，得出所述图像采集模组的位姿信息；

其中，每一所述体素的目标世界坐标是根据多个样本图像对，更新每一所述体素的初始世界坐标而获得的，所述样本图像对包括二维样本图像和深度样本图像。

在其他实施例中，如图4b所示，所述装置400还包括量化处理模块403、坐标更新模块404和地图构建模块405；其中，量化处理模块403，配置对特定物理空间的尺寸进行量化处理，得到多个体素的初始世界坐标；坐标更新模块404，配置为根据所述图像采集模组在所述特定物理空间中采集的多个样本图像对，对每一所述体素的初始世界坐标进行更新，得到每一所述体素的目标世界坐标，所述样本图像对包括二维样本图像和深度样本图像；地图构建模块405，配置为根据每一所述体素的目标世界坐标，构建所述点云地图。

在其他实施例中，坐标更新模块404，包括：控制子模块，配置为控制所述图像采集模组按照预设帧率采集样本图像对；坐标更新子模块，配置为根据所述图像采集模组在当前时刻采集的第一样本图像对和在历史时刻采集的第二样本图像对，更新每一所述体素的初始世界坐标；根据所述第一样本图像对和所述图像采集模组在下一时刻采集的第三样本图像对，继续更新每一所述体素的当前世界坐标，直到样本图像采集结束时，将所述体素的当前世界坐标作为所述目标世界坐标。

在其他实施例中，所述坐标更新子模块，包括：相机坐标确定单元，配置为根据所述第一样本图像对和所述第二样本图像对，确定每一所述体素的当前相机坐标；深度值获取单元，配置为从所述第一样本图像对的深度样本图像中，获取与每一所述体素的当前像素坐标对应的深度值；坐标更新单元，配置为：根据每一所述体素的当前相机坐标和与每一所述体素的当前像素坐标对应的深度值，更新与所述体素对应的初始世界坐标；根据所述第一样本图像对和所述图像采集模组在下一时刻采集的第三样本图像对，继续更新每一所述体素的当前世界坐标，直到样本图像采集结束时，将所述体素的当前世界坐标作为所述目标世界坐标。

在其他实施例中，所述坐标更新单元，配置为：获取每一所述体素到物体表面的历史距离；将每一所述体素的当前相机坐标的z轴坐标值、与每一所述体素的当前像素坐标对应的深度值和每一所述体素到物体表面的历史距离，输入至与所述体素对应的距离模型中，以更新所述历史距离，得到目标距离；将每一所述体素到物体表面的目标距离，更新为与所述体素对应的初始世界坐标中的z轴坐标值，以实现对与所述体素对应的初始世界坐标进行更新；根据所述第一样本图像对和所述图像采集模组在下一时刻采集的第三样本图像对，继续更新每一所述体素的当前世界坐标，直到样本图像采集结束时，将所述体素的当前世界坐标作为所述目标世界坐标。

在其他实施例中，所述相机坐标确定单元，配置为：根据所述第一样本图像对和所述第二样本图像对，确定相机坐标系相对于世界坐标系的当前变换关系；根据所述当前变换关系，将每一所述体素的初始世界坐标转换为当前相机坐标。

在其他实施例中，所述深度值获取单元，配置为：根据所述图像采集模组的内参矩阵，将每一所述体素的当前相机坐标转换为当前像素坐标；从所述第一样本图像对的深度样本图像中，获取与每一所述体素的当前像素坐标对应的深度值。

在其他实施例中，所述定位模块402，包括：迭代子模块，配置为根据迭代策略，将每一所述像素点的相机坐标与所述多个体素的目标世界坐标进行匹配，得出相机坐标系相对于世界坐标系的目标变换关系；定位子模块，配置为根据所述目标变换关系，确定所述图像采集模组的位姿信息。

在其他实施例中，所述迭代子模块，包括：选取单元，配置为从所述多个体素中选取与每一所述像素点匹配的初始目标体素；确定单元，配置为：根据每一所述像素点的相机坐标和对应的初始目标体素的目标世界坐标，确定所述相机坐标系相对于所述世界坐标系的第一变换关系；根据所述第一变换关系、每一所述像素点的相机坐标和对应的初始目标体素的目标世界坐标，确定匹配误差；如果所述匹配误差大于预设阈值，重新选取初始目标体素，并重新确定匹配误差；将重新确定的匹配误差小于等于所述预设阈值时的第一变换关系确定为所述目标变换关系。

在其他实施例中，所述选取单元，配置为：获取所述相机坐标系相对于所述世界坐标系的第二变换关系；根据所述第二变换关系和第j个像素点的相机坐标，确定所述第j个像素点的第一世界坐标，j为大于0的整数；将每一所述像素点的第一世界坐标与所述多个体素的目标世界坐标进行匹配，得出对应的初始目标体素。

在其他实施例中，所述确定单元，配置为：根据所述第一变换关系和第j个像素点的相机坐标，确定所述第j个像素点的第二世界坐标，j为大于0的整数；根据每一所述像素点的第二世界坐标和对应的初始目标体素的目标世界坐标，确定所述匹配误差。

在其他实施例中，所述确定单元，配置为：确定每一所述像素点的第二世界坐标与对应的初始目标体素的目标世界坐标之间的距离；根据每一所述距离，确定所述匹配误差。

在其他实施例中，所述选取单元，配置为：如果所述匹配误差大于预设阈值，将所述第一变换关系作为所述第二变换关系，重新选取初始目标体素。

以上装置实施例的描述，与上述方法实施例的描述是类似的，具有同方法实施例相似的有益效果。对于本申请装置实施例中未披露的技术细节，请参照本申请方法实施例的描述而理解。

需要说明的是，本申请实施例中，如果以软件功能模块的形式实现上述的定位方法，并作为独立的产品销售或使用时，也可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请实施例的技术方案本质上或者说对相关技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得电子设备(可以是手机、平板电脑、笔记本电脑、台式计算机、服务器、机器人、无人机等)执行本申请各个实施例所述方法的全部或部分。而前述的存储介质包括：u盘、移动硬盘、只读存储器(readonlymemory，rom)、磁碟或者光盘等各种可以存储程序代码的介质。这样，本申请实施例不限制于任何特定的硬件和软件结合。

对应地，本申请实施例提供一种电子设备，图5为本申请实施例电子设备的一种硬件实体示意图，如图5所示，该电子设备500的硬件实体包括：包括存储器501和处理器502，所述存储器501存储有可在处理器502上运行的计算机程序，所述处理器502执行所述程序时实现上述实施例中提供的定位方法中的步骤。

存储器501配置为存储由处理器502可执行的指令和应用，还可以缓存待处理器502以及电子设备500中各模块待处理或已经处理的数据(例如，图像数据、音频数据、语音通信数据和视频通信数据)，可以通过闪存(flash)或随机访问存储器(randomaccessmemory，ram)实现。

对应地，本申请实施例提供一种计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现上述实施例中提供的定位方法中的步骤。

这里需要指出的是：以上存储介质和设备实施例的描述，与上述方法实施例的描述是类似的，具有同方法实施例相似的有益效果。对于本申请存储介质和设备实施例中未披露的技术细节，请参照本申请方法实施例的描述而理解。

应理解，说明书通篇中提到的“一个实施例”或“一实施例”意味着与实施例有关的特定特征、结构或特性包括在本申请的至少一个实施例中。因此，在整个说明书各处出现的“在一个实施例中”或“在一实施例中”未必一定指相同的实施例。此外，这些特定的特征、结构或特性可以任意适合的方式结合在一个或多个实施例中。应理解，在本申请的各种实施例中，上述各过程的序号的大小并不意味着执行顺序的先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本申请实施例的实施过程构成任何限定。上述本申请实施例序号仅仅为了描述，不代表实施例的优劣。

需要说明的是，在本文中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者装置不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者装置所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括该要素的过程、方法、物品或者装置中还存在另外的相同要素。

在本申请所提供的几个实施例中，应该理解到，所揭露的设备和方法，可以通过其它的方式实现。以上所描述的设备实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，如：多个单元或组件可以结合，或可以集成到另一个系统，或一些特征可以忽略，或不执行。另外，所显示或讨论的各组成部分相互之间的耦合、或直接耦合、或通信连接可以是通过一些接口，设备或单元的间接耦合或通信连接，可以是电性的、机械的或其它形式的。

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

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

本领域普通技术人员可以理解：实现上述方法实施例的全部或部分步骤可以通过程序指令相关的硬件来完成，前述的程序可以存储于计算机可读取存储介质中，该程序在执行时，执行包括上述方法实施例的步骤；而前述的存储介质包括：移动存储设备、只读存储器(readonlymemory，rom)、磁碟或者光盘等各种可以存储程序代码的介质。

或者，本申请上述集成的单元如果以软件功能模块的形式实现并作为独立的产品销售或使用时，也可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请实施例的技术方案本质上或者说对相关技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得电子设备(可以是手机、平板电脑、笔记本电脑、台式计算机、服务器、机器人、无人机等)执行本申请各个实施例所述方法的全部或部分。而前述的存储介质包括：移动存储设备、rom、磁碟或者光盘等各种可以存储程序代码的介质。

本申请所提供的几个方法实施例中所揭露的方法，在不冲突的情况下可以任意组合，得到新的方法实施例。

本申请所提供的几个产品实施例中所揭露的特征，在不冲突的情况下可以任意组合，得到新的产品实施例。

本申请所提供的几个方法或设备实施例中所揭露的特征，在不冲突的情况下可以任意组合，得到新的方法实施例或设备实施例。

以上所述，仅为本申请的实施方式，但本申请的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本申请的保护范围之内。因此，本申请的保护范围应以所述权利要求的保护范围为准。