CN108564625B - 图优化方法、装置、电子设备及存储介质 - Google Patents

Abstract

本发明提出一种图优化方法、装置、电子设备及存储介质。其中方法包括：预先构建SLAM的待优化图，图的节点表示待求解的线性方程的待优化变量，边表示待优化变量的约束关系，节点包括第一节点和第二节点，分别表示三维点坐标和相机运动参数；将待优化图中的第一节点复制成两个以上第三节点，使第三节点中的任意一个与n个第二约束节点存在约束关系且在n个第二约束节点中有m个与相邻的第三节点存在相同的约束关系，第二约束节点与至少一个第三节点存在约束关系，n为预设的控制参数，第二约束节点是与第一节点存在约束关系的第二节点；边缘化第三节点以减小待求解的线性方程的稠密程度。本发明实施例提升了优化效率，降低了计算功耗和硬件成本。

Description

图优化方法、装置、电子设备及存储介质

技术领域

本发明涉及信息技术领域，尤其涉及一种图优化方法、装置、电子设备及计算机可读存储介质。

背景技术

移动机器人、无人车、无人机等是目前科学技术发展最为活跃的领域之一，有着广泛的应用前景。以移动机器人为例，其应用中的一个至关重要的环节是需要实时定位机器人在场景中的方位。目前主流的方案需要依靠激光雷达、差分GPS(Global PositioningSystem，全球定位系统)、高精度惯导系统等昂贵的硬件设备，或事先在场景中部署信号源或电磁轨道，才能实现精确定位和导航，极大限制了移动机器人的应用场景。SLAM(Simultaneous Localization and Mapping，同时定位与制图)技术能通过低成本的相机、IMU(Inertial Measurement Unit，惯性测量单元)等传感器，即可在未知环境中实现移动机器人的自身定位，从而突破上述方案在硬件成本、应用场景方面的局限。

在实际应用中，SLAM的定位精度和计算效率是最为重要的两项指标。现有的SLAM方法在精度和效率方面存在权衡。高精度的SLAM方案计算量大、功耗高，需要配备高性能计算设备，加大了硬件成本；高效的SLAM方案又往往需要牺牲定位精度，无法满足高精度的定位需求。从实现框架上，可以将SLAM分解为前端的图构建和后端的图优化两个过程。求解这种图优化问题，通常将图转换为一个线性方程的求解问题，通常通过边缘化(marginalization)三维点的方式减小线性方程的系数矩阵的稠密程度。

现有技术的缺陷在于：由于IMU约束随着两帧时间间隔增加而减弱，为了更好地利用IMU信息，通常需要较为稠密地选择待优化相机的连续帧图像，导致图优化效率下降。原因如下：1)同一个三维点在很多的相机连续帧图像中可见，边缘化这个三维点需要计算非常多的额外约束；2)边缘化所有三维点后得到的RCS(Reduced Camera System)非常稠密，相机的两个连续帧图像只要能同时看见同一个三维点就有一个非零分量。

发明内容

本发明实施例提供一种图优化方法、装置、电子设备及计算机可读存储介质，以至少解决现有技术中的一个或多个技术问题。

第一方面，本发明实施例提供了一种图优化方法，包括：预先构建同时定位与制图SLAM的待优化图，所述待优化图包括节点和连接两个所述节点构成的边，所述节点用于表示待求解的线性方程的待优化变量，所述边用于表示所连接的两个节点对应的待优化变量之间的约束关系，其中，所述待优化变量包括待求解的三维点坐标和待求解的相机运动参数，所述节点包括第一节点和第二节点，所述第一节点用于表示所述待求解的三维点坐标，所述第二节点用于表示所述待求解的相机运动参数；将预先构建的所述SLAM的待优化图中的所述第一节点复制成两个以上第三节点，并使所述第三节点中的任意一个与n个第二约束节点存在约束关系且在n个所述第二约束节点中有m个所述第二约束节点与相邻的所述第三节点存在相同的约束关系，所述第二约束节点与至少一个所述第三节点存在约束关系，其中，n为预设的控制参数，n-1≥m≥1,所述第二约束节点是与所述第一节点存在约束关系的所述第二节点；用所述第三节点代替对应的预先构建的所述SLAM的待优化图中的所述第一节点，得到复制节点后的SLAM的待优化图；以及边缘化所述第三节点，以减小所述待求解的线性方程的系数矩阵的稠密程度。

