CN101464134A - 一种空间目标三维位姿视觉测量方法 - Google Patents

Abstract

本发明提供一种三维位姿计算采用绝对定向问题解算和景深估计两阶段迭代求解的空间目标三维位姿视觉测量方法。本发明采用在被测目标上设置特征光标点的视觉测量方式，包括相机标定、对目标成像、图像处理、特征点提取和匹配、三维位姿计算。三维位姿计算是一个基于逆投影线的包含绝对定向问题解算和景深估计两阶段的迭代过程：在绝对定向解算阶段采用绝对定向解算解析算法计算空间目标的相对位姿；在景深估计阶段利用前一阶段给出的相对位姿重构各特征点物空间坐标，并用其在逆投影线上的投影更新各特征点的景深。三维位姿计算采用两解析算法迭代进行的求解方式，具有精度高、收敛快、计算量小、适用范围广等优点。

Description

一种空间目标三维位姿视觉测量方法

(一)技术领域

本发明涉及测量技术，具体说就是一种空间目标三维位姿视觉测量方法。

(二)背景技术

三维位姿测量是指求解两个坐标系间的相对位姿关系，包括三个平移量和分别绕三个坐标轴的旋转量。基于点特征的单目视觉位姿测量方法通过单相机对目标成像，利用目标像中3个以上非共线特征点的像坐标求解目标与相机间的相对位置和相对姿态，由于其结构简单、易于实现等特点成为视觉测量领域研究热点之一，并广泛应用于汽车、机器人、飞机、航天器等领域的非接触式测量任务。

目前，基于点特征的单目视觉位姿测量算法大致可分为解析算法和迭代算法两类，解析算法在特征点数较少时能够推导出目标相对位姿参数的解析解，具有运算量小、计算速度快等优点，然而由于在实际应用成像过程中的测量误差、量化误差、特征点提取误差或特征点匹配错误等因素影响，使用解析算法可能会产生很大的误差，并且解析算法仅适用于特征点数小于6的情况。迭代算法是利用透视成像的基本原理，将相对位姿确定问题表示为受约束的非线性优化问题，通过求解该优化问题得到目标相对位姿的数值解，能够有效提高存在误差情况下的相对位姿参数测量精度。

DeMenthon等提出利用弱透视像机模型，基于特征点对应的迭代算法，当使用Newton递推算法时，迭代算法能够以超线性速度局部收敛。然而，这些数值方法的性能取决于初始值选取，且算法通常会收敛到局部最小值，或者收敛到一个错误解。为解决该问题Lee等推导了约束流形上的Gauss-Newton迭代算法，把相对位姿确定问题描述为一个在旋转矩阵流形和为确保正的深度参数所决定矩阵约束锥的交空间上三个旋转参数最优化问题，并且提出了在Gauss-Newton方向和随机方向之间变化的解析测地线搜索，以确保不重新初始化算法就能跳出局部最小，收敛至全局最小。然而上述基于优化方法的相对位姿测量算法是一个优化变量空间为N+6维(N为点特征数)的非线性问题，计算量较大，难以满足工业领域应用的实时性要求。

Haralick等提出了一种基于点特征同时计算目标位姿和特征点景深的位姿确定算法，该算法通过引入特征点景深变量消除了由于透视投影所产生位姿确定问题的非线性，且具有全局收敛性，然而该算法局部收敛速度较慢。

(三)发明内容

本发明的目的在于提供一种具有精度高、收敛快、计算量小、适用范围广、三维位姿计算过程采用两解析算法迭代进行求解的空间目标三维位姿视觉测量方法。

本发明的目的是这样实现的：采用在被测空间目标上设置特征光标点的视觉测量方式，包括视觉相机标定、对目标成像、图像处理、特征点提取和匹配、三维位姿计算。所述的三维位姿计算是一个基于逆投影线的包含绝对定向问题解算和景深估计两阶段的迭代过程，且绝对定向问题解算和景深估计两个阶段均为解析方式求解；其过程为：

(1)初始化各特征光标的景深值

i＝1，2，...，N，利用像点坐标计算相应的逆投影线单位矢量 ${\mathrm{\υ}}_i={\left(\sqrt{(X_i^2+Y_i^2+f^2)}\right)}^{-1}{\left(\begin{array}{ccc}{X}_i& Y_i& f\end{array}\right)}^i,$ 置迭代计数器k＝1；

