CN111062966B - 基于l-m算法和多项式插值对相机跟踪进行优化的方法 - Google Patents

Abstract

本发明提供一种基于L‑M算法和多项式插值对相机跟踪进行优化的方法，涉及增强现实技术领域。该方法首先对目标图片进行检测识别，识别到目标图片后，移除目标图片，进入无标识式AR跟追踪状态，并创建AR场景地图；将AR场景地图的坐标系以及AR场景中的虚拟对象的坐标系进行虚实注册，获取虚拟对象的三维坐标与标准的二维成像平面之间的对应关系；移除识别的目标图片，完全进入ORB_SLAM2跟踪，利用列文伯格‑马夸尔特算法和多项式插值优化法对相机跟踪曲线进行平滑优化；根据优化后的相机跟踪曲线返回相机的外参，最终结合相机内参计算优化后的相机参数矩阵，进行虚实注册，保证AR场景中虚拟模型的稳定。

Description

基于L-M算法和多项式插值对相机跟踪进行优化的方法

技术领域

本发明涉及增强现实技术领域，尤其涉及一种基于L-M算法和多项式插值对相机跟踪进行优化的方法。

背景技术

增强现实是一种通过将虚拟模型准确叠加到现实场景中，达到虚实结合效果的技术。增强现实场景比传统的信息呈现方式更加直观，有着广泛的应用。SLAM(simultaneouslocalization and mapping，即时定位与地图构建)作为支撑增强现实的关键技术，用于在未知环境中确定自身位置并且同时构建周围环境三维地图，以此保证虚拟模型与真实场景的几何一致性。当存在识别图时，AR场景是稳定的，但是移除识别图后，AR场景中的虚拟物体将变得不稳定。虽然系统会帮助我们最小化误差，使相机参数满足最小重投影误差的约束，但是相邻帧对应的相机参数的突变和波动没有完全得到改善，这是导致虚拟场景抖动的原因。

非线性最小二乘是一种优化的手段，效果较好的非线性最小二乘方法是列文伯格-马夸尔特法，即L-M算法。列文伯格-马夸尔特法是对相机外参进行初步优化，缩小误差范围。多项式插值也是一种优化方法。传统的一步优化难以保证相机跟踪曲线的平滑，多种优化方法的结合使用会取得较好的效果。

发明内容

本发明要解决的技术问题是针对上述现有技术的不足，提供一种基于L-M算法和多项式插值对相机跟踪进行优化的方法，实现对相机跟踪进行优化。

为解决上述技术问题，本发明所采取的技术方案是：基于L-M算法和多项式插值对相机跟踪进行优化的方法，包括以下步骤：

步骤1：运行ORB_SLAM2程序，对目标图片进行检测识别，在没有识别到目标图像之前保持识别状态；

步骤2：识别到目标图片后，移除目标图片，进入无标识式AR跟追踪状态，跟踪的每一帧都绘制虚拟物体，同时调用ORB_SLAM2程序对AR场景进行初始化，即构建AR场景地图和保持对原目标图片的跟踪状态；

步骤3：在跟踪状态下，如果跟踪失败重新执行步骤1进行目标检测，否则检测成功后重新执行步骤2，继续调用ORB_SLAM2对AR场景进行初始化，直到AR场景地图创建成功为止；

步骤4：构建AR场景地图成功的同时，将AR场景地图的坐标系以及AR场景中的虚拟对象的坐标系进行虚实注册，获取虚拟对象的三维坐标与标准的二维成像平面之间的对应关系；具体涉及到AR场景中虚拟对象的坐标系、世界坐标系、相机坐标系和二维成像平面坐标系之间的转换；设定虚拟对象坐标系坐标为(X_v，Y_v，Z_v)，世界坐标系坐标为(X_w，Y_w，Z_w)，相机坐标系坐标为(X_c，Y_c，Z_c)；

所述世界坐标系到相机坐标系的转换，具体如下：

世界坐标系到相机坐标系的转换是为了确定相机与真实场景的相对位置和方向，转换公式如下所示：

其中，V_3x3是一个3x3的矩阵，反应相机相对于世界坐标系的旋转分量；W_3x1是一个3x1的矩阵，表示的是相机相对于世界坐标系的平移分量；

所述相机坐标系到二维成像平面坐标系的转换过程，具体如下：

首先对相机进行标定，获取相机内参；利用小孔成像原理，完成相机从三维坐标到二维平面坐标的转换；

根据针孔相机的成像原理，设相机坐标系中某点的坐标M(X_c，Y_c，Z_c)到二维平面上的投影为M(x_u，y_u)，有相似比例关系得：

将上述公式用齐次坐标和矩阵表示为：