结合第一方面，本发明在第一方面的第一种实施方式中，所述第三节点包括：与n个连续帧中的所述待求解的相机运动参数存在约束关系的节点。

结合第一方面，本发明在第一方面的第二种实施方式中，用所述第三节点代替对应的预先构建的所述SLAM的待优化图中的所述第一节点，得到复制节点后的SLAM的待优化图，还包括：设置所述第三节点的每条边的权重，以使所述复制节点后的SLAM的待优化图与预先构建的所述SLAM的待优化图的能量值相同。

结合第一方面、第一方面的第一种实施方式、第一方面的第二种实施方式，本发明在第一方面的第三种实施方式中，在边缘化所述第三节点之后，还包括：求解边缘化后的线性方程，得到所述相机运动参数；将所述相机运动参数代回预先构建的所述SLAM的待优化图，求解预先构建的所述SLAM的待优化图中的所述三维点坐标。

结合第一方面的第三种实施方式，本发明在第一方面的第四种实施方式中，所述求解边缘化后的线性方程，包括：用预条件共轭梯度法求解所述边缘化后的线性方程。

第二方面，本发明实施例提供了一种图优化装置，包括：图构建单元，用于预先构建同时定位与制图SLAM的待优化图，所述待优化图包括节点和连接两个所述节点构成的边，所述节点用于表示待求解的线性方程的待优化变量，所述边用于表示所连接的两个节点对应的待优化变量之间的约束关系，其中，所述待优化变量包括待求解的三维点坐标和待求解的相机运动参数，所述节点包括第一节点和第二节点，所述第一节点用于表示所述待求解的三维点坐标，所述第二节点用于表示所述待求解的相机运动参数；节点复制单元，用于将预先构建的所述SLAM的待优化图中的所述第一节点复制成两个以上第三节点，并使所述第三节点中的任意一个与n个第二约束节点存在约束关系且在n个所述第二约束节点中有m个所述第二约束节点与相邻的所述第三节点存在相同的约束关系，所述第二约束节点与至少一个所述第三节点存在约束关系，其中，n为预设的控制参数，n-1≥m≥1,所述第二约束节点是与所述第一节点存在约束关系的所述第二节点；图优化单元，用于用所述第三节点代替对应的预先构建的所述SLAM的待优化图中的所述第一节点，得到复制节点后的SLAM的待优化图；以及边缘化单元，用于边缘化所述第三节点，以减小所述待求解的线性方程的系数矩阵的稠密程度。

结合第二方面，本发明在第二方面的第一种实施方式中，所述第三节点包括：与n个连续帧中的所述待求解的相机运动参数存在约束关系的节点。

结合第二方面，本发明在第二方面的第二种实施方式中，所述图优化单元还用于：设置所述第三节点的每条边的权重，以使所述复制节点后的SLAM的待优化图与预先构建的所述SLAM的待优化图的能量值相同。

结合第二方面、第二方面的第一种实施方式、第二方面的第二种实施方式，本发明在第二方面的第三种实施方式中，还包括方程求解单元，用于：求解边缘化后的线性方程，得到所述相机运动参数；将所述相机运动参数代回预先构建的所述SLAM的待优化图，求解预先构建的所述SLAM的待优化图中的所述三维点坐标。

结合第二方面的第三种实施方式，本发明在第二方面的第四种实施方式中，所述方程求解单元还用于：用预条件共轭梯度法求解所述边缘化后的线性方程。

在一个可能的设计中，图优化装置的结构中包括处理器和存储器，所述存储器用于存储支持图优化装置执行上述第一方面中图优化方法的程序，所述处理器被配置为用于执行所述存储器中存储的程序。

第三方面，本发明实施例提供了一种电子设备，包括：一个或多个处理器；存储装置，用于存储一个或多个程序；当所述一个或多个程序被所述一个或多个处理器执行时，使得所述一个或多个处理器实现上述第一方面中任一所述的方法。