(2)利用各特征点景深值

重构各特征点坐标 $S_i^{\′\left(k\right)}=d_i^{\left(k\right)}{\mathrm{\υ}}_i,$ 并采用Umeyama算法等解析算法求解绝对定位问题得到空间目标的三维位姿参数

(3)计算 $A_i=I-{\mathrm{\υ}}_i{\mathrm{\υ}}_i^l,$ 以及 $T^{(k+1)}=-{\left(\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{A}_i\right)}^{-1}\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{A}_i(M_{\mathrm{Ume}}^{(k+1)}{s}_i+t_{\mathrm{Ume}}^{(k+1)}),$ 修正步骤(2)给出三维位姿为 $M^{(k+1)}=M_{\mathrm{Ume}}^{(k+1)},$ $t^{(k+1)}=t_{\mathrm{Ume}}^{(k+1)}+T^{(k+1)};$

(4)更新景深值 $d_i^{(k+1)}={\mathrm{\υ}}_i^t(M^{(k+1)}{s}_i+t^{(k+1)});$

(5)计算误差

$e^2(M^{(k+1)},t^{(k+1)},\left\{d_i^{(k+1)}\right\})=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\left|\right|d_i^{(k+1)}{\mathrm{\υ}}_i-(M^{(k+1)}{s}_i+t^{(k+1)})\left|\right|}^2;$

如果误差小于预定误差阈值E_T，即 $e^2(M^{(k+1)},t^{(k+1)},\left\{d_i^{(k+1)}\right\})<E_T,$

则进入第(7)步，否则，置k：＝k+1；

(6)若迭代次数少于预定的最大迭代次数，返回到第(2)步，否则进入第(7)步；

(7)迭代结束，输出结果。

本发明一种空间目标三维位姿视觉测量方法，所述的三维位姿计算采用两解析算法迭代进行的求解方式，具有精度高、收敛快、计算量小、适用范围广等优点，具体体现在：

(1)采用迭代方式求解空间目标三维位姿，有效降低了测量过程中各类误差的影响；

(2)迭代求解的两阶段都是解析方式求解，降低了计算复杂度，并具有良好的收敛性；

(3)本发明求解方法的推导过程中没有利用任何有关特征光标点的构型信息，因此能够适用于3个以上不同特征点数、各种非共线构型的测量特征光标的空间目标三维位姿测量。

(四)附图说明

图1为本发明的三维位姿计算方法流程框图；

图2为本发明所涉及到的坐标系，图中分别定义了相机参考坐标系、像平面坐标系和目标参考坐标系；

图3为本发明物空间内物像共线误差示意图；

图4为本发明实施例的测量特征光标点设置示意图；

图5为本发明实施例的各特征点景深运行结果曲线；

图6为本发明实施例的空间目标相对位置运行结果曲线；

图7为本发明实施例的空间目标相对姿态运行结果曲线。

(五)具体实施方式

下面结合附图举例对本发明作进一步说明。

实施例1，结合图1、图3，本发明空间目标三维位姿视觉测量方法是基于逆投影线思想，引入特征点的景深变量，将三维位姿计算过程分解为迭代进行的绝对定向解算和景深估计两阶段。在绝对定向解算阶段采用绝对定向解算解析算法计算空间目标的相对姿态旋转矩阵和相对平移矢量；在景深估计阶段利用绝对定向解算阶段给出的相对位姿重构各特征点物空间坐标，并用重构的特征点物空间坐标在逆投影线上的投影更新各特征点的景深。上述两阶段迭代进行直至结果收敛。

所述的三维位姿计算中，每个特征光标像点都对应一条从相机投影中心出发的射线，该射线穿过特征光标像点指向空间目标，由于该射线方向与特征光标点的投影线相反，故称为逆投影线，其单位矢量υ₁可表示为：

$\begin{array}{cc}{\mathrm{\&upsi;}}_i=\frac{1}{\sqrt{(X_i^2+Y_i^2+f^2)}}\left(\begin{array}{c}{X}_i\\ Y_i\\ f\end{array}\right)& i=\mathrm{1,2},...,N\end{array}---\left(1\right)$

