CN113506337B - 一种基于EPnP的双目相机位姿估计方法 - Google Patents

Abstract

本发明公开了一种基于EPnP的双目相机位姿估计方法，该方法步骤如下：计算双目相机的3D点在世界坐标系下的三维坐标和测量误差；利用已知的3D点，基于主成分分析原理选择世界坐标系内四个3D控制点；据此求解世界坐标系下每个3D点对应的hb坐标；求解四个3D控制点在相机坐标系下的坐标；根据控制点，将所有的3D点在相机坐标系下的坐标恢复出来；根据世界坐标系下每个3D点坐标和相机坐标系下的3D点坐标，计算相机坐标系相对世界坐标系的旋转分量和平移分量，得到相机位姿。本发明增强了双目相机下位姿估计的鲁棒性，提高了相对位姿估计的准确性，实现了双目相机***下，高效、精准的相机位姿估计。

Description

一种基于EPnP的双目相机位姿估计方法

技术领域

本发明属于机器人视觉技术领域，特别是一种基于EPnP的双目相机位姿估计方法。

背景技术

在一个机器人视觉***中，机器人实现自主导航定位***的关键在于获取相机在位姿参数。该问题可以通过求解n点透视问题(PnP)获得。PnP能在已知多对3D世界坐标与2D像素坐标匹配点的情况下，求解相机坐标系相对世界坐标系的旋转变量R和平移分量t。

长期以来，PnP算法及其改进算法在图像测、计算机视觉、机器人学、AR等领域都获得了研究关注。目前，大多数非迭代的PnP算法会首先求解特征点的深度，以获得特征点在相机坐标系中的3D坐标，如P3P算法，但是此类算法在求解过程中存在无解和无穷多解的情况。

发明内容

本发明的目的在于提供一种鲁棒性好、准确性高的基于EPnP的双目相机位姿估计方法。

实现本发明目的的技术解决方案为：一种基于EPnP的双目相机位姿估计方法，包括以下步骤：

步骤1、计算双目相机的3D点在世界坐标系下的三维坐标和测量误差；

步骤2、利用已知的3D点，基于主成分分析PCA原理选择世界坐标系内四个3D控制点C_wj，j＝1，...，4；

步骤3、根据步骤2得到的四个3D控制点，求解世界坐标系下每个3D点P_wi对应的hb坐标；

步骤4、求解四个3D控制点在相机坐标系下的坐标C_cj；

步骤5、根据控制点C_cj，将所有的3D点在相机坐标系下的坐标P_ci恢复出来；

步骤6、根据P_wi和P_ci，计算相机坐标系相对世界坐标系的旋转分量R和平移分量t，得到相机位姿。

进一步地，步骤1中双目相机的3D点是通过上一帧图像F_k-1和当前帧图像F_k的左相机进行特征提取和匹配获得的，k为当前图像编号，且k为大于等于2的自然数；

其中，当前帧图像F_k中左相机中的n个点的相机坐标为p_ci＝[u_ci v_ci]^T，和上一帧对应的n个3D点在世界坐标系下的三维坐标为P_wi＝[X_wi Y_wi Z_wi]^T，i＝1...n，其中n为上一帧图像F_k-1和当前帧图像F_k的左相机进行特征提取和匹配获得的3D点的数量。

进一步地，步骤1所述的世界坐标系下的三维坐标和测量误差，假设左右相机坐标系的x轴在同一条直线上，根据相似三角形原理，计算3D点的深度d

其中Δx表示视差，x_cl、x_cr分别为3D点在左相机和右相机成像平面上的投影位置的横坐标值，b为双目相机基线长度；

当使用视差法计算3D点三维坐标，由像素化误差导致视差产生误差，则带有误差的深度为

其中，为带有误差的视差，δΔx为视差误差，δd为深度误差；

继而推得：

其中，δx_cl、δx_cr分别为左右两相机的数字图像像素化误差；δΔx远小于Δx，则δd和δx同分布，δd的期望和Δx成反比；

在实际情况中，3D点在左相机和右相机成像平面上的投影位置的纵坐标值y_cl、y_cr不同；因此，能够得到带有误差的3D点相机坐标系坐标

其中，P、分别为3D点相机坐标系在相机坐标系下的坐标真值和测量值，δP_c为观测误差，p_cl为左相机坐标中3D点P在左相机平面的投影坐标；