第四方面，本发明实施例提供了一种计算机可读存储介质，其存储有计算机程序，该程序被处理器执行时实现上述第一方面任一所述的方法。

上述技术方案具有如下优点或有益效果：在不牺牲优化精度的同时，极大地提升了优化效率；在实现同样功能的情况下，降低了计算功耗和硬件成本。

上述概述仅仅是为了说明书的目的，并不意图以任何方式进行限制。除上述描述的示意性的方面、实施方式和特征之外，通过参考附图和以下的详细描述，本发明进一步的方面、实施方式和特征将会是容易明白的。

附图说明

在附图中，除非另外规定，否则贯穿多个附图相同的附图标记表示相同或相似的部件或元素。这些附图不一定是按照比例绘制的。应该理解，这些附图仅描绘了根据本发明公开的一些实施方式，而不应将其视为是对本发明范围的限制。

图1为SLAM后端优化方法的待优化图示例；

图2为现有技术的SLAM的待优化图的边缘化变量示意图；

图3为本发明实施例的图优化方法的整体框架图；

图4为本发明提供的图优化方法的一种优选实施例的节点复制示意图；

图5为本发明提供的图优化方法的一种优选实施例的步骤流程图；

图6是本发明提供的图优化方法的预条件子矩阵示意图；

图7为本发明实施例的图优化装置的整体框架图；

图8为本发明提供的图优化装置的一种优选实施例的结构示意图。

具体实施方式

在下文中，仅简单地描述了某些示例性实施例。正如本领域技术人员可认识到的那样，在不脱离本发明的精神或范围的情况下，可通过各种不同方式修改所描述的实施例。因此，附图和描述被认为本质上是示例性的而非限制性的。

移动机器人，如仓储、物流、服务机器人，无人车、无人机等，能够协助或取代人类工作，有着广泛的应用前景。以移动机器人为例，其应用中一个至关重要的环节是需要实时定位机器人在场景中的方位。只有获取了机器人的方位信息，才能控制其如何移动。通过SLAM技术可在未知环境中实现移动机器人的自身定位，具体地，SLAM问题可以描述为:机器人在未知环境中从一个未知位置开始移动，在移动过程中根据位置估计和地图进行自身定位，同时在自身定位的基础上建造增量式地图，实现机器人的自主定位和导航。纯视觉SLAM简称V-SLAM；融合视觉和IMU数据的SLAM称为Visual-Inertial SLAM，简称VI-SLAM。

以VI-SLAM为例，VI-SLAM的计算分为前端和后端两个部分。前端负责从传感器中提取可用于判断机器人运动的观测量，比如提取场景在图像中的运动；后端由前端提取的观测量，同时推测机器人的运动以及场景的三维结构，比如随着机器人运动，场景在图像中不断放大，则可推测出机器人正在靠近场景。前端本质上可以看成是一个图像处理的过程，可以通过特定的硬件提升计算效率，比如通过GPU(Graphics Processing Unit，图形处理器)、FPGA(Field－Programmable Gate Array，现场可编程门阵列)等提升计算效率。后端本质上是一个优化问题，通过降低优化算法的复杂度提升计算效率。

以VI-SLAM为例，现有的VI-SLAM后端优化方法可以归结为一个图优化问题，待优化变量为图的节点，两变量之间若存在约束，则为两节点间连一条边。对于VI-SLAM问题，优化图有着特殊的结构，优化图中的变量包括待求解的相机运动参数C₁、C₂、…、C_n和待求解的三维点坐标X₁、X₂、…、X_m。其中，C₁、C₂、…、C_n表示在连续帧中的相机运动参数，相机运动参数包括相机的朝向、相机的位置、相机的线速度，以及IMU的漂移量；X₁、X₂、…、X_m表示在特征点的三维坐标，例如边缘点或角点的三维坐标。

图1为SLAM后端优化方法的待优化图示例。如图1所示，图1中节点C₁、C₂、C₃、C₄为待求解的相机运动参数，X₁、X₂、X₃为待求解的三维点坐标。VI-SLAM有两种类型的观测：视觉观测，即相机对应图像中观测一个三维点，每个三维点坐标X与若干个图像帧中的相机运动参数C存在约束关系，对应于图1中的若干条从三维点坐标X到相机运动参数C的边；IMU观测，约束相邻图像帧中的两个相机运动参数，对应于图1中的若干条连接两个相机运动参数C之间的边。SLAM算法要计算三维点坐标X和相机运动参数C的精确值才能实现定位。

