三维地图重建方法、自移动设备及计算机可读存储介质与流程

1.本发明涉及三维重建技术领域，尤其涉及一种三维地图重建方法、自移动设备及计算机可读存储介质。


背景技术：

2.如今，三维重建功能在自移动设备(如机器人)上的运用逐渐广泛与成熟，三维重建的原理为：依靠传感器对周围环境的感知，同时实现对机器人自身位置和环境的描述，通过机器人当前观测和对历史建图的匹配，可确定机器人当前最新位置，当确定最新位置后，可将最新观测作用在当前位置上，实现对建图的更新和补充。
3.然而，相关技术中的三维地图重建方法，对算力要求高，耗时大，需要离线或依赖于额外的图像处理设备gpu(graphics processing unit，图形处理器)，无法实现在移动设备端算力、内存条件有限情况下的实时运行。


技术实现要素：

4.本发明旨在至少在一定程度上解决相关技术中的技术问题之一。
5.为达到上述目的，本发明第一方面实施例提出了一种三维地图重建方法，应用于自移动设备中，所述方法包括：确定同一时刻的rgb图、深度图和里程计位姿；根据所述同一时刻的rgb图、深度图和里程计位姿确定当前局部地图；将所述当前局部地图与全局地图进行匹配，以对所述里程计位姿进行修正；根据修正后的里程计位姿和滑动窗口策略对所述当前局部地图进行更新；将更新后的当前局部地图与所述rgb图进行融合，以更新所述全局地图。
6.为达到上述目的，本发明第二方面实施例提出了一种移动设备，包括图像装置、存储器、控制器及存储在存储器上并可在控制器上运行的三维重建程序，所述控制器执行所述三维重建程序时，实现上述的三维地图重建方法。
7.为达到上述目的，本发明第三方面实施例提出了一种计算机可读存储介质，其上存储有三维重建程序，该三维重建程序被处理器执行时实现上述的三维地图重建方法。
8.本发明实施例的三维地图重建方法、自移动设备及计算机可读存储介质，可以在确定同一时刻的rgb图、深度图、里程计位姿之后，根据该同一时刻的rgb图、深度图、里程计位姿确定当前局部地图，并根据当前局部地图匹配全局地图以对里程计位姿进行修正，以保证里程计位姿的精确度，避免里程计位姿的误差对三维重建构成影响。而且，在修正里程计位姿后，还根据修正后的里程计位姿对当前局部地图进行移动更新。进一步地，使更新后的当前局部地图与rgb图进行融合以对全局地图进行更新，从而利用该融合大幅降低三维重建过程中的内存占用与算力要求，提高三维地图重建方法的运算速度，使得本发明实施例的三维地图重建方法可以被移动设备直接应用，无需额外设置gpu、高端cpu(central processing unit，中央处理器)等设备。而且，由于构建了局部地图，并利用局部地图对全局地图进行更新，可以进一步降低三维重建的算力要求。
9.本发明附加的方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。
附图说明
10.图1是本发明一个示例的移动设备的示意图；
11.图2是本发明一个实施例的三维地图重建方法的流程图；
12.图3是本发明另一个实施例的三维地图重建方法的流程图；
13.图4是本发明又一个实施例的三维地图重建方法的流程图；
14.图5是本发明又一个实施例的三维地图重建方法的流程图；
15.图6是本发明又一个实施例的三维地图重建方法的流程图。
具体实施方式
16.下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。
17.下面参考附图描述本发明实施例的三维地图重建方法、自移动设备及计算机可读存储介质。
18.图1是本发明一个实施例的三维地图重建方法的流程图。
19.在本发明实施例中，该三维地图重建方法应用于自移动设备中，该自移动设备例如可以为无人配送小车、无人机、仓储机器人、商场服务机器人、送餐机器人、扫地机器人、扫拖机器人、洗地机器人等机器人，再例如可以为手持式三维重建扫描仪、固定式三维重建扫描设备、头戴式ar仪等可以获取rgb图、深度图、里程计位姿的设备。
20.作为一个示例，上述自移动设备为扫拖机器人，该扫拖机器人100可以如图1所示。参见图1，该扫拖机器人包括：测距组件102、控制装置104、行走组件106、图像装置108。上述图像装置108设置在扫拖机器人的前端，例如可以为rgb相机和深度相机，进而通过rgb相机采集得到rgb图，并通过深度相机采集得到深度图。同时，上述测距组件102例如可以为里程计，进而通过测距组件102获取里程计位姿，以获取扫拖机器人当前所在的位姿信息，该位姿信息包括三维坐标信息和姿态信息，该三维坐标信息具体包括对应x轴、y轴、z轴的坐标，该自移动设备的姿态信息即为该扫拖机器人的朝向信息，其具体包括航向偏角(pitch)、偏航(yaw)和侧滚(roll)。
21.如图1所示，该扫拖机器人可用于实现三维地图重建方法，该方法步骤包括：
22.s11，确定同一时刻的rgb图、深度图和里程计位姿。
23.具体地，获取当前时刻的深度图以及相邻时刻的若干rgb图；其中，深度图以及若干rgb图中每个rgb图均携带有对应的时间戳；计算若干rgb图中每个rgb图与深度图的时间戳差值；若存在时间戳差值不大于第一时间阈值且rgb图的数量为一个，则将rgb图与深度图视为同一时刻的rgb图和深度图，并将rgb图对应的位姿作为同一时刻的里程计位姿；若存在时间戳差值不大于第一时间阈值且rgb图的数量至少为两个，则将最新时间戳所对应的rgb图与深度图视为同一时刻的rgb图和深度图，且以深度图的时间戳为基准，采用线性插值的方式确定同一时刻的里程计位姿。
24.具体而言，可以获取最新获得的深度图的时间戳，并以该时间戳为基准获取相邻时刻的若干rgb图，进而计算该若干rgb图的时间戳与该深度图的时间戳之间的时间戳差值，从而得到若干时间戳差值，并将计算结果与第一时间阈值进行比较。若仅有一个时间戳差值小于等于该第一时间阈值，则将与该时间戳差值对应的rgb图视为与该深度图同一时刻的rgb图，并将该rgb图对应的位姿作为与该深度图同一时刻的里程计位姿。若有多个时间戳差值小于等于该第一时间阈值，则获取该多个时间戳差值所对应的多个rgb图中最新获得的rgb图，并将其视为与该深度图统一时刻的rgb图，且以深度图的时间戳为基准，采用线性插值的方式确定同一时刻的里程计位姿。
25.作为一个示例，假设最新获得的深度图的时间戳为1s，则以该时间戳为基准获取相邻时刻的若干rgb图，并计算该若干rgb图的时间戳与1s之间的差值以得到若干个时间戳差值。假设设置第一时间阈值为0.2s，获取该若干个时间戳差值中小于等于0.2s的时间戳差值。若仅有一个时间戳差值小于等于0.2s，则将与该时间戳差值对应的rgb图视为与该深度图同一时刻的rgb图，并将该rgb图对应的位姿作为与该深度图同一时刻的里程计位姿。若存在至少两个时间戳差值小于等于0.2s，则将最新时间戳所对应的rgb图视为与该深度图同一时刻的rgb图；例如，若存在时间戳为0.9s与1.1s的两个rgb图，则将时间戳为1.1s的rgb图视为与该深度图同一时刻的rgb图。进而以该深度图的时间戳为基准，采用线性差值的方式确定同一时刻的里程计位姿。
26.其中，上述以深度图的时间戳为基准，采用线性插值的方式确定同一时刻的里程计位姿的方法可以参见图3，包括如下步骤：
27.s111，获取与深度图的时间戳差值最小的第一rgb图和第二rgb图；其中，第一rgb图携带第一时间戳以及第一位姿信息，第二rgb图携带有第二时间戳以及第二位姿信息。
28.具体地，获取与深度图的时间戳差值最小的第一rgb图和第二rgb图，该第一rgb图的里程计位姿为(x1，y1，z1，roll1，pitch1，yaw1)，该第二rgb图的里程计位姿为(x2，y2，z2，roll2，pitch2，yaw2)，且第一rgb图的第一时间戳比该深度图的时间戳早，第二rgb图的第二时间戳比该深度图的时间戳晚。其中，上述(x1，y1，z1)、(x2，y2，z2)为与第一rgb图、第二rgb图的时间戳分别对应的表示自移动设备的位置的三维坐标信息，上述(roll1，pitch1，yaw1)、(roll2，pitch2，yaw2)为与第一rgb图、第二rgb图的时间戳分别对应的自移动设备的姿态信息，且姿态信息中的航向偏角表示沿着x轴旋转的角度，姿态信息中的偏航表示沿着y轴旋转的角度，姿态信息中的侧滚表示沿着z轴旋转的角度。
29.s112，根据第一时间戳与第二时间戳的比例关系，得到第一rgb图与深度图的第一权重以及第二rgb图与深度图的第二权重。
30.具体地，获取第一时间戳与深度图的时间戳之间的第一差值，并获取第二时间戳与深度图的时间戳之间的第二差值，进而以差值越小则权重越大为原则，根据第一差值与第二差值之间的比值获取第一权重ai与第二权重bi。
31.作为一个示例，假设深度图的时间戳为1s，而第一时间戳与第二时间戳分别为0.8s与1.2s，则由于0.8s、1.2s与1s之间的差别都是0.2s，因而第一权重ai与第二权重bi均为50％。
32.s113，根据第一位姿信息、第一权重以及第二位姿信息、第二权重，得到理论里程计位姿，以作为同一时刻的里程计位姿。
33.具体地，在获取第一权重ai与第二权重bi之后，可以根据如下公式计算得到与深度图同一时刻的里程计位姿(x，y，z，roll，pitch，yaw)：
34.x＝ai*x1+bi*x2，
35.y＝ai*y1+bi*y2，
36.z＝ai*z1+bi*z2，
37.roll＝ai*roll1+bi*roll2，
38.pitch＝ai*pitch1+bi*pitch2，
39.yaw＝ai*yaw1+bi*yaw2。
40.由此，可以实现在获取rgb图、深度图和里程计位姿之后，对其进行预处理得到同一时刻的rgb图、深度图和里程计位姿。由于rgb图只决定地图点的颜色，时间戳差一点对结果的影响较小，而深度图决定了地图点的位置，对重建精度的影响较大，因而采用深度图的时间戳作为基准，可以实现更高的重建精度。而且，通过对rgb图、深度图和里程计位姿进行时间戳统一，使得移动设备上所安装的传感器如摄像机无需采用专门硬件，可以采用普通的摄像机，从而降低成本，便于维护。
41.s12，根据同一时刻的rgb图、深度图和里程计位姿确定当前局部地图。
42.需要说明的是，移动设备上的计算模块每过一个预设时间便获取一组同一时刻的rgb图、深度图，并将其记为一帧，且该帧携带有里程计位姿。计算模块每获取到一帧，便将该帧加入滑动窗口，若此时滑动窗口已满，则首先将滑动窗口内时间戳最靠前的一帧移出滑动窗口，再将新获取到的帧加入滑动窗口。作为一个示例，假设滑动窗口的大小为10，且当前在滑动窗口内的帧为90～100帧；则在获取到第101帧时，将90帧移出滑动窗口，并将第101帧加入滑动窗口。
43.具体地，移动设备在新获取到一帧的同时，将该新的一帧加入滑动窗口，并将该滑动窗口内的地图点记为当前局部地图的地图点。作为一个示例，假设滑动窗口的大小为10，且当前在滑动窗口内的帧为90～100帧，则在获取到第101帧时，第101帧加入滑动窗口，91～101帧的地图点即为当前局部地图的地图点。
44.s13，将当前局部地图与全局地图进行匹配，以对里程计位姿进行修正。
45.具体地，获取里程计位姿对应的当前局部地图；根据当前局部地图中的每个地图点，在全局地图中寻找与对应的地图点相匹配的点云集；根据点云集构建最小二乘平面，并根据每个地图点到对应的最小二乘平面的距离来计算里程计位姿的第一修正量；根据第一修改量对里程计位姿进行修正，得到修正后的里程计位姿。
46.具体而言，由于在移动设备运行的过程中，里程计位姿可能会存在误差，尤其是在长时间运行后，里程计位姿产生的误差会累计，进而使里程计位姿产生较大的误差。为了解决该误差造成的影响，在确定当前的局部地图后，可以确定全局地图中与当前的局部地图对应的部分，进而采用点-面icp(iterative closest point，迭代最近点算法)匹配技术，将局部地图与全局地图中进行匹配以获取里程计位姿的修正量，具体为：根据当前局部地图中的每个地图点，在全局地图中寻找相匹配的点云集，该点云集例如可以为预设个数个距离最近的地图点；进而根据点云集构建最小二乘平面，并通过优化每个地图点到对应的最小二乘平面的距离，获得里程计位姿的第一修正量，从而根据第一修正量对里程计位姿进行修正得到修正后的里程计位姿。由此，可以消除里程计位姿的误差对三维重建构成的
影响。其中，上述局部地图与上述全局地图均为由地图点构成，每个地图点均包括与其对应的3d坐标(x轴、y轴、z轴坐标)和颜色，还需要包括该地图点的法向量信息。
47.作为一个示例，假设一个机器人在20分钟前来到了一个房间，而当前该机器人又一次来到了该房间，则20分钟前该机器人在该房间内得到的地图点在全局地图内，当前在该房间内得到的地图点在局部地图内。采用点-面icp匹配技术，获得里程计位姿的第一修正量。
48.需要说明的是，在将当前局部地图与全局地图进行匹配，以对里程计位姿进行修正之后，还需要进行如图4所示的步骤：
49.s131，根据里程计位姿和修正后的里程计位姿，得到里程计位姿的相对位姿变换矩阵。
50.s132，获取当前局部地图中每个地图点的坐标信息。
51.s133，将相对位姿变换矩阵分别乘以每个地图点的坐标信息，得到各地图点更新后的坐标信息。
52.具体地，在利用修正量对里程计位姿进行修正后，还需要根据修正前的里程计位姿和修正后的里程计位姿，得到里程计位姿的相对位姿变换矩阵t_delta，进而通过下式对当前局部地图进行修正：p_new＝t_delta*p_old，其中，p_old为修正前的当前局部地图的地图点坐标，p_new为修正后的当前局部地图的地图点坐标。
53.s14，根据修正后的里程计位姿和滑动窗口策略对当前局部地图进行更新。
54.具体地，将一组同一时刻的rgb图和深度图作为一帧且该帧携带有里程计位姿；以该帧作为当前时刻对应的当前帧，获取当前帧对应的多个临近图像帧。具体而言，在对里程计位姿进行修正后，获取与当前时刻对应的当前帧，该当前帧包括有同一时刻的rgb图、深度图，且该帧携带有修正后的里程计位姿。获取与当前帧对应的多个临近图像帧，采用滑动窗口策略，依据临近图像帧对当前局部地图进行更新。
55.其中，上述获取与当前帧对应的多个临近图像帧的方法可以参见图5，包括如下所示的步骤：
56.s141，获取当前帧的里程计位姿；其中，当前帧携带有时间戳。
57.s142，根据当前帧对应的时间戳，获取当前帧预设时间段内的多个临近帧。
58.s143，对当前帧进行预设次数的回环检测得到若干回环帧，回环检测包括：根据当前帧获取目标帧，并获取目标帧的多个候选历史帧，根据各候选历史帧的里程计位姿与目标帧的里程计位姿之间的差值，从多个候选历史帧中筛选出差值小于第二预设阈值的候选历史帧作为回环帧；其中，每个候选历史帧携带有对应的里程计位姿。
59.具体地，在获取当前帧之后，首先以当前帧为目标帧，并获取与当前帧时间较近的历史帧，并将该历史帧作为候选历史帧；作为一个示例，假设当前帧为第50000帧，则目标帧为第50000帧，候选历史帧为属于(49000,50000)范围内的帧，也可首先去除预设时间段内的临近帧，进而在剩余的历史帧中确定候选历史帧。由于每个候选历史帧均携带有对应的里程计位姿，且上述第二预设阈值包括第二预设距离阈值与第二预设角度阈值，因而可以首先获取各候选历史帧对应的坐标，以及将目标帧对应的坐标与候选历史帧对应的坐标进行比较，选取坐标之差小于第二预设距离阈值的候选历史帧。进而获取目标帧的roll、pitch、yaw角，并获取上述选取的候选历史帧的roll、pitch、yaw角，计算上述目标帧与候选
历史帧之间的roll、pitch、yaw角的差异，若某一个候选历史帧与目标帧之间的角度差异小于第二预设角度阈值，则确定该候选历史帧为目标帧的回环帧。
60.可选地，在选取坐标之差小于预设距离阈值的候选历史帧之后，还可分别计算目标帧与选取的候选历史帧的旋转向量和旋转角，并获取目标帧与候选历史帧之间的差值，若某一个候选历史帧对应的差值小于预设值，则确定该候选历史帧为当前帧的回环帧。
61.进一步地，在采用上述方法得到回环帧后，还可将该回环帧视为新的目标帧，并重复上述步骤。重复该过程预设次数，从而得到回环帧的回环帧、回环帧的回环帧的回环帧、回环帧的回环帧的回环帧的回环帧、
…
。由此，可以实现获取所有的与当前帧具有相近位置、相近观测角度的若干回环帧。作为一个示例，假设当前帧为第100帧，对该第100帧计算回环帧，得到回环帧为20、21帧，进而获取20、21帧的回环帧为0、1、2、3帧，重复该过程10次，记录得到的所有回环帧，从而确保局部地图中包含了所有与第100帧具有相近位置、相近观测角度的若干回环帧。由此，可以实现通过回环的方式获取若干回环帧，从而降低三维重建的算力要求。
62.需要说明的是，可以设置在自移动设备运行的过程中，每获取到一帧，均需要采用上述方法得到其的回环帧、回环帧的回环帧、回环帧的回环帧的回环帧
…
，还可以设置在获取到一帧的回环帧后，可以记录与该帧对应的回环帧，进而若某一帧计算得到的回环帧为该帧，可以直接利用记录结果得到与该帧对应的回环帧。
63.s144，将若干回环帧与多个临近帧均作为当前帧的临近图像帧。
64.具体地，将上述方法得到的所有回环帧以及上述获取的当前帧预设时间段内的多个临近帧均作为当前帧的临近图像帧，进而将上述临近图像帧加入滑动窗口。
65.由此，可以实现采用滑动窗口策略对局部地图进行更新。
66.其中，上述滑动窗口策略为：将多个临近图像帧加入预设大小的滑动窗口时，将滑动窗口内对应的若干临近图像帧所对应的地图点生成当前局部地图，将未加入到滑动窗口对应的剩余临近图像帧所对应的地图点加入到全局地图；其中，将未加入到滑动窗口对应的剩余临近图像帧所对应的地图点加入到全局地图，包括：获取滑动窗口内的图像帧中里程计位姿与当前帧的里程计位姿之间的差值大于第一预设阈值的剩余临近图像帧，在剩余临近图像帧退出滑动窗口时，将剩余临近图像帧所对应的地图点从当前局部地图中去除，并将剩余临近图像帧所对应的地图点加入全局地图。
67.具体地，上述第一预设阈值包括第一预设距离阈值与第一预设角度阈值。在获取当前帧后，需要将滑动窗口内的帧对应的位置、朝向与当前帧的位置、朝向进行比较，若滑动窗口内某一帧的位置与当前帧的位置之间的差值大于第一预设距离阈值，或者滑动窗口内某一帧的朝向与当前帧的朝向之间的差值大于第一预设角度阈值，则确定该帧为剩余临近图像帧，进而将该剩余临近图像帧移出滑动窗口。
68.在获取当前帧的临近图像帧，将临近图像帧加入滑动窗口时，将临近图像帧对应的地图点从全局地图中去除，并将临近图像帧对应的地图点加入当前局部地图；在差异逐渐变大的剩余临近图像帧退出滑动窗口时，将剩余临近图像帧所对应的地图点从当前局部地图中去除，并将剩余临近图像帧所对应的地图点加入全局地图。
69.s15，将更新后的当前局部地图与rgb图进行融合，以更新全局地图。
70.具体地，参见图6，该将更新后的当前局部地图与rgb图进行融合，以更新全局地
图，包括如下步骤：
71.s151，对rgb图进行图像分割，划分为若干个超像素区域。
72.具体地，超像素区域由一系列位置相邻且颜色、亮度、纹理等特征相似的像素点组成的小区域。在本技术实施例中，对于当前帧，可以根据该帧中的rgb图中像素的坐标、颜色以及对应的深度值对rgb图进行划分，得到超像素区域。例如，可以将该rgb图中坐标差值小于等于预设坐标阈值、颜色差值小于等于预设颜色阈值、深度值差值小于等于预设深度值阈值的像素划分为一个超像素区域。再例如，可以预设多个坐标区间、颜色区间、深度值区间，进而将坐标位于一个坐标区间内、颜色位于一个颜色区间内且深度值位于一个深度值区间内的像素划分为一个超像素区域。
73.对于每一个超像素区域，获取组成其区域所包含的所有像素点的平均颜色、平均深度、平均法向量，并将该平均颜色、平均深度、平均法向量作为该超像素区域的颜色、深度、法向量。
74.其中，上述像素点的法向量的获取方法为：获取与某一个像素点附近的若干个像素点，例如，可以获取该像素点右边的像素点中与该像素点最近的像素点，并获取该像素点下面的像素点中与该像素点最近的像素点；进而在获取该像素点附近的若干个像素点后，利用该像素点与该像素点附近的若干个像素点构建一个平面，获取该平面的法向量，该平面的法向量中与摄像头具有相同朝向的法向量即为该像素点的法向量。
75.由此，可以实现对数据进行合并，减少内存占用，而且，在求均值的过程中还可以提高对噪声的鲁棒性。
76.可选地，在对rgb图进行图像分割时，还可以直接将该rgb图平均分割为若干个正方形，并将每个正方形近似为一个超像素区域。对于嵌入式设备，优选使用该获取超像素区域的方法。
77.s152，根据修正后的里程计位姿和移动设备上的图像传感器的配置参数，将更新后的当前局部地图中的每个地图点投影到rgb图所在平面上，获得每个地图点对应的像素坐标。
78.具体地，根据如下计算式将更新后的当前局部地图投影到rgb图上：
79.u＝k*t*p，
80.其中，k为图像传感器的配置参数，t为修正后的里程计位姿，p为更新后的当前局部地图中的每一个地图点，u为与每一个地图点对应的像素坐标。
81.s153，存在某一像素坐标处于任意一个超像素区域的范围内时，将像素坐标与对应的超像素区域进行特征差异比较。
82.具体地，对于某一个像素坐标，若其处于某个超像素区域的范围内，则将该像素坐标和该超像素区域进行法向量和深度值的差异比较。
83.s154，根据比较结果获取与该超像素区域是关于同一个环境特征的描述的像素坐标，并将像素坐标对应的地图点融入对应的超像素区域。
84.具体地，如果该像素坐标与该超像素区域的法向量方向接近且深度值差异不大，则可认为该像素坐标与该超像素区域是关于同一个环境特征的描述，可以对该像素坐标与该超像素区域进行融合，具体而言，对该像素坐标与该超像素区域的法向量、坐标、颜色分别进行加权融合，其中像素坐标的权重值是与其对应的局部地图点融合过的历史超像素区
域权重值的和，而该该超像素区域的权重值为其的深度的倒数的平方。
85.若存在没有被融合的超像素区域，则认为该超像素区域对应的环境特征为移动设备新观测到的环境特征，进而利用该超像素区域的颜色、法向量、深度以及与该超像素区域对应的里程计位姿在全局地图中创建新的地图点，从而实现对地图进行补充。
86.由此，可以实现对全局地图的更新。而且，在更新过程中通过将局部地图重新投影到rgb图上，实现无需针对深度图上的每个值均创建一个新的地图点，从而大幅降低三维重建过程中的内存占用，而且，通过像素坐标与超像素区域的融合，防止深度图的误差对三维重建构成影响，同时可以实现对于同一特征采用多次观测的数据来描述，从而进一步提高三维重建的精度。
87.在本发明的一个实施例中，虽然在对当前局部地图进行移动之前，需要首先对里程计位姿进行修正，但该修正并不能做到绝对消除误差，随着误差的累计，全局地图中的垂直与水平的平面如墙、桌子、地面、柜子等会不断变厚。为了应对该情况，在更新全局地图之后，三维地图重建方法还包括：对全局地图的精度进行维护处理。
88.具体地，获取全局地图中各个地图点的法向量，根据法向量获取垂直与水平的平面中的每一个地图点，并对每一个地图点均进行如下操作：获取全局地图中与该地图点最近的若干个点，利用该若干个点构建一个最小二乘平面。进而将该地图点沿着该最小二乘平面的法向量方向移动，直至该地图点落在该最小二乘平面上。
89.由此，可以实现对全局地图进行消误差处理，使得局部地图可以更好地与全局地图进行匹配。
90.在本发明的一个实施例中，在根据同一时刻的rgb图、深度图和里程计位姿确定当前局部地图后，还可根据预设方法判断该里程计位姿是否需要修正，进而若判断不需要修正，则省去上述步骤s13，直接根据里程计位姿对当前局部地图进行移动。例如，可以获取移动设备所使用的里程计的种类，若移动设备所使用的里程计为如imu(inertial measurement unit，惯性测量单元)里程计等容易产生误差的种类，则判断需要修正，若移动设备所使用的里程计是如激光里程计之类的较准确的里程计，则判断不需要修正。
91.在本发明的一个实施例中，在采用滑动窗口策略对当前的局部地图进行更新时，可以获取当前移动设备的运行时间，若当前移动设备的运行时间大于预设时间阈值，则采用上述实施例所示的方法。
92.若当前移动设备的运行时间小于等于预设时间阈值，移动设备可以首先根据当前帧对应的时间戳，获取当前帧预设时间段内的多个临近帧，将该多个临近帧加入滑动窗口，进而获取所有的历史帧对应的坐标，并将其与并将当前帧对应的坐标与历史帧对应的坐标进行比较，选取坐标之差小于预设距离阈值的历史帧。进一步地，获取当前帧的roll、pitch、yaw角，并获取上述选取的历史帧的roll、pitch、yaw角，计算上述当前帧与历史帧之间的roll、pitch、yaw角的差异，若某一个历史帧与当前帧之间的角度差异小于预设的角度阈值，则确定该历史帧为当前帧的临近图像帧。
93.需要说明的是，采用移动设备的运行时间作为判断标准仅仅为本发明实施例中的一个具体示例，在实际应用中并不局限于此，例如可以设置判断标准为历史帧的数量。
94.由此，可以实现在移动设备运行时间较短，获取的历史帧数量不大时，采用遍历历史帧的方式获取临近图像帧，以增大获取临近图像帧的准确性；而在历史帧数量较大时，采
用回环的方式获取临近图像帧，从而提高获取临近图像帧的效率。
95.在本发明的一个实施例中，在根据同一时刻的rgb图、深度图和里程计位姿确定当前局部地图之前，三维地图重建方法还包括：采用icp点云配准技术对里程计位姿进行实时矫正。即在确定同一时刻的rgb图、深度图和里程计位姿后，可以选择采用icp点云配准技术首先对里程计位姿进行矫正，再利用矫正后的里程计位姿确定当前局部地图。
96.综上，本发明实施例的三维地图重建方法，可以在确定同一时刻的rgb图、深度图、里程计位姿之后，根据该同一时刻的rgb图、深度图、里程计位姿确定当前局部地图，并根据当前局部地图匹配全局地图以对里程计位姿进行修正，以保证里程计位姿的精确度，避免里程计位姿的误差对三维重建构成影响。而且，在修正里程计位姿后，根据修正后的里程计位姿对当前局部地图进行移动更新。进一步地，使更新后的当前局部地图与rgb图进行融合以对全局地图进行更新，从而利用该融合大幅降低三维重建过程中的内存占用与算力要求，提高三维地图重建方法的运算速度，使得本发明实施例的三维地图重建方法可以被移动设备直接应用，无需额外设置gpu、高端cpu等设备，且能够实现在线运行，以及能够被运行于嵌入式设备上。而且，由于构建了局部地图，并利用局部地图对全局地图进行更新，可以进一步降低三维重建的算力要求。
97.进一步地，本发明提出一种移动设备。
98.在本发明实施例中，移动设备包括图像装置、存储器、控制器及存储在存储器上并可在控制器上运行的三维重建程序，控制器执行三维重建程序时，实现上述的三维地图重建方法。
99.本发明实施例的移动设备，通过实现上述的三维地图重建方法，可以在确定同一时刻的rgb图、深度图、里程计位姿之后，根据该同一时刻的rgb图、深度图、里程计位姿确定当前局部地图，并根据当前局部地图匹配全局地图以对里程计位姿进行修正，以保证里程计位姿的精确度，避免里程计位姿的误差对三维重建构成影响。而且，在修正里程计位姿后，根据修正后的里程计位姿对当前局部地图进行移动更新。进一步地，使更新后的当前局部地图与rgb图进行融合以对全局地图进行更新，从而利用该融合大幅降低三维重建过程中的内存占用与算力要求，提高三维地图重建方法的运算速度，使得本发明实施例的三维地图重建方法可以被移动设备直接应用，无需额外设置gpu、高端cpu等设备，且能够实现在线运行，以及能够被运行于嵌入式设备上。
100.进一步地，本发明提出一种计算机可读存储介质。
101.在本发明实施例中，计算机可读存储介质上存储有三维重建程序，该三维重建程序被处理器执行时实现上述的三维地图重建方法。
102.本发明实施例的计算机可读存储介质，在其上的计算机程序被处理器执行时，可以在确定同一时刻的rgb图、深度图、里程计位姿之后，根据该同一时刻的rgb图、深度图、里程计位姿确定当前局部地图，并根据当前局部地图匹配全局地图以对里程计位姿进行修正，以保证里程计位姿的精确度，避免里程计位姿的误差对三维重建构成影响。而且，在修正里程计位姿后，根据修正后的里程计位姿对当前局部地图进行移动更新。进一步地，使更新后的当前局部地图与rgb图进行融合以对全局地图进行更新，从而利用该融合大幅降低三维重建过程中的内存占用与算力要求，提高三维地图重建方法的运算速度，使得本发明实施例的三维地图重建方法可以被移动设备直接应用，无需额外设置gpu、高端cpu等设备，
且能够实现在线运行，以及能够被运行于嵌入式设备上。
103.需要说明的是，在流程图中表示或在此以其他方式描述的逻辑和/或步骤，例如，可以被认为是用于实现逻辑功能的可执行指令的定序列表，可以具体实现在任何计算机可读介质中，以供指令执行系统、装置或设备(如基于计算机的系统、包括处理器的系统或其他可以从指令执行系统、装置或设备取指令并执行指令的系统)使用，或结合这些指令执行系统、装置或设备而使用。就本说明书而言，"计算机可读介质"可以是任何可以包含、存储、通信、传播或传输程序以供指令执行系统、装置或设备或结合这些指令执行系统、装置或设备而使用的装置。计算机可读介质的更具体的示例(非穷尽性列表)包括以下：具有一个或多个布线的电连接部(电子装置)，便携式计算机盘盒(磁装置)，随机存取存储器(ram)，只读存储器(rom)，可擦除可编辑只读存储器(eprom或闪速存储器)，光纤装置，以及便携式光盘只读存储器(cdrom)。另外，计算机可读介质甚至可以是可在其上打印所述程序的纸或其他合适的介质，因为可以例如通过对纸或其他介质进行光学扫描，接着进行编辑、解译或必要时以其他合适方式进行处理来以电子方式获得所述程序，然后将其存储在计算机存储器中。
104.应当理解，本发明的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中，多个步骤或方法可以用存储在存储器中且由合适的指令执行系统执行的软件或固件来实现。例如，如果用硬件来实现，和在另一实施方式中一样，可用本领域公知的下列技术中的任一项或他们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路，具有合适的组合逻辑门电路的专用集成电路，可编程门阵列(pga)，现场可编程门阵列(fpga)等。
105.在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。
106.在本发明的描述中，需要理解的是，术语“中心”、“纵向”、“横向”、“长度”、“宽度”、“厚度”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”“内”、“外”、“顺时针”、“逆时针”、“轴向”、“径向”、“周向”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。
107.此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本发明的描述中，“多个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。
108.在本发明中，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”、“固定”等术语应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或成一体；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通或两个元件的相互作用关系，除非另有明确的限定。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。
109.在本发明中，除非另有明确的规定和限定，第一特征在第二特征“上”或“下”可以是第一和第二特征直接接触，或第一和第二特征通过中间媒介间接接触。而且，第一特征在第二特征“之上”、“上方”和“上面”可是第一特征在第二特征正上方或斜上方，或仅仅表示第一特征水平高度高于第二特征。第一特征在第二特征“之下”、“下方”和“下面”可以是第一特征在第二特征正下方或斜下方，或仅仅表示第一特征水平高度小于第二特征。
110.尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。