因此，双目相机对3D点的观测误差δP_c为：

假设|δx_cl|≤δ_x，max，|δy_cl|≤δ_y，max，得到

其中δ_x，max和δ_y，max是p_c在x轴和y轴的最大误差，取δ_x，max＝0.5/f_x，δ_y，max＝0.5/f_y；δX_c、δY_c、δZ_c是3D点对应的P_c在x轴、y轴、z轴的误差；δX_c，max、δY_c，max、δZ_c，max是3D点对应的P_c在x轴、y轴、z轴的最大误差；

定义权重向量K_δc＝[k_δc1 k_δc1 k_δc1]^T＝[1/δX_c，max 1/δY_c，max 1/δZ_c，max]^T，K_δc用来描述对应3D点相机坐标三个维度的可靠度。

进一步地，步骤2中基于主成分分析PCA原理选择控制点的过程如下：

步骤2.1、设置控制点C_w1，第一个控制点C_w1选择在n个3D点的重心位

其中，K_δwi≈R_wK_δc为P_wi对应的权重向量，R_w为世界坐标系下相机的位姿的旋转分量，X_w1、Y_w1、Z_w1分别为加权和的X、Y、Z轴分量；

步骤2.2、将P_wi点集进行中心化处理，得到A

步骤2.3、计算其余控制点：计算A^TA的3个特征点λ₁、λ₂、λ₃，对应特征向量是v₁、v₂、v₃，则其余的控制点为

其中，C_wj，j＝2，...，4为求解得到的其余控制点，n＝4。

进一步地，步骤3中求解世界坐标系下每个3D点P_wi对应的hb坐标，对于世界坐标系内四个3D控制点C_wj，j＝1...n，使得所有的P_wi都能够用C_wj线性表示，即：

其中称a_ij为hb坐标，求解每个P_wi对应的hb坐标a_ij，j＝1...4。

进一步地，步骤3中四个3D控制点C_wj，j＝1...n，当相机位姿为四个3D控制点C_wj在相机坐标系下的坐标C_cj＝R^T·(C_wj-t)，3D点在相机坐标系下的三维坐标P_ci为

其中，R、t分别为相机位姿T的旋转分量和水平分量，a_ij为每个P_wi对应的hb坐标。

进一步地，步骤4中求解控制点在相机坐标系下的坐标C_cj，具体步骤如下：

步骤4.1、因为已知所有3D点匹配的相机平面的p_ci＝[u_ci v_ci]^T，i＝1...n，则列出一组线性方程组

将上式方程组概括为

Mx＝0

其中，M为齐次方程组的系数矩阵，M的大小为2n×12；

方程组的解为

其中v_i为M的0奇异值对应的右奇异向量，β_i为v_i对于的系数；x＝[C_c1 C_c2 C_c3 C_c4]^T，包含4个控制点在相机坐标系中的三维坐标，共12维；N表示前N个零奇异值，在此取1≤N≤3；

步骤4.2、若相机坐标系中的控制点坐标和世界坐标系中的控制点坐标一一对应，则两两的距离一定相等，于是有

||C_wa-C_wb||²＝||C_ca-C_cb||²

其中，||C_wa-C_wb||表示第a和第b个控制点在世界坐标系下的距离，||C_ca-C_cb||表示第a和第b个控制点在空间坐标系下的距离，a，b∈[1∶4]且a≠b；分别取N为1或2或3：

当N＝1，x＝βv，转化为||βv_a-βv_b||²＝||C_ca-C_cb||²，则

当N＝2，x＝β₁v₁+β₂v₂，转化为

||β₁(v_1a-v_1b)+β₂(v_2a-v_2b)||²＝||C_ca-C_cb||²

通过解该方程组来求出β₁和β₂；其中，设s₁＝v_1a-v_1b，s₂＝v_2a-v_2b，c＝||C_ca-C_cb||²；

N＝3时β₁、β₂和β₃的解法同N＝2；

步骤4.3、以步骤4.2求出的N为1、2和3时β的近似解为初值，通过高斯牛顿法，求解β值。

进一步地，步骤5中根据控制点C_cj，将所有的3D点在相机坐标系下的坐标P_ci恢复出来，使用步骤3中求解出的hb坐标，计算出P_ci：

其中，a_ij为每个P_wi对应的hb坐标，C_cj为四个3D控制点C_wj在相机坐标系下的坐标。