求解这种图优化问题，通常将图转换为一个线性方程的求解问题。SLAM问题待优化变量数目极大，直接求解并不现实，通常通过边缘化(marginalization)的方式减小线性方程的系数矩阵的稠密程度。边缘化也就是去掉变量，对方程作消元处理，在SLAM问题中通常通过边缘化(marginalization)三维点的方式减小线性方程的系数矩阵的稠密程度。将一个变量边缘化后，其连接的变量两两之间会产生额外约束。图2为现有技术的SLAM的待优化图的边缘化变量示意图。如图2所示，在边缘化的过程中，将变量X₁作消元处理，参见图2中的带有叉号的边，将X₁和其连接的约束边缘化时，C₁、C₂、C₃间会产生额外约束，产生的额外约束对应于图2中用虚线表示的边。

图优化的效率极大取决于边缘化变量的顺序，不同的边缘化顺序会产生不同的稀疏性，越稀疏的图计算效率越高。对于VI-SLAM问题，由于图像帧中相机运动参数的个数通常远小于三维点个数，且可观测到某个三维点的相机图像帧的个数通常远小于某个图像帧中相机可观测到的三维点数，因此最高效的图优化步骤为：

1)逐一边缘化所有三维点，得到一个只与相机变量有关的较小的线性系统，称为RCS(Reduced Camera System)；

2)求解RCS，得到相机运动参数；

3)已知相机运动参数，代回边缘化前的图，逐一求解三维点坐标。

现有技术的缺陷在于：由于IMU约束随着两帧时间间隔增加而减弱。为了更好的利用IMU信息，通常需要较为稠密地选择待优化相机的连续帧图像，导致图优化效率下降。原因如下：

1)同一个三维点在很多的相机连续帧图像中可见，边缘化这个三维点需要计算非常多的额外约束。

2)边缘化所有三维点后得到的RCS非常稠密，相机的两个连续帧图像只要能同时看见同一个三维点就有一个非零分量。其中，非零分量对应于图示中的用虚线表示的边。

有鉴于此，本发明实施例提供了一种图优化方法。图3为本发明实施例的图优化方法的整体框架图。如图3所示，本发明实施例的图优化方法，包括：步骤S110，预先构建同时定位与制图SLAM的待优化图，所述待优化图包括节点和连接两个所述节点构成的边，所述节点用于表示待求解的线性方程的待优化变量，所述边用于表示所连接的两个节点表示的待优化变量之间的约束关系，其中，所述待优化变量包括待求解的三维点坐标和待求解的相机运动参数，所述节点包括第一节点和第二节点，所述第一节点用于表示所述待求解的三维点坐标，所述第二节点用于表示所述待求解的相机运动参数；步骤S120，将预先构建的所述SLAM的待优化图中的所述第一节点复制成两个以上第三节点，并使所述第三节点中的任意一个与n个第二约束节点存在约束关系且在n个所述第二约束节点中有m个所述第二约束节点与相邻的所述第三节点存在相同的约束关系，所述第二约束节点与至少一个所述第三节点存在约束关系，其中，n为预设的控制参数，n-1≥m≥1,所述第二约束节点是与所述第一节点存在约束关系的所述第二节点；步骤S130，用所述第三节点代替对应的预先构建的所述SLAM的待优化图中的所述第一节点，得到复制节点后的SLAM的待优化图；以及步骤S140，边缘化所述第三节点，以减小所述待求解的线性方程的系数矩阵的稠密程度。

本发明的实施例通过复制表示三维点坐标的节点，使复制后的每个表示三维点坐标的节点，也就是第三节点，只与n个相机运动参数存在约束关系，由于每边缘化一个复制点，仅产生n个相机运动参数两两间的额外约束，因此大大减少了由于边缘化所产生的额外约束的数量。本发明的实施例在不牺牲优化精度的前提下，提升了后端优化效率；在实现同样功能的情况下，降低了计算功耗和硬件成本。