理想情况下特征像点{S₁：(X₁，Y_i)}和特征点

在逆投影线υ₁上，称之为共线条件，因此特征点

在相机坐标系中可表示为：

$S_i^{\&prime;}=d_i{\mathrm{\&upsi;}}_i$ i＝1，2，...，N                   (2)

其中d₁为目标特征点

距离投影中心的景深。

共线条件也可表述为：特征点

在逆投影线V₁上的投影等于其自身，因此共线方程也可表示为：

$S_i^{\&prime;}={\mathrm{\&upsi;}}_i^t{S}_i^{\&prime;}{\mathrm{\&upsi;}}_i={\mathrm{\&upsi;}}_i^t({\mathrm{Ms}}_i+t){\mathrm{\&upsi;}}_i$ i＝1，2，...，N             (3)

对比式(2)和式(3)可知，目标特征点

的景深d₁可近似表示为：

$d_i={\mathrm{\&upsi;}}_i^t({\mathrm{Ms}}_i+t)---\left(4\right)$

由于图像处理过程中存在误差，目标特征点通常不在由特征像点所重构的逆投影线上，由式(2)表示的特征点坐标S₁与实际特征点坐标有一定误差，可以利用该误差定义目标函数：

$e^2(M,t,\left\{d_i\right\})=\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{\left|\right|d_i{\mathrm{\&upsi;}}_i-({\mathrm{Ms}}_i+t)\left|\right|}^2---\left(5\right)$

基于点特征的空间目标三维位姿视觉测量问题可描述为：给定特征光标点在目标坐标系中的物点坐标{(x_i，y_i，z_i)，i＝1，2，...，N}，及其相应的像点坐标{：(X_i，Y_i)，i＝1，2，...，N}，求解旋转矩阵M、平移矢量t和景深{d，i＝1，2，...，N}，使得式(5)表示的目标函数取最小值。

在引入逆投影线和景深变量后，空间目标三维位姿计算问题可分解为两个问题：其一，计算各特征点的景深{d_i，i＝1，2，...，N}，进而由式(2)重构各特征点的三维物空间坐标

；其二，利用特征点三维重构坐标和3维测量特征模型坐标s_i计算测量特征相对相机的相对姿态和相对位置，进而得到空间目标三位位姿。

上述两个问题的求解分别称为景深估计和绝对定向问题，因此，本发明给出了一种基于逆投影思想的包括景深估计和绝对定向解算两阶段的空间目标三维位姿计算方法，在景深恢复阶段计算各特征点的景深{d_i，i＝1，2，...，N}，在绝对定向解算阶段利用特征点三维重构坐标

和3维模型坐标s_i并基于绝对定向解算解析算法计算出空间目标三维位姿，上述两阶段迭代进行直至结果收敛。

1、绝对定向解算阶段

该阶段是利用特征点三维重构坐标

和三维模型坐标s_i求解两个三维模型所在坐标系之间的平移和旋转参数，即绝对定向结算问题。目前，针对该问题有多种解析算法，包括Faugeras和Horn分别提出的基于四元数的解析算法，Arun等提出的基于旋转矩阵特征值分解的解析算法，以及Umeyama在Arun算法基础上的修正算法等。由于Umeyama所提出的基于特征值分解的修正算法在具有测量误差时的鲁棒性，以及运算量小等优点，本发明选择该算法进行绝对方位解算，具体的Umeyama算法参见S.Umeyama等的论文“Least-squares estimation of transformation parameters between twopoint patterns”(S.Umeyama.Least-squares estimation of transformationparameters between two point patterns.IEEE Transactions on PatternAnalysis and Machine Intelligence.1991，13(4)：367-380)。

采用解析算法求解能够显著有效降低算法计算量，提高算法的运算速度和收敛速度。

2、景深估计阶段

在由Umeyama算法给出空间目标相对位置和相对姿态

之后，将式(4)表示的景深近似算法修正为：

$d_i^{(k+1)}={\mathrm{\&upsi;}}_i^t(M_{\mathrm{Ume}}^{(k+1)}{s}_i+t_{\mathrm{Ume}}^{(k+1)}+T^{(k+1)})---\left(6\right)$