进一步地，步骤6中根据P_wi和P_ci，计算R和t，具体步骤如下：

步骤6.1、对所有P_wi和P_ci进行去中心化得到Q_w、Q_c：

其中

步骤6.2、对做奇异值分解，得到

W^TW＝U∑V^T

R＝UV^T

其中，U的列向量是矩阵W^TW的特征向量，V的列向量是矩阵WW^T的特征向量；

步骤6.3、重复步骤6.1、6.2，分别对N为1、2和3三种情况计算R和t；用重投影误差来评估R和t，取误差最小的R和t作为最终的结果。

本发明与现有技术相比，其显著优点为：

(1)针对双目相机在测量过程中误差的不确定性，提出了针对双目相机测量模型的改进EPnP算法，能够在已知前后两帧双目图像、上一帧通过双目相机三角测量获得的3D点坐标以及该3D点在当前帧图像上的投影坐标，计算出当前帧相对上一帧的相对位姿，提高了EPnP算法在双目相机***的鲁棒性；

(2)在EPnP算法的基础上，引入了误差模型，将世界坐标系中的3D坐标表示为一组虚拟的控制点的加权和，由于四个控制点不能共面，故而可以求解出唯一的相机坐标系下的坐标，提高了位姿估计结果的准确性；

(3)能够快速计算出前后两帧图像之间相机的位姿变换，实现了双目相机***下，高效、精准的相机位姿估计，为机器人自主定位导航技术提供了有力保障；

附图说明

图1为本发明基于EPnP的双目相机位姿估计方法的流程图。

图2为本发明中的双目视差模型示意图。

具体实施方式

如图1、2所示，一种基于EPnP的双目相机位姿估计方法，包括如下步骤：

步骤1、计算双目相机的3D点在世界坐标系下的三维坐标和测量误差；

所述双目相机的3D点是通过上一帧图像F_k-1和当前帧图像F_k的左相机进行特征提取和匹配获得的，k为当前图像编号，且k为大于等于2的自然数；其中，当前帧图像F_k中左相机中的n个点的相机坐标为p_ci＝[u_ci v_ci]^T，和上一帧对应的n个3D点在世界坐标系下的三维坐标为P_wi＝[X_wi Y_wi Z_wi]^T，i＝1...n，其中n为上一帧图像F_k-1和当前帧图像F_k的左相机进行特征提取和匹配获得的3D点的数量。

所述的世界坐标系下的三维坐标和测量误差，假设左右相机坐标系的x轴在同一条直线上，根据相似三角形原理，计算3D点的深度

其中Δx表示视差，x_cl、x_cr分别为3D点在左相机和右相机成像平面上的投影位置的横坐标值，b为双目相机基线长度；

当使用视差法计算3D点三维坐标，由像素化误差导致视差产生误差，则带有误差的深度为

其中，为带有误差的视差，δΔx为视差误差，δd为深度误差；

继而推得：

其中，δx_cl、δx_cr分别为左右两相机的数字图像像素化误差；δΔx远小于Δx，则δd和δx同分布，δd的期望和Δx成反比；

在实际情况中，3D点在左相机和右相机成像平面上的投影位置的纵坐标值y_cl、y_cr不同；因此，能够得到带有误差的3D点相机坐标系坐标

其中，P、分别为3D点相机坐标系在相机坐标系下的坐标真值和测量值，δP_c为观测误差，p_cl为左相机坐标中3D点P在左相机平面的投影坐标；

因此，双目相机对3D点的观测误差δP_c为：

假设|δx_cl|≤δ_x，max，|δy_cl|≤δ_y，max，得到

其中δ_x，max和δ_y，max是p_c在x轴和y轴的最大误差，取δ_x，max＝0.5/f_x，δ_y，max＝0.5/f_y；δX_c、δY_c、δZ_c是3D点对应的P_c在x轴、y轴、z轴的误差；δX_c，max、δY_c，max、δZ_c，max是3D点对应的P_c在x轴、y轴、z轴的最大误差；

通过分析δP_c可知每个3D点坐标的误差不同，这样不利于求解相机位姿。为解决这一问题，结合以上分析，定义权重向量K_δc＝[k_δc1 k_δc1 k_δc1]^T＝[1/δX_c，max 1/δY_c，max1/δZ_c，max]^T，K_δc用来描述对应3D点相机坐标三个维度的可靠度。