技术特征：
1.一种三维地图重建方法，其特征在于，应用于自移动设备中，所述方法包括：确定同一时刻的rgb图、深度图和里程计位姿；根据所述同一时刻的rgb图、深度图和里程计位姿确定当前局部地图；将所述当前局部地图与全局地图进行匹配，以对所述里程计位姿进行修正；根据修正后的里程计位姿和滑动窗口策略对所述当前局部地图进行更新；将更新后的当前局部地图与所述rgb图进行融合，以更新所述全局地图。2.根据权利要求1所述的三维地图重建方法，其特征在于，所述确定同一时刻的rgb图、深度图和里程计位姿，包括：获取当前时刻的深度图以及相邻时刻的若干rgb图；其中，所述深度图以及所述若干rgb图中每个rgb图均携带有对应的时间戳；计算所述若干rgb图中每个rgb图与所述深度图的时间戳差值；若存在所述时间戳差值不大于第一时间阈值且rgb图的数量为一个，则将所述rgb图与所述深度图视为同一时刻的rgb图和深度图，并将所述rgb图对应的位姿作为所述同一时刻的里程计位姿；若存在所述时间戳差值不大于第一时间阈值且rgb图的数量至少为两个，则将最新时间戳所对应的rgb图与所述深度图视为同一时刻的rgb图和深度图，且以所述深度图的时间戳为基准，采用线性插值的方式确定所述同一时刻的里程计位姿。3.根据权利要求2所述的三维地图重建方法，其特征在于，所述以所述深度图的时间戳为基准，采用线性插值的方式确定所述同一时刻的里程计位姿，包括：获取与所述深度图的时间戳差值最小的第一rgb图和第二rgb图；其中，所述第一rgb图携带第一时间戳以及第一位姿信息，所述第二rgb图携带有第二时间戳以及第二位姿信息；根据所述第一时间戳与所述第二时间戳的比例关系，得到所述第一rgb图与所述深度图的第一权重以及所述第二rgb图与所述深度图的第二权重；根据所述第一位姿信息、所述第一权重以及所述第二位姿信息、第二权重，得到理论里程计位姿，以作为所述同一时刻的里程计位姿。4.根据权利要求1或2所述的三维地图重建方法，其特征在于，在根据所述同一时刻的rgb图、深度图和里程计位姿确定当前局部地图之前，所述方法还包括：采用icp点云配准技术对所述里程计位姿进行实时矫正。5.根据权利要求1所述的三维地图重建方法，其特征在于，所述将所述当前局部地图与全局地图进行匹配，以对所述里程计位姿进行修正，包括：获取所述里程计位姿对应的当前局部地图；根据所述当前局部地图中的每个地图点，在所述全局地图中寻找与对应的地图点相匹配的点云集；根据所述点云集构建最小二乘平面，并根据所述每个地图点到对应的最小二乘平面的距离来计算所述里程计位姿的第一修正量；根据所述第一修改量对所述里程计位姿进行修正，得到修正后的里程计位姿。6.根据权利要求5所述的三维地图重建方法，其特征在于，所述将所述当前局部地图与全局地图进行匹配，以对所述里程计位姿进行修正之后，包括：根据所述里程计位姿和所述修正后的里程计位姿，得到所述里程计位姿的相对位姿变
换矩阵；获取所述当前局部地图中每个地图点的坐标信息；将所述相对位姿变换矩阵分别乘以每个地图点的坐标信息，得到各地图点更新后的坐标信息。7.根据权利要求1所述的三维地图重建方法，其特征在于，所述根据修正后的里程计位姿和滑动窗口策略对所述当前局部地图进行更新，包括：将一组所述同一时刻的rgb图和深度图作为一帧且该帧携带有所述里程计位姿；以该帧作为当前时刻对应的当前帧，获取所述当前帧对应的多个临近图像帧；所述滑动窗口策略为：将多个所述临近图像帧加入预设大小的滑动窗口时，将所述滑动窗口内对应的若干临近图像帧所对应的地图点生成当前局部地图，将未加入到所述滑动窗口对应的剩余临近图像帧所对应的地图点加入到全局地图；其中，所述将未加入到所述滑动窗口对应的剩余临近图像帧所对应的地图点加入到全局地图，包括：获取所述滑动窗口内的图像帧中里程计位姿与所述当前帧的里程计位姿之间的差值大于第一预设阈值的所述剩余临近图像帧，在所述剩余临近图像帧退出所述滑动窗口时，将所述剩余临近图像帧所对应的地图点从当前局部地图中去除，并将所述剩余临近图像帧所对应的地图点加入所述全局地图。8.根据权利要求7所述的三维地图重建方法，其特征在于，所述临近图像帧包括多个临近帧和多个历史帧，所述获取所述当前帧对应的多个临近图像帧，包括：获取所述当前帧的所述里程计位姿；其中，所述当前帧携带有时间戳；根据所述当前帧对应的时间戳，获取所述当前帧预设时间段内的多个临近帧；对所述当前帧进行预设次数的回环检测得到若干回环帧，所述回环检测包括：根据所述当前帧获取目标帧，并获取所述目标帧的多个候选历史帧，根据各候选历史帧的里程计位姿与所述目标帧的里程计位姿之间的差值，从所述多个候选历史帧中筛选出差值小于第二预设阈值的候选历史帧作为回环帧；其中，每个候选历史帧携带有对应的里程计位姿；将所述若干回环帧与所述多个临近帧均作为所述当前帧的临近图像帧。9.根据权利要求1所述的三维地图重建方法，其特征在于，将更新后的当前局部地图与所述rgb图进行融合，包括：对所述rgb图进行图像分割，划分为若干个超像素区域；根据修正后的里程计位姿和所述移动设备上的图像传感器的配置参数，将更新后的当前局部地图中的每个地图点投影到所述rgb图所在平面上，获得每个地图点对应的像素坐标；存在某一像素坐标处于任意一个所述超像素区域的范围内时，将所述像素坐标与对应的超像素区域进行特征差异比较；根据比较结果获取与该超像素区域是关于同一个环境特征的描述的像素坐标，并将所述像素坐标对应的地图点融入对应的所述超像素区域。10.根据权利要求9所述的三维地图重建方法，其特征在于，所述对所述rgb图进行图像分割，划分为若干个超像素区域，包括：根据所述rgb图中像素的坐标、颜色以及对应的深度值对所述rgb图进行划分，得到所
述超像素区域。11.一种自移动设备，其特征在于，包括图像装置、存储器、控制器及存储在存储器上并可在控制器上运行的三维重建程序，所述控制器执行所述三维重建程序时，实现根据权利要求1-10中任一项所述的三维地图重建方法。12.一种计算机可读存储介质，其特征在于，其上存储有三维重建程序，该三维重建程序被处理器执行时实现根据权利要求1-10中任一项所述的三维地图重建方法。

技术总结
本发明公开了一种三维地图重建方法、自移动设备及计算机可读存储介质。其中，三维地图重建方法应用于自移动设备中，方法包括：确定同一时刻的RGB图、深度图和里程计位姿；根据同一时刻的RGB图、深度图和里程计位姿确定当前局部地图；将当前局部地图与全局地图进行匹配，以对里程计位姿进行修正；根据修正后的里程计位姿和滑动窗口策略对当前局部地图进行更新；将更新后的当前局部地图与RGB图进行融合，以更新全局地图。该三维地图重建方法，可以降低运行时的内存占用，降低算力要求，可被应用于移动设备端。用于移动设备端。用于移动设备端。


技术研发人员：杨勇 胡瑞琪 宋昱慧
受保护的技术使用者：深圳市杉川机器人有限公司
技术研发日：2022.02.11
技术公布日：2022/5/25