图4为本发明提供的图优化方法的一种优选实施例的节点复制示意图。如图4所示，将一个三维点X复制成若干个三维坐标与原三维点相同、只约束n个连续帧相机的复制点，n为控制参数。参见图4(a)，在原图中，三维点X约束了{C₁、C₂、C₃、C₄、C₅}；再参见图4(b)，将约束了{C₁、C₂、C₃、C₄、C₅}的三维点X复制成X₁、X₂、X₃，其中，X₁约束相机运动参数{C₁、C₂、C₃}、X₂约束相机运动参数{C₂、C₃、C₄}、X₃约束相机运动参数{C₃、C₄、C₅}。

然后逐一边缘化复制点。由于每边缘化一个复制点，仅产生n个连续帧图像中相机运动参数的两两间的额外约束，边缘化得到的RCS非常稀疏，提升了后端优化效率。

根据本发明图优化方法的一种实施方式，所述第三节点包括：与n个连续帧中的所述待求解的相机运动参数存在约束关系的节点。具体地，首先设置控制参数n，控制复制后的每个表示三维点坐标的节点只与n个相机运动参数存在约束关系，一般地可取n＝5；在图4(b)中以取值n＝3、m＝2的情况示例，将三维点X复制成三维坐标相同的三个复制点X₁、X₂、X₃，这三个复制点分别约束3个连续帧中的相机运动参数，在一个复制点约束的3个第二约束节点中有2个与相邻的第三节点存在相同的约束关系，如复制点X₁约束的3个第二约束节点C₁、C₂、C₃中，C₂、和C₃与复制点X₂存在相同的约束关系。通过复制节点，限制了每个节点约束的相机运动参数的节点个数，复制节点后的SLAM的待优化图与之前相比，有效控制了由于边缘化而产生的额外约束的数量。

根据本发明图优化方法的一种实施方式，用所述第三节点代替对应的预先构建的所述SLAM的待优化图中的所述第一节点，得到复制节点后的SLAM的待优化图，还包括：设置所述第三节点的每条边的权重，以使所述复制节点后的SLAM的待优化图与预先构建的所述SLAM的待优化图的能量值相同。

能量优化方法即寻找研究体系能量极小的状态，从数学角度看是处理多维函数的优化问题。图优化的过程也就是使能量函数的值最小化的过程。图的总能量值是图中所有的边的能量值的加权和，权重是根据每条边在定位与制图中的重要性设置的系数，图的总能量值等于每条边的能量值与权重相乘之后再相加的总和。本发明实施例通过设置每条新边的权重，保证复制前后待优化图的能量完全相同，从这个意义上说，复制节点是将原待优化图做了等价变换，以此来达到减少边缘化产生的额外约束的目的。

图5为本发明提供的图优化方法的一种优选实施例的步骤流程图。如图5所示，根据本发明图优化方法的一种实施方式，在步骤S140，边缘化所述第三节点之后，还包括：步骤S150，求解边缘化后的线性方程，得到所述相机运动参数；步骤S160，将所述相机运动参数代回预先构建的所述SLAM的待优化图，求解预先构建的所述SLAM的待优化图中的所述三维点坐标。

参见图4及图5，在逐一边缘化复制点之后，求解稀疏的RCS，得到相机运动参数。最后将相机运动参数代回原图(图4(a))，逐一求解原三维点坐标。

根据本发明图优化方法的一种实施方式，所述求解边缘化后的线性方程，包括：用预条件共轭梯度法求解所述边缘化后的线性方程。预条件共轭梯度法(PreconditionedConjugate Gradient，简称PCG)是解线性方程组的有效方法之一，它是基于共轭梯度法的收敛速度依赖于系数矩阵A的特征值分布这一性态，在使用时先对A进行预处理使其特征值分布较为集中，进而提高其收敛速度。