步骤2、利用已知的3D点，基于主成分分析PCA原理选择世界坐标系内四个3D控制点C_wj，j＝1，...，4，具体如下：

步骤2.1、设置控制点C_w1，第一个控制点C_w1选择在n个3D点的重心位

其中，K_δwi≈R_wK_δc为P_wi对应的权重向量，R_w为世界坐标系下相机的位姿的旋转分量，X_w1、Y_w1、Z_w1分别为加权和的X、Y、Z轴分量；

步骤2.2、将P_wi点集进行中心化处理，得到A

步骤2.3、计算其余控制点：计算A^TA的3个特征点λ₁、λ₂、λ₃，对应特征向量是v₁、v₂、v₃，则其余的控制点为

其中，C_wj，j＝2，...，4为求解得到的其余控制点，n＝4。

步骤3、根据步骤2得到的四个3D控制点，求解世界坐标系下每个3D点P_wi对应的hb坐标。

步骤3中求解世界坐标系下每个3D点P_wi对应的hb坐标，对于世界坐标系内四个3D控制点C_wj，j＝1...n，使得所有的P_wi都能够用C_wj线性表示，即：

其中称a_ij为hb坐标(homogeneous barycentric coordinates)，代入上式，获得每个P_wi对应的a_ij，j＝1...4；

优选地，四个3D控制点C_wj，j＝1...n，当相机位姿为四个3D控制点C_wj在相机坐标系下的坐标C_cj＝R^T·(C_wj-t)，3D点在相机坐标系下的三维坐标P_ci为

其中，R、t分别为相机位姿T的旋转分量和水平分量，a_ij为每个P_wi对应的hb坐标。

步骤4、求解四个3D控制点在相机坐标系下的坐标C_cj，具体步骤如下：

步骤4.1、因为已知所有3D点匹配的相机平面的p_ci＝[u_ci v_ci]^T，i＝1...n，则列出一组线性方程组

将上式方程组概括为

Mx＝0

其中，M为齐次方程组的系数矩阵，M的大小为2n×12；

方程组的解为

其中v_i为M的0奇异值对应的右奇异向量，β_i为v_i对于的系数；x＝[C_c1 C_c2 C_c3 C_c4]^T，包含4个控制点在相机坐标系中的三维坐标，共12维；N表示前N个零奇异值，在此取1≤N≤3；

步骤4.2、若相机坐标系中的控制点坐标和世界坐标系中的控制点坐标一一对应，则两两的距离一定相等，于是有

||C_wa-C_wb||²＝||C_ca-C_cb||²

其中，||C_wa-C_wb||表示第a和第b个控制点在世界坐标系下的距离，||C_ca-C_cb||表示第a和第b个控制点在空间坐标系下的距离，a，b∈[1∶4]且a≠b；分别取N为1或2或3：

当N＝1，x＝βv，转化为||βv_a-βv_b||²＝||C_ca-C_cb||²，则

当N＝2，x＝β₁v₁+β₂v₂，转化为

||β₁(v_1a-v_1b)+β₂(v_2a-v_2b)||²＝||C_ca-C_cb||²

通过解该方程组来求出β₁和β₂；其中，设s₁＝v_1a-v_1b，s₂＝v_2a-v_2b，c＝||C_ca-C_cb||²；

N＝3时β₁、β₂和β₃的解法同N＝2；

步骤4.3、以步骤4.2求出的N为1、2和3时β的近似解为初值，通过高斯牛顿法，求解β值。

步骤5、根据控制点C_cj，将所有的3D点在相机坐标系下的坐标P_ci恢复出来。

假设当相机位姿为在相机坐标系内有C_cj＝R^T·(C_wj-t)，3D点在相机坐标系下的三维坐标为

因此世界坐标系下的hb坐标与其他相机坐标系下的hb坐标相同。使用步骤3中求解出的hb坐标，计算出P_ci：

其中，a_ij为每个P_wi对应的hb坐标，C_cj为四个3D控制点C_wj在相机坐标系下的坐标。

步骤6、根据P_wi和P_ci，计算相机坐标系相对世界坐标系的旋转分量R和平移分量t，得到相机位姿，具体步骤如下：

步骤6.1、对所有P_wi和P_ci进行去中心化得到Q_w、Q_c：

其中

步骤6.2、对做奇异值分解，得到