其中，T^(k+1)为第k+1次迭代的平移矢量修正量。

在景深更新算法修正为式(6)后，式(5)表示的目标函数可表示为：

$e^2(M_{\mathrm{SVD}}^{(k+1)},t_{\mathrm{SVD}}^{(k+1)},\left\{d_i^{(k+1)}\right\},T^{(k+1)})=\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{\left|\right|d_i^{(k+1)}{\mathrm{\&upsi;}}_i-(M_{\mathrm{Ume}}^{(k+1)}{s}_i+t_{\mathrm{Ume}}^{(k+1)}+T^{(k+1)})\left|\right|}^2---\left(7\right)$

对式(7)相对T^(k+1)求偏导并令其为零得：

$\frac{{\&PartialD;e}^2(M_{\mathrm{Ume}}^{(k+1)},t_{\mathrm{Ume}}^{(k+1)},\left\{d_i^{(k+1)}\right\},T^{(k+1)})}{{\&PartialD;T}_{(k,k+1)}}=0---\left(8\right)$

$\&DoubleRightArrow;T^{(k+1)}=\frac{1}{N}\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}[d_i^{(k+1)}{\mathrm{\&upsi;}}_i-(M_{\mathrm{Ume}}^{(k+1)}{s}_i+t_{\mathrm{Ume}}^{(k+1)})]$

将式(6)代入式(8)得：

$T^{(k+1)}=\frac{1}{N}\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}[{\mathrm{\&upsi;}}_i^t(M_{\mathrm{Ume}}^{(k+1)}{s}_i+t_{\mathrm{Ume}}^{(k+1)}+T^{(k+1)}){\mathrm{\&upsi;}}_i-(M_{\mathrm{Ume}}^{(k+1)}{s}_i+t_{\mathrm{Ume}}^{(k+1)})]$

$=\frac{1}{N}\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}[{\mathrm{\&upsi;}}_i^t(M_{\mathrm{Ume}}^{(k+1)}{s}_i+t_{\mathrm{Ume}}^{(k+1)}){\mathrm{\&upsi;}}_i-(M_{\mathrm{Ume}}^{(k+1)}{s}_i+t_{\mathrm{Ume}}^{(k+1)})]+\frac{1}{N}\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{\mathrm{\&upsi;}}_i^t{T}^{(k+1)}{\mathrm{\&upsi;}}_i---\left(9\right)$

$=\frac{1}{N}\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}[\left({\mathrm{\&upsi;}}_i{\mathrm{\&upsi;}}_i^t\right)(M_{\mathrm{Ume}}^{(k+1)}{s}_i+t_{\mathrm{Ume}}^{(k+1)})-(M_{\mathrm{Ume}}^{(k+1)}{s}_i+t_{\mathrm{Ume}}^{(k+1)})]+\frac{1}{N}\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}\left({\mathrm{\&upsi;}}_i{\mathrm{\&upsi;}}_i^t\right)T^{(k+1)}$

由式(9)可得：

${(\mathrm{NI}-\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}\left({\mathrm{\&upsi;}}_i{\mathrm{\&upsi;}}_i^t\right))}^{\mathrm{}}{T}^{(k+1)}=\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}({\mathrm{\&upsi;}}_i{\mathrm{\&upsi;}}_i^t-I)(M_{\mathrm{Ume}}^{(k+1)}{s}_i+t_{\mathrm{Ume}}^{(k+1)})---\left(10\right)$

其中I为3阶单位方阵。

由于

是正定矩阵，其逆存在，由式(10)可得平移矢量增量T^(k+1)为：

$T^{(k+1)}=-{\left(\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}(I-{\mathrm{\&upsi;}}_i{\mathrm{\&upsi;}}_i^t)\right)}^{-1}\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}(I-{\mathrm{\&upsi;}}_i{\mathrm{\&upsi;}}_i^t)(M_{\mathrm{Ume}}^{(k+1)}{s}_i+t_{\mathrm{Ume}}^{(k+1)})---\left(11\right)$

第k+1次迭代的相对位姿参数M^(k+1)，t^(k+1)为：

$M^{(k+1)}=M_{\mathrm{Ume}}^{(k+1)}---\left(12\right)$