其中，f_x、f_y、u₀、v₀均为相机的内部参数，f_x为焦距在平面坐标系x轴上的缩放结果，f_y为焦距在平面坐标系y轴上的缩放结果，u₀、v₀分别表示像素坐标系原点沿平面坐标系y、x方向平移的距离；

所述虚拟对象坐标系到世界坐标系的转换，具体如下：

虚拟对象坐标系到世界坐标系的转换，确定了虚拟物体在真实场景三维空间中的位置和方向，转换关系如下所示：

其中，dx和dy表示数字离散化后的像素在平面坐标系x轴和y轴方向上的尺度变换参数；

由此得AR场景中虚拟对象坐标到二维成像平面坐标的映射关系，如下：

其中，N为坐标变换矩阵，f为相机焦距；

步骤5：移除识别的目标图片，完全进入ORB_SLAM2跟踪，利用列文伯格-马夸尔特算法对相机跟踪曲线进行初步平滑优化；

所述利用列文伯格-马夸尔特算法对相机跟踪曲线进行初步平滑优化的具体方法为：

(1)首先取第一帧的相机外参x₀作为列文伯格-马夸尔特算法的初始值，给定初始优化半径μ和区域信赖范围ρ；

(2)对于第k次迭代，求解：

其中，μ_k是优化半径，D为非负数对角阵，f(x_k)是相机跟踪曲线关于相机位姿x_k的函数，J(x_k)是相机跟踪曲线关于相机位姿x_k的一阶导数，Δx_k表示相机位姿x_k的增量；

(3)根据公式计算第k次迭代时的区域信赖范围ρ_k；若则μ_k+1＝2μ_k；若/>则μ_k+1＝0.5μ_k，x_k+1＝x_k；若/>则μ_k+1＝μ_k，x_k+1＝x_k；如果ρ_k大于设定阀值，令x_k+1＝x_k+Δx_k；

(4)判断Δx_k是否小于10^-4，若否，则算法不收敛，重新执行(2)继续迭代，否则完成对相机跟踪曲线的初步平滑优化，执行步骤6；

步骤6：检测相机跟踪的连续五个帧之间的相机跟踪曲线波动是否大于cos0.9，若大于则采用多项式插值优化法对连续五个帧进行插值处理，完成对相机跟踪曲线的最终优化，否则相机跟踪曲线平滑，执行步骤7；

步骤7：根据优化后的相机跟踪曲线返回相机的外参，最终结合相机内参计算优化后的相机参数矩阵，进行虚实注册，保证AR场景中虚拟模型的稳定。

采用上述技术方案所产生的有益效果在于：本发明提供的基于L-M算法和多项式插值对相机跟踪进行优化的方法，利用列文伯格-马夸尔特法进行对相机跟踪曲线进行初步平滑优化，再采用多项式插值优化法对连续五个帧进行插值处理，完成对相机跟踪曲线的最终优化，使用列文伯格-马夸尔特法可在一定程度上避免线性方程组的系数矩阵的非奇异和病态问题为优化提供更稳定、更准确的增量；多项式插值法保证了局部平滑。用列文伯格-马夸尔特法结合多项式插值法对相机跟踪曲线进行优化，使虚拟模型能够稳定地出现在原来的地方，在室内静态场景中取得了较好的效果。

附图说明

图1为本发明实施例提供的基于L-M算法和多项式插值对相机跟踪进行优化的方法的框架图。

具体实施方式

下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。以下实施例用于说明本发明，但不用来限制本发明的范围。

本实施例中，基于L-M算法和多项式插值对相机跟踪进行优化的方法，如图1所示，包括以下步骤：

步骤1：运行ORB_SLAM2程序，对目标图片进行检测识别，在没有识别到目标图像之前保持识别状态；

步骤2：识别到目标图片后，移除目标图片，进入无标识式AR跟追踪状态，跟踪的每一帧都绘制虚拟物体，同时调用ORB_SLAM2程序对AR场景进行初始化，即构建AR场景地图和保持对原目标图片的跟踪状态；

步骤3：在跟踪状态下，如果跟踪失败重新执行步骤1进行目标检测，否则检测成功后重新执行步骤2，继续调用ORB_SLAM2对AR场景进行初始化，直到AR场景地图全部创建成功为止；

步骤4：构建AR场景地图成功的同时，将AR场景地图的坐标系以及AR场景中的虚拟对象的坐标系进行虚实注册，获取虚拟对象的三维坐标与标准的二维成像平面之间的对应关系；具体涉及到AR场景中虚拟对象的坐标系、世界坐标系、相机坐标系和二维成像平面坐标系之间的转换；设定虚拟对象坐标系坐标为(X_v，Y_v，Z_v)，世界坐标系坐标为(X_w，Y_w，Z_w)，相机坐标系坐标为(X_c，Y_c，Z_c)；