图6是本发明提供的图优化方法的预条件子矩阵示意图。用预条件共轭梯度法求解，预条件子(Preconditioner)选为RCS中带宽2n-1的对角线带状子矩阵。如n＝2时，预条件子如图6所示。相机在移动的过程中摄取图像，如果经过一段时间相机又回到的原来的位置，这种情况下会存在循环回路。在这种实施方式中，如果不存在循环回路，即第i帧相机与第(i-n,i+n)帧之外的相机并无约束，则预条件子与RCS相同，PCG一次收敛；即使存在循环回路约束，通常这样的约束也非常稀疏，因此这样的预条件子也能极大加快PCG的收敛速度。

本发明实施例在不牺牲优化精度的同时，极大了提升优化效率。其中，不牺牲精度的原因有：a)复制前后待优化图的能量完全相同；b)由于是将相机运动参数代回预先构建的所述SLAM的待优化图，求解预先构建的所述SLAM的待优化图中的所述三维点坐标，如在前文的示例中，最后是将相机运动参数代回原图(图4(a))，逐一求解原三维点坐标，由此可见，求解三维点所用的约束条件与原问题完全相同。另外，效率提升的原因有：a)同一个复制三维点仅在n个连续帧的相机中可见，边缘化这个复制点需要计算的额外约束数量极大减少；b)边缘化所有复制点后得到的RCS有着特殊的稀疏结构，通常情况下是对角线带状和循环回路产生的稀疏的回路约束，可以利用PCG非常高效地求解。

另一方面，本发明实施例提供了一种图优化装置。图7为本发明实施例的图优化装置的整体框架图。如图7所示，本发明实施例的图优化装置包括：图构建单元100，用于预先构建同时定位与制图SLAM的待优化图，所述待优化图包括节点和连接两个所述节点构成的边，所述节点用于表示待求解的线性方程的待优化变量，所述边用于表示所连接的两个节点表示的待优化变量之间的约束关系，其中，所述待优化变量包括待求解的三维点坐标和待求解的相机运动参数，所述节点包括第一节点和第二节点，所述第一节点用于表示所述待求解的三维点坐标，所述第二节点用于表示所述待求解的相机运动参数；节点复制单元200，用于将预先构建的所述SLAM的待优化图中的所述第一节点复制成两个以上第三节点，并使所述第三节点中的任意一个与n个第二约束节点存在约束关系且在n个所述第二约束节点中有m个所述第二约束节点与相邻的所述第三节点存在相同的约束关系，所述第二约束节点与至少一个所述第三节点存在约束关系，其中，n为预设的控制参数，n-1≥m≥1,所述第二约束节点是与所述第一节点存在约束关系的所述第二节点；图优化单元300，用于用所述第三节点代替对应的预先构建的所述SLAM的待优化图中的所述第一节点，得到复制节点后的SLAM的待优化图；以及边缘化单元400，用于边缘化所述第三节点，以减小所述待求解的线性方程的系数矩阵的稠密程度。

根据本发明图优化装置的一种实施方式，所述第三节点包括：与n个连续帧中的所述待求解的相机运动参数存在约束关系的节点。

根据本发明图优化装置的一种实施方式，所述图优化单元300还用于：设置所述第三节点的每条边的权重，以使所述复制节点后的SLAM的待优化图与预先构建的所述SLAM的待优化图的能量值相同。

图8为本发明提供的图优化装置的一种优选实施例的结构示意图。如图8所示，根据本发明图优化装置的一种实施方式，还包括方程求解单元500，用于：求解边缘化后的线性方程，得到所述相机运动参数；将所述相机运动参数代回预先构建的所述SLAM的待优化图，求解预先构建的所述SLAM的待优化图中的所述三维点坐标。

根据本发明图优化装置的一种实施方式，所述方程求解单元500还用于：用预条件共轭梯度法求解所述边缘化后的线性方程。

在一个可能的设计中，图优化装置的结构中包括处理器和存储器，所述存储器用于存储支持图优化装置执行上述图优化方法中的程序，所述处理器被配置为用于执行所述存储器中存储的程序。

又一方面，本发明实施例提供了一种电子设备，包括：一个或多个处理器；存储装置，用于存储一个或多个程序；当所述一个或多个程序被所述一个或多个处理器执行时，使得所述一个或多个处理器实现上述图优化方法中任一所述的方法。

再一方面，本发明实施例提供了一种计算机可读存储介质，其存储有计算机程序，该程序被处理器执行时实现上述图优化方法中任一所述的方法。