W^TW＝U∑V^T

R＝UV^T

其中，U的列向量是矩阵W^TW的特征向量，V的列向量是矩阵WW^T的特征向量；

步骤6.3、重复步骤6.1、6.2，分别对N为1、2和3三种情况计算R和t。用重投影误差来评估R和t，取误差最小的R和t作为改进EPnP的结果。

综上所述，本发明针对双目相机在测量过程中误差的不确定性，提出了针对双目相机测量模型的改进EPnP算法，提高了EPnP算法在双目相机***的鲁棒性；在EPnP算法的基础上，引入了误差模型，提高了位姿估计的准确性；能够快速计算出前后两帧图像之间相机的位姿变换，为机器人自主定位导航技术提供有力保障。

Claims (7)

1.一种基于EPnP的双目相机位姿估计方法，其特征在于，包括以下步骤：

步骤1、计算双目相机的3D点在世界坐标系下的三维坐标和测量误差；

步骤2、利用已知的3D点，基于主成分分析PCA原理选择世界坐标系内四个3D控制点C_wj，j＝1,...,4；

步骤3、根据步骤2得到的四个3D控制点，求解世界坐标系下每个3D点P_wi对应的hb坐标；

步骤4、求解四个3D控制点在相机坐标系下的坐标C_cj；

步骤5、根据控制点C_cj，将所有的3D点在相机坐标系下的坐标P_ci恢复出来；

步骤6、根据P_wi和P_ci，计算相机坐标系相对世界坐标系的旋转分量R和平移分量t，得到相机位姿；

步骤1中双目相机的3D点是通过上一帧图像F_k-1和当前帧图像F_k的左相机进行特征提取和匹配获得的，k为当前图像编号，且k为大于等于2的自然数；

其中，当前帧图像F_k中左相机中的n个点的相机坐标为p_ci＝[u_ci v_ci]^T，和上一帧对应的n个3D点在世界坐标系下的三维坐标为P_wi＝[X_wi Y_wi Z_wi]^T，i＝1…n，其中n为上一帧图像F_k-1和当前帧图像F_k的左相机进行特征提取和匹配获得的3D点的数量；

步骤1所述的世界坐标系下的三维坐标和测量误差，假设左右相机坐标系的x轴在同一条直线上，根据相似三角形原理，计算3D点的深度d

其中Δx表示视差，x_cl、x_cr分别为3D点在左相机和右相机成像平面上的投影位置的横坐标值，b为双目相机基线长度；

当使用视差法计算3D点三维坐标，由像素化误差导致视差产生误差，则带有误差的深度为

其中，为带有误差的视差，δΔx为视差误差，δd为深度误差；

继而推得：

其中，δx_cl、δx_cr分别为左右两相机的数字图像像素化误差；δΔx远小于Δx，则δd和δx同分布，δd的期望和Δx成反比；

在实际情况中，3D点在左相机和右相机成像平面上的投影位置的纵坐标值y_cl、y_cr不同；因此，能够得到带有误差的3D点相机坐标系坐标

其中，P、分别为3D点相机坐标系在相机坐标系下的坐标真值和测量值，δP_c为观测误差，p_cl为左相机坐标中3D点P在左相机平面的投影坐标；

因此，双目相机对3D点的观测误差δP_c为：

假设|δx_cl|≤δ_x,max，|δy_cl|≤δ_y,max，得到

其中δ_x,max和δ_y,max是P_c在x轴和y轴的最大误差，取δ_x,max＝0.5/f_x，δ_y,max＝0.5/f_y；δX_c、δY_c、δZ_c是3D点对应的P_c在x轴、y轴、z轴的误差；δX_c,max、δY_c,max、δZ_c,max是3D点对应的P_c在x轴、y轴、z轴的最大误差；

定义权重向量K_δc＝[k_δc1 k_δc1 k_δc1]^T＝[1/δX_c,max 1/δY_c,max 1/δZ_c,max]^T，K_δc用来描述对应3D点相机坐标三个维度的可靠度。

2.根据权利要求1所述的基于EPnP的双目相机位姿估计方法，其特征在于，步骤2中基于主成分分析PCA原理选择控制点的过程如下：

步骤2.1、设置控制点C_w1，第一个控制点C_w1选择在n个3D点的重心位