所述世界坐标系到相机坐标系的转换，具体如下：

世界坐标系到相机坐标系的转换是为了确定相机与真实场景的相对位置和方向，转换公式如下所示：

其中，V_3x3是一个3x3的矩阵，反应相机相对于世界坐标系的旋转分量；W_3x1是一个3x1的矩阵，表示的是相机相对于世界坐标系的平移分量；

所述相机坐标系到二维成像平面坐标系的转换过程，具体如下：

首先对相机进行标定，获取相机内参；利用小孔成像原理，完成相机从三维坐标到二维平面坐标的转换；

根据针孔相机的成像原理，设相机坐标系中某点的坐标M(X_c，Y_c，Z_c)到二维平面上的投影为M(x_u，y_u)，有相似比例关系得：

将上述公式用齐次坐标和矩阵表示为：

其中，f_x、f_y、u₀、v₀均为相机的内部参数，像素坐标系与成像平面之间差了一个缩放和一个原点的平移，f_x为焦距在平面坐标系x轴上的缩放结果，f_y为焦距在平面坐标系y轴上的缩放结果，u₀、v₀分别表示像素坐标系原点沿平面坐标系y、x方向平移的距离；

所述虚拟对象坐标系到世界坐标系的转换，具体如下：

虚拟对象坐标系到世界坐标系的转换，确定了虚拟物体在真实场景三维空间中的位置和方向，转换关系如下所示：

其中，dx和dy表示数字离散化后的像素在平面坐标系x轴和y轴方向上的尺度变换参数；

由此得AR场景中虚拟对象坐标到二维成像平面坐标的映射关系，如下：

其中，N为坐标变换矩阵，f为相机焦距；

步骤5：移除识别的目标图片，完全进入ORB_SLAM2跟踪，利用列文伯格-马夸尔特算法对相机跟踪曲线进行初步平滑优化；

所述利用列文伯格-马夸尔特算法对相机跟踪曲线进行初步平滑优化的具体方法为：

(1)首先取第一帧的相机外参x₀作为列文伯格-马夸尔特算法的初始值，给定初始优化半径μ和区域信赖范围ρ；

(2)对于第k次迭代，求解：

其中，μ_k是优化半径，D为非负数对角阵，对角阵上的元素取J^TJ的对角元素平方根，使得在梯度小的维度上约束范围更大一些；f(x_k)是相机跟踪曲线关于相机位姿x_k的函数，J(x_k)是相机跟踪曲线关于相机位姿x_k的一阶导数，Δx_k表示相机位姿x_k的增量；

(3)根据公式计算第k次迭代时的区域信赖范围ρ_k；若则μ_k+1＝2μ_k；若/>则μ_k+1＝0.5μ_k，x_k+1＝x_k；若/>则μ_k+1＝μ_k，x_k+1＝x_k；如果ρ_k大于设定阀值，令x_k+1＝x_k+Δx_k；

(4)判断Δx_k是否小于10^-4，若否，则算法不收敛，重新执行(2)继续迭代，否则完成对机跟踪曲线的初步平滑优化，执行步骤6；

步骤6：检测相机跟踪的连续五个帧之间的相机跟踪曲线波动是否大于cos0.9，若大于则采用多项式插值优化法对连续五个帧进行插值处理，完成对相机跟踪曲线的最终优化，否则相机跟踪曲线平滑，执行步骤7；

多项式插值法的具体方法为：

给定n+1个点{(x_i，y_i)}(i＝0，1，...，n)(称为插值点)，找到一个多项式：

y＝P(x)＝a_kx^k+a_k-1x^k-1+…+a₁x+a₀

使y_i＝P(x_i)经过给定的各点。

计算插值多项式的主要方法有：直接法、拉格朗日多项式插值和牛顿多项式插值。这三种方法得到的插值多项式在理论上是一致的，而且误差也相同。但是直接法计算过程简单、计算量较小，所以本实施例选择使用直接法求解。由插值条件y_i＝P(x_i)，得到关于系数a_n，a_n-1，…，a₁，a₀的线性方程组：

通过求解这个线性方程组，即得到插值多项式。

步骤7：根据优化后的相机跟踪曲线返回相机的外参，最终结合相机内参计算优化后的相机参数矩阵，进行虚实注册，保证AR场景中虚拟模型的稳定。

最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明权利要求所限定的范围。

Claims (2)

1.一种基于L-M算法和多项式插值对相机跟踪进行优化的方法，其特征在于：包括以下步骤：

步骤1：运行ORB_SLAM2程序，对目标图片进行检测识别，在没有识别到目标图像之前保持识别状态；