$t^{(k+1)}=t_{\mathrm{Ume}}^{(k+1)}+T^{(k+1)}$

式(6)给出的景深更新算法变为：

$d_i^{(k+1)}={\mathrm{\&upsi;}}_i^t(M^{(k+1)}{s}_i+t^{(k+1)})---\left(13\right)$

进行上述修正后能够有效利用特征点物空间坐标的第k+1次迭代值，由其在逆投影线上的投影长度精确近似第k+1次景深。

理论证明本发明所提出的算法具有全局收敛性，该方法流程框图如图1所示。

实施例2，本发明空间目标三维位姿视觉测量方法，还具有以下技术特征：

(1)所述的绝对定向解算算法是一种绝对定向问题求解的解析算法，如Umeyama解析算法等；

(2)所述的绝对定向解算阶段还包括对Umeyama解析算法给出的三维位姿的修正，即 $M^{(k+1)}=M_{\mathrm{Ume}}^{(k+1)},$ $t^{(k+1)}=t_{\mathrm{Ume}}^{(k+1)}+T^{(k+1)};$

其中：

$T^{(k+1)}=-{\left(\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}(I-{\mathrm{\&upsi;}}_i{\mathrm{\&upsi;}}_i^t)\right)}^{-1}\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}(I-{\mathrm{\&upsi;}}_i{\mathrm{\&upsi;}}_i^t)(M_{\mathrm{Ume}}^{(k+1)}{s}_i+t_{\mathrm{Ume}}^{(k+1)})$

本发明的三维位姿测量方法包含如下步骤：

(1)初始化各特征光标的景深值

i＝1，2，...，N，利用像点坐标计算相应的逆投影线单位矢量 ${\mathrm{\&upsi;}}_i={\left(\sqrt{(X_i^2+Y_i^2+f^2)}\right)}^{-1}{\left(\begin{array}{ccc}{X}_i& Y_i& f\end{array}\right)}^i,$ 置迭代计数器k＝1；

(2)利用各特征点景深值重构各特征点坐标 $S_i^{\&prime;\left(k\right)}=d_i^{\left(k\right)}{\mathrm{\&upsi;}}_i,$ 并采用Umeyama解析算法求解绝对定位问题得到空间目标的三维位姿参数

(3)计算 $A_i=I-{\mathrm{\&upsi;}}_i{\mathrm{\&upsi;}}_i^l,$ 并计算 $T^{(k+1)}=-{\left(\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{A}_i\right)}^{-1}\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{A}_i(M_{\mathrm{Ume}}^{(k+1)}{s}_i+t_{\mathrm{Ume}}^{(k+1)}),$ 修正步骤(2)给出三维位姿为 $M^{(k+1)}=M_{\mathrm{Ume}}^{(k+1)},t^{(k+1)}=t_{\mathrm{Ume}}^{(k+1)}+T^{(k+1)};$

(4)更新景深值 $d_i^{(k+1)}={\mathrm{\&upsi;}}_i^t(M^{(k+1)}{s}_i+t^{(k+1)});$

(5)计算误差

$e^2(M^{(k+1)},t^{(k+1)},\left\{d_i^{(k+1)}\right\})=\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{\left|\right|d_i^{(k+1)}{\mathrm{\&upsi;}}_i-(M^{(k+1)}{s}_i+t^{(k+1)})\left|\right|}^2;$

如果误差小于预定误差阈值ET，即 $e^2(M^{(k+1)},t^{(k+1)},\left\{d_i^{(k+1)}\right\})<E_T,$ 则进入第(7)步，否则，置k：＝k+1；

(6)若迭代次数少于预定的最大迭代次数，返回到第(2)步，否则进入第(7)步；

(7)迭代结束，输出结果。

本发明的三维位姿计算采用两解析算法迭代进行的求解方式，具有精度高、收敛快、计算量小、适用范围广等优点，具体体现在：

(1)采用迭代方式求解空间目标三维位姿，有效降低了测量过程中各类误差的影响；

(2)迭代求解的两阶段都是解析求解，降低了计算复杂度，并具有良好的收敛性；

(3)本发明求解方法的推导过程中没有利用任何有关特征光标点的构型信息，因此能够适用于3个以上不同特征点数、各种非共线构型的测量特征光标的空间目标三维位姿测量。