上述技术方案具有如下优点或有益效果：在不牺牲优化精度的同时，极大地提升了优化效率；在实现同样功能的情况下，降低了计算功耗和硬件成本。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。

此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或隐含地包括至少一个该特征。在本发明的描述中，“多个”的含义是两个或两个以上，除非另有明确具体的限定。

流程图中或在此以其他方式描述的任何过程或方法描述可以被理解为，表示包括一个或更多个用于实现特定逻辑功能或过程的步骤的可执行指令的代码的模块、片段或部分，并且本发明的优选实施方式的范围包括另外的实现，其中可以不按所示出或讨论的顺序，包括根据所涉及的功能按基本同时的方式或按相反的顺序，来执行功能，这应被本发明的实施例所属技术领域的技术人员所理解。

在流程图中表示或在此以其他方式描述的逻辑和/或步骤，例如，可以被认为是用于实现逻辑功能的可执行指令的定序列表，可以具体实现在任何计算机可读介质中，以供指令执行系统、装置或设备(如基于计算机的系统、包括处理器的系统或其他可以从指令执行系统、装置或设备取指令并执行指令的系统)使用，或结合这些指令执行系统、装置或设备而使用。就本说明书而言，“计算机可读介质”可以是任何可以包含、存储、通信、传播或传输程序以供指令执行系统、装置或设备或结合这些指令执行系统、装置或设备而使用的装置。计算机可读介质的更具体的示例(非穷尽性列表)包括以下：具有一个或多个布线的电连接部(电子装置)，便携式计算机盘盒(磁装置)，随机存取存储器(RAM)，只读存储器(ROM)，可擦除可编辑只读存储器(EPROM或闪速存储器)，光纤装置，以及便携式只读存储器(CDROM)。另外，计算机可读介质甚至可以是可在其上打印所述程序的纸或其他合适的介质，因为可以例如通过对纸或其他介质进行光学扫描，接着进行编辑、解译或必要时以其他合适方式进行处理来以电子方式获得所述程序，然后将其存储在计算机存储器中。

应当理解，本发明的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中，多个步骤或方法可以用存储在存储器中且由合适的指令执行系统执行的软件或固件来实现。例如，如果用硬件来实现，和在另一实施方式中一样，可用本领域公知的下列技术中的任一项或他们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路，具有合适的组合逻辑门电路的专用集成电路，可编程门阵列(PGA)，现场可编程门阵列(FPGA)等。

本技术领域的普通技术人员可以理解实现上述实施例方法携带的全部或部分步骤是可以通过程序来指令相关的硬件完成，所述的程序可以存储于一种计算机可读存储介质中，该程序在执行时，包括方法实施例的步骤之一或其组合。其中装置实施方式与方法的实施方式相对应，因此装置的实施方式描述比较简略，相关描述可参照方法的实施方式的描述即可。

此外，在本发明各个实施例中的各功能单元可以集成在一个处理模块中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个模块中。上述集成的模块既可以采用硬件的形式实现，也可以采用软件功能模块的形式实现。所述集成的模块如果以软件功能模块的形式实现并作为独立的产品销售或使用时，也可以存储在一个计算机可读存储介质中。所述存储介质可以是只读存储器，磁盘或光盘等。

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

Claims (12)

1.一种图优化方法，其特征在于，包括：

预先构建同时定位与制图SLAM的待优化图，所述待优化图包括节点和连接两个所述节点构成的边，所述节点用于表示待求解的线性方程的待优化变量，所述边用于表示所连接的两个节点对应的待优化变量之间的约束关系，其中，所述待优化变量包括待求解的三维点坐标和待求解的相机运动参数，所述节点包括第一节点和第二节点，所述第一节点用于表示所述待求解的三维点坐标，所述第二节点用于表示所述待求解的相机运动参数；

将预先构建的所述SLAM的待优化图中的所述第一节点复制成两个以上第三节点，并使所述第三节点中的任意一个与n个第二约束节点存在约束关系且在n个所述第二约束节点中有m个所述第二约束节点与相邻的所述第三节点存在相同的约束关系，所述第二约束节点与至少一个所述第三节点存在约束关系，其中，n为预设的控制参数，n-1≥m≥1,所述第二约束节点是与所述第一节点存在约束关系的所述第二节点；