步骤2：识别到目标图片后，移除目标图片，进入无标识式AR跟追踪状态，跟踪的每一帧都绘制虚拟物体，同时调用ORB_SLAM2程序对AR场景进行初始化，即构建AR场景地图和保持对原目标图片的跟踪状态；

步骤3：在跟踪状态下，如果跟踪失败重新执行步骤1进行目标检测，否则检测成功后重新执行步骤2，继续调用ORB_SLAM2对AR场景进行初始化，直到AR场景地图创建成功为止；

步骤4：构建AR场景地图成功的同时，将AR场景地图的坐标系以及AR场景中的虚拟对象的坐标系进行虚实注册，获取虚拟对象的三维坐标与标准的二维成像平面之间的对应关系；具体涉及到AR场景中虚拟对象的坐标系、世界坐标系、相机坐标系和二维成像平面坐标系之间的相互转换；

步骤5：移除识别的目标图片，完全进入ORB_SLAM2跟踪，利用列文伯格-马夸尔特算法对相机跟踪曲线进行初步平滑优化；

步骤6：检测相机跟踪的连续五个帧之间的相机跟踪曲线波动是否大于cos0.9，若大于则采用多项式插值优化法对连续五个帧进行插值处理，完成对相机跟踪曲线的最终优化，否则相机跟踪曲线平滑，执行步骤7；

步骤7：根据优化后的相机跟踪曲线返回相机的外参，最终结合相机内参计算优化后的相机参数矩阵，进行虚实注册，保证AR场景中虚拟模型的稳定；

所述AR场景中虚拟对象的坐标系、世界坐标系、相机坐标系和二维成像平面坐标系之间的相互转换的具体方法为：

设定虚拟对象坐标系坐标为(X_v，Y_v，Z_v)，世界坐标系坐标为(X_w，Y_w，Z_w)，相机坐标系坐标为(X_c，Y_c，Z_c)；

所述世界坐标系到相机坐标系的转换，具体如下：

世界坐标系到相机坐标系的转换是为了确定相机与真实场景的相对位置和方向，转换公式如下所示：

其中，V_3x3是一个3x3的矩阵，反应相机相对于世界坐标系的旋转分量；W_3x1是一个3x1的矩阵，表示的是相机相对于世界坐标系的平移分量；

所述相机坐标系到二维成像平面坐标系的转换过程，具体如下：

首先对相机进行标定，获取相机内参；利用小孔成像原理，完成相机从三维坐标到二维平面坐标的转换；

根据针孔相机的成像原理，设相机坐标系中某点的坐标M(X_c,Y_c,Z_c)到二维平面上的投影为M(x_u,y_u)，有相似比例关系得：

将上述公式用齐次坐标和矩阵表示为：

其中，f_x、f_y、u₀、v₀均为相机的内部参数，f_x为焦距在平面坐标系x轴上的缩放结果，f_y为焦距在平面坐标系y轴上的缩放结果，u₀、v₀分别表示像素坐标系原点沿平面坐标系y、x方向平移的距离；

所述虚拟对象坐标系到世界坐标系的转换，具体如下：

虚拟对象坐标系到世界坐标系的转换，确定了虚拟物体在真实场景三维空间中的位置和方向，转换关系如下所示：

其中，dx和dy表示数字离散化后的像素在平面坐标系x轴和y轴方向上的尺度变换参数；

由此得AR场景中虚拟对象坐标到二维成像平面坐标的映射关系，如下：

其中，N为坐标变换矩阵，f为相机焦距。

2.根据权利要求1所述的基于L-M算法和多项式插值对相机跟踪进行优化的方法，其特征在于：步骤5所述利用列文伯格-马夸尔特算法对相机跟踪曲线进行初步平滑优化的具体方法为：

(1)首先取第一帧的相机外参x₀作为列文伯格-马夸尔特算法的初始值，给定初始优化半径μ和区域信赖范围ρ；

(2)对于第k次迭代，求解：

其中，μ_k是优化半径，D为非负数对角阵，f(x_k)是相机跟踪曲线关于相机位姿x_k的函数，J(x_k)是相机跟踪曲线关于相机位姿x_k的一阶导数，△x_k表示相机位姿x_k的增量；

(3)根据公式计算第k次迭代时的区域信赖范围ρ_k；若/>则μ_k+1＝2μ_k；若/>则μ_k+1＝0.5μ_k，x_k+1＝x_k；若/>则μ_k+1＝μ_k，x_k+1＝x_k；如果ρ_k大于设定阀值，令x_k+1＝x_k+△x_k；

(4)判断△x_k是否小于10^-4，若否，则算法不收敛，重新执行(2)继续迭代，否则完成对相机跟踪曲线的初步平滑优化，执行步骤6。