实施例3，结合图4，根据本发明提出的空间目标三维位姿视觉测量方法，建立了实际测量系统，光标点设置如图4所示，光标点s_i：(x_i，y_i，z_i)^T，i＝1，...，4在目标坐标系中的坐标分别为：

$s_1:\left[\begin{array}{c}{x}_1\\ y_1\\ z_1\end{array}\right]=\left[\begin{array}{c}0\\ -0.2\\ 0\end{array}\right],$ $s_2:\left[\begin{array}{c}{x}_2\\ y_2\\ z_2\end{array}\right]=\left[\begin{array}{c}-0.2\\ 0\\ 0\end{array}\right],$ $s_3:\left[\begin{array}{c}{x}_3\\ y_3\\ z_3\end{array}\right]=\left[\begin{array}{c}0.2\\ 0\\ 0\end{array}\right],$ $s_4:\left[\begin{array}{c}{x}_4\\ y_4\\ z_4\end{array}\right]=\left[\begin{array}{c}0\\ 0.2\\ 0\end{array}\right]$

空间目标相对测量相机的相对姿态为[φ，θ，Ψ]＝[10°，15°，8°]，相对位置为t＝[4，-1，6]ⁱ；相机焦距：0.051m；像平面大小：19.2mm×14.4mm；像平面分辨率：1600×1200；像元大小：12μm×12μm；特征点提取误差：1像素(3σ)。利用本发明的运行结果如图5-图7所示，从图中可以看出在较大初值偏差情况下，所提出的三维位姿计算方法在30次迭代内能够收敛到正确值，验证了其全局收敛性。相对姿态角测量精度优于0.5度，相对位置测量精度优于2cm，能够满足实际应用的测量精度要求。

Claims (2)

1.一种空间目标三维位姿视觉测量方法，基于单目视觉的空间目标三维位姿测量方法，采用在被测空间目标上设置特征光标点的视觉测量方式，包括视觉相机标定、对目标成像、图像处理、特征点提取和匹配、三维位姿计算过程，其特征在于：所述的三维位姿计算是一个基于逆投影线的包含绝对定向问题解算和景深估计两阶段的迭代过程。

2.根据权利要求1所述的三维位姿计算，其特征在于：绝对定向问题解算和景深估计两个阶段均为解析方式求解；其过程为：

(1)初始化各特征光标的景深值

利用像点坐标计算相应的逆投影线单位矢量 $v_t={\left(\sqrt{(X_i^2+Y_i^2+f^2)}\right)}^{-1}{\left(\begin{array}{ccc}{X}_i& Y_i& f\end{array}\right)}^t,$ 置迭代计数器k＝1；

(2)利用各特征点景深值，重构各特征点坐标

并采用Umeyama解析算法求解绝对定位问题得到空间目标的三维位姿参数

(3)计算 $A_i=I-v_i{v}_i^t,$ 以及 $T^{(k+1)}=-{\left(\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{A}_i\right)}^{-1}\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{A}_i(M_{\mathrm{Ume}}^{(k+1)}{s}_i+t_{\mathrm{Ume}}^{(k+1)}),$ 修正步骤(2)给出三维位姿为 $M^{(k+1)}=M_{\mathrm{Ume}}^{(k+1)},t^{(k+1)}=t_{\mathrm{Ume}}^{(k+1)}+T^{(k+1)};$

(4)更新景深值 $d_i^{(k+1)}=v_i^t(M^{(k+1)}{s}_i+t^{(k+1)});$

(5)计算误差

$e^2(M^{(k+1)},t^{(k+1)},\left\{d_i^{(k+1)}\right\})=\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{\left|\right|d_i^{(k+1)}{v}_i-(M^{(k+1)}{s}_i+t^{(k+1)})\left|\right|}^2;$

如果误差小于预定误差阈值ET，即 $e^2(M^{(k+1)},t^{(k+1)},\left\{d_i^{(k+1)}\right\})<E_T,$

则进入第(7)步，否则，置k：＝k+1；

(6)若迭代次数少于预定的最大迭代次数，返回到第(2)步，否则进入第(7)步；

(7)迭代结束，输出结果。

Images