用所述第三节点代替对应的预先构建的所述SLAM的待优化图中的所述第一节点，得到复制节点后的SLAM的待优化图；以及

边缘化所述第三节点，以减小所述待求解的线性方程的系数矩阵的稠密程度。

2.根据权利要求1所述的方法，其特征在于，还包括：所述第三节点包括：与n个连续帧中的所述待求解的相机运动参数存在约束关系的节点，其中，n为所述预设的控制参数。

3.根据权利要求1所述的方法，其特征在于，用所述第三节点代替对应的预先构建的所述SLAM的待优化图中的所述第一节点，得到复制节点后的SLAM的待优化图，还包括：

设置所述第三节点的每条边的权重，以使所述复制节点后的SLAM的待优化图与预先构建的所述SLAM的待优化图的能量值相同。

4.根据权利要求1-3中任一项所述的方法，其特征在于，在边缘化所述第三节点之后，还包括：

求解边缘化后的线性方程，得到所述相机运动参数；

将所述相机运动参数代回预先构建的所述SLAM的待优化图，求解预先构建的所述SLAM的待优化图中的所述三维点坐标。

5.根据权利要求4所述的方法，其特征在于，所述求解边缘化后的线性方程，包括：用预条件共轭梯度法求解所述边缘化后的线性方程。

6.一种图优化装置，其特征在于，包括：

图构建单元，用于预先构建同时定位与制图SLAM的待优化图，所述待优化图包括节点和连接两个所述节点构成的边，所述节点用于表示待求解的线性方程的待优化变量，所述边用于表示所连接的两个节点对应的待优化变量之间的约束关系，其中，所述待优化变量包括待求解的三维点坐标和待求解的相机运动参数，所述节点包括第一节点和第二节点，所述第一节点用于表示所述待求解的三维点坐标，所述第二节点用于表示所述待求解的相机运动参数；

节点复制单元，用于将预先构建的所述SLAM的待优化图中的所述第一节点复制成两个以上第三节点，并使所述第三节点中的任意一个与n个第二约束节点存在约束关系且在n个所述第二约束节点中有m个所述第二约束节点与相邻的所述第三节点存在相同的约束关系，所述第二约束节点与至少一个所述第三节点存在约束关系，其中，n为预设的控制参数，n-1≥m≥1,所述第二约束节点是与所述第一节点存在约束关系的所述第二节点；

图优化单元，用于用所述第三节点代替对应的预先构建的所述SLAM的待优化图中的所述第一节点，得到复制节点后的SLAM的待优化图；以及

边缘化单元，用于边缘化所述第三节点，以减小所述待求解的线性方程的系数矩阵的稠密程度。

7.根据权利要求6所述的装置，其特征在于，还包括：所述第三节点包括：与n个连续帧中的所述待求解的相机运动参数存在约束关系的节点，其中，n为所述预设的控制参数。

8.根据权利要求6所述的装置，其特征在于，所述图优化单元还用于：

设置所述第三节点的每条边的权重，以使所述复制节点后的SLAM的待优化图与预先构建的所述SLAM的待优化图的能量值相同。

9.根据权利要求6-8中任一项所述的装置，其特征在于，还包括方程求解单元，用于：

求解边缘化后的线性方程，得到所述相机运动参数；

将所述相机运动参数代回预先构建的所述SLAM的待优化图，求解预先构建的所述SLAM的待优化图中的所述三维点坐标。

10.根据权利要求9所述的装置，其特征在于，所述方程求解单元还用于：用预条件共轭梯度法求解所述边缘化后的线性方程。

11.一种电子设备，其特征在于，所述电子设备具有SLAM功能，包括：

一个或多个处理器；

存储装置，用于存储一个或多个程序；

当所述一个或多个程序被所述一个或多个处理器执行时，使得所述一个或多个处理器实现如权利要求1-5中任一所述的方法。

12.一种计算机可读存储介质，其存储有计算机程序，其特征在于，该程序被处理器执行时实现如权利要求1-5中任一所述的方法。