其中，K_δwi≈R_wK_δc为P_wi对应的权重向量，R_w为世界坐标系下相机的位姿的旋转分量，X_w1、Y_w1、Z_w1分别为加权和的X、Y、Z轴分量；

步骤2.2、将P_wi点集进行中心化处理，得到A

步骤2.3、计算其余控制点：计算A^TA的3个特征点λ₁、λ₂、λ₃，对应特征向量是v₁、v₂、v₃，则其余的控制点为

其中，C_wj，j＝2,...,4为求解得到的其余控制点，n＝4。

3.根据权利要求2所述的基于EPnP的双目相机位姿估计方法，其特征在于，步骤3中求解世界坐标系下每个3D点P_wi对应的hb坐标，对于世界坐标系内四个3D控制点C_wj，j＝1…n，使得所有的P_wi都能够用C_wj线性表示，即：

其中称a_ij为hb坐标，求解每个P_wi对应的hb坐标a_ij，j＝1…4。

4.根据权利要求3所述的基于EPnP的双目相机位姿估计方法，其特征在于，步骤3中四个3D控制点C_wj，j＝1…n，当相机位姿为四个3D控制点C_wj在相机坐标系下的坐标C_cj＝R^T·(C_wj-t)，3D点在相机坐标系下的三维坐标P_ci为

其中，R、t分别为相机位姿T的旋转分量和水平分量，a_ij为每个P_wi对应的hb坐标。

5.根据权利要求4所述的基于EPnP的双目相机位姿估计方法，其特征在于，步骤4中求解控制点在相机坐标系下的坐标C_cj，具体步骤如下：

步骤4.1、因为已知所有3D点匹配的相机平面的p_ci＝[u_ci v_ci]^T，i＝1…n，则列出一组线性方程组

将上式方程组概括为

Mx＝0

其中，M为齐次方程组的系数矩阵，M的大小为2n×12；

方程组的解为

其中v_i为M的0奇异值对应的右奇异向量，β_i为v_i对于的系数；x＝[C_c1 C_c2 C_c3 C_c4]^T，包含4个控制点在相机坐标系中的三维坐标，共12维；N表示前N个零奇异值，在此取1≤N≤3；

步骤4.2、若相机坐标系中的控制点坐标和世界坐标系中的控制点坐标一一对应，则两两的距离一定相等，于是有

‖C_wa-C_wb‖²＝‖C_ca-C_cb‖²

其中，‖C_wa-C_wb‖表示第a和第b个控制点在世界坐标系下的距离，‖C_ca-C_cb‖表示第a和第b个控制点在空间坐标系下的距离，a,b∈[1:4]且a≠b；分别取N为1或2或3：

当N＝1，x＝βv，转化为‖βv_a-βv_b‖²＝‖C_ca-C_cb‖²，则

当N＝2，x＝β₁v₁+β₂v₂，转化为

‖β₁(v_1a-v_1b)+β₂(v_2a-v_2b)‖²＝‖C_ca-C_cb‖²

通过解该方程组来求出β₁和β₂；其中，设s₁＝v_1a-v_1b，s₂＝v_2a-v_2b，c＝‖C_ca-C_cb‖²；

N＝3时β₁、β₂和β₃的解法同N＝2；

步骤4.3、以步骤4.2求出的N为1、2和3时β的近似解为初值，通过高斯牛顿法，求解β值。

6.根据权利要求5所述的基于EPnP的双目相机位姿估计方法，其特征在于，步骤5中根据控制点C_cj，将所有的3D点在相机坐标系下的坐标P_ci恢复出来，使用步骤3中求解出的hb坐标，计算出P_ci：

其中，a_ij为每个P_wi对应的hb坐标，C_cj为四个3D控制点C_wj在相机坐标系下的坐标。

7.根据权利要求6所述的基于EPnP的双目相机位姿估计方法，其特征在于，步骤6中根据P_wi和P_ci，计算R和t，具体步骤如下：

步骤6.1、对所有P_wi和P_ci进行去中心化得到Q_w、Q_c：

其中

步骤6.2、对做奇异值分解，得到

W^TW＝U∑V^T

R＝UV^T

其中，U的列向量是矩阵W^TW的特征向量，V的列向量是矩阵WW^T的特征向量；

步骤6.3、重复步骤6.1、6.2，分别对N为1、2和3三种情况计算R和t；用重投影误差来评估R和t，取误差最小的R和t作为最终的结果。