TOF相机是深度相机的一种，根据景深测量技术而得名，每曝光一次同时采集灰度图像（或RGB图像）和深度图像，深度图像可以根据透视投影模型直接变换为三维点云，Lange^[1]详细阐述了TOF相机的工作原理，经过数十年的发展，TOF相机的硬件技术与数据的预处理技术^[2]较为完善，但是应用软件及算法设计方面仍有较大的研究空间.

TOF相机以其独特的优势在诸多领域均有广泛的应用，大致分为3类：场景建模类应用^[3-4]、对象建模类应用^[5-6]和人机交互类应用^[7-8]. TOF相机视野有限，前述几种应用均需要根据相机位姿将不同位置采集的多帧数据拼接来生成完整的点云^[9]. 多帧拼接的实质是计算每帧数据对应的相机位姿，从2帧点云数据公共区域匹配三维点对，计算其间的相对位姿，应用迭代最近点算法（iterative closest point，ICP）^[10-12]循环“匹配-计算”迭代过程，提高相对位姿精度. 多帧数据的位姿可以通过连乘相对位姿得到，但是会导致位姿误差累积，从而降低数据拼接质量，为此，即时定位与地图构建（simultaneous localization and mapping，SLAM）问题的研究人员主要应用滤波器和平滑理论来设计抑制误差累积的算法. 常用的基于滤波的算法有扩展卡尔曼滤波^[13-14]和粒子滤波^[15]等方法，其最主要的问题在于线性化和更新效率，难以应用于场景较大的环境重建. 基于图的相机位姿优化算法^[16-17]是平滑类算法的典型代表，现在仍是三维环境重建等研究领域的热点，最初由Lu等^[16]提出并应用于二维空间中三自由度位姿的优化，后由Borrmann等^[17]扩展到三维空间. 将位姿优化问题抽象在图结构中，相机采集数据时的绝对位姿作为顶点，相机两位置之间的相对位姿作为边. 图结构位姿优化的实质是调整顶点以满足边的约束.

计算相对位姿及表征其精度的协方差矩阵是应用图结构位姿优化算法的关键，而从2帧点云中匹配三维点对（即帧间配准）是基础. 近年来，各国研究人员对帧间配准的关注度越来越高，涌现出很多优秀的科研成果. 比如，为解决闭合环路问题^[3]，可以将当前帧点云与前一帧点云在整个模型中的光线投影侧面点云进行配准，避免与前一帧点云直接配准导致的误差累积. 舒程珣等^[18]利用卷积神经网络进行点云配准，提高配准效率. 林桂潮等^[19]假设点云元素服从高斯混合分布，通过融合高斯混合模型和点到面距离来实现点云配准过程. 现有的绝大多数图结构优化算法根据给定的初始相对位姿，采用ICP算法迭代搜索最近点作为匹配点，数据帧中每一点均要参与匹配过程，匹配时间复杂度较高；另外直接从整个点云数据中搜索匹配点（距离最近的点）容易产生误匹配，使得位姿优化精度较低. 本文针对现有应用图结构算法对TOF相机位姿优化时存在计算效率低、优化精度低等问题，利用TOF相机的优势，设计快速准确配准帧间点云数据的算法. 充分利用TOF相机能够采集图像的优势，从中提取尺度不变特征点^[20]，匹配2幅图像中的特征点，并扩展到三维空间，得到三维特征对. 根据齐次变换不改变三维空间中线段长度的原理，位于特征点周围的三维点，其在另一帧点云中的匹配点一定位于另一个特征点周围. 匹配点的搜索范围缩小到以特征点为中心的球形区域内，假设横滚角变化较小（该假设在大多数应用中可以得到满足），搜索范围可以进一步缩小. 将上述帧间配准算法应用于基于图结构的相机位姿优化过程，不仅能够提高优化效率，还可避免三维点对误匹配，从而保证相机位姿优化精度.

本文内容安排如下：详细阐述基于图像的帧间点云数据配准算法；简要介绍基于图结构的相机位姿优化算法后，将提出的帧间配准算法融入到图结构算法中；设计综合实验验证帧间点云数据配准算法在相机位姿优化效果改善上的重要作用并得出结论.

对于2帧数据 ${{{D}}_i}\left( {{{{I}}_i},\;{{{P}}_i}} \right)$和 ${{{D}}_j}\left( {{{{I}}_j},\;{{{P}}_j}} \right)$来说，首先从图像 ${{{I}}_i}$和 ${{{I}}_j}$中提取并匹配^[20]尺度不变特征点对，滤除误匹配^[21]特征点对后，将特征对扩展到三维空间，滤除无有效三维点的特征对，获得原始的三维特征对. 如图1所示为二维及三维特征对匹配结果图，其中包括二维特征44对，滤除无效特征后，剩余三维特征32对，三维特征对用相同颜色的圆球表示.

假设共有 $n$ 对特征，如下式所示：

(1) ${{{F}}_{ij}} = \left\{ {\left( {f_{\rm{1}}^i,f_{\rm{1}}^j} \right),\left( {f_{\rm{2}}^i,f_{\rm{2}}^j} \right),\cdots,\left( {f_n^i,f_n^j} \right)} \right\}.$

由于三维特征在点云中的分布不均匀性，应用网格法对三维特征对进行降采样. 网格法即是将来自一帧数据（以 ${{{D}}_i}$为例）的三维特征集合（见式（2））置于三维网格中，网格范围即是能够覆盖该帧数据所有三维点的最小三维空间区域，网格分辨率与降采样后的特征点数强相关，分辨率越高，特征点数越多. 特征点数对位姿优化精度及效率均有影响，特征点数越多，以特征点为中心的球形半径越小，参与位姿优化的普通三维点越少，可能会导致优化精度降低，当然优化效率会变高. 若某一栅格存在特征点，则将距离网格中心点最近的特征点作为采样点. ${{{F}}_i}$经过降采样后剩余 $m$个特征点（见式（3））.

(2) ${{{F}}_i} = \left\{ {f_1^i,f_2^i,\cdots,f_n^i} \right\},$

(3) ${{F}}_i^{{'}} = \left\{ {f_{{a_1}}^i,f_{{a_2}}^i,\cdots,f_{{{\rm{a}}_m}}^i} \right\}.$

式中： $1 \leqslant {a_1} < $ ${a_2} < \cdots < {a_{{m}}} \leqslant n $.

由式（1）和（3）得到降采样后的特征对：

(4) ${{F}}_{ij}^{{'}} = \left\{ {\left( {f_{{a_1}}^i,f_{{a_1}}^j} \right),\left( {f_{{a_2}}^i,f_{{a_2}}^j} \right),\cdots,\left( {f_{{a_m}}^i,f_{{a_m}}^j} \right)} \right\}.$

为搜索特征周围的三维点，针对任意一对（第k对）三维特征 $\left( {f_{{a_k}}^i,f_{{a_k}}^j} \right) \in {{F}}_{ij}^{{'}}$，计算搜索半径 ${r_k}$，半径的取值要保证不同特征对相关的搜索区域无交集. 具体计算过程如下：首先计算任意特征 $f_{{a_w}}^i \in {{F}}_i^{{'}},w \ne k$与 ${{f}}_{{a_k}}^i$的三维距离，然后取最短距离的一半作为搜索半径 ${r_k}$. 在点云 ${{{P}}_i}$中以 $f_{{a_k}}^i$中心， ${r_k}$为半径的区域内搜索三维点集 ${{P}}_{{a_k}}^i \subset {{{P}}_i}$，同理在点云 ${{{P}}_j}$中以 $f_{{a_k}}^j$中心. ${r_k}$为半径的区域内搜索三维点集 ${{P}}_{{a_k}}^j \subset {{{P}}_j}$，组成三维点子集对 $\left( {{{P}}_{{a_k}}^i{{,P}}_{{a_k}}^j} \right)$. 从 ${{{P}}_i}$中提取m个三维点子集，如式（5）所示；从 ${{{P}}_j}$中提取m个三维点子集，如式（6）所示；组成m对三维点子集，如式（7）所示.

(5) ${{S}}_{\rm{C}}^i{{ = }}\left\{ {{{P}}_{{a_{\rm{1}}}}^i,\;{{P}}_{{a_{\rm{2}}}}^i,\;\cdots,\;{{P}}_{{a_m}}^i} \right\},$

(6) ${{S}}_{\rm{C}}^j{{ = }}\left\{ {{{P}}_{{a_{\rm{1}}}}^j,\;{{P}}_{{a_2}}^j,\;\cdots,\;{{P}}_{{a_m}}^j} \right\},$

(7) ${{S}}_{{\rm{CP}}}^{ij} = \left\{ {\left( {{{P}}_{{a_{\rm{1}}}}^i,\;{{P}}_{{a_{\rm{1}}}}^j} \right),\;\left( {{{P}}_{{a_{\rm{2}}}}^i,\;{{P}}_{{a_{\rm{2}}}}^j} \right),\;\cdots,\;\left( {{{P}}_{{a_m}}^i,\;{{P}}_{{a_m}}^j} \right)} \right\}.$

如图2所示为4个特征对的实例，搜索半径及搜索到的三维点数如表1所示，其中 $k$为特征对索引， ${r_k}$为搜索半径， $\left| {{{P}}_{{a_k}}^i} \right|$与 $\left| {{{P}}_{{a_k}}^j} \right|$均为子集中点数. 分别从点云 ${{{P}}_i}$和 ${{{P}}_j}$中搜索4个三维点子集，图中圆点代表三维特征（即搜索区域的中心），标号相同的子集构成三维点集合对.

假设TOF相机在连续采集数据过程中横滚角（Roll角）变化较小，根据三维点对应的像素在图像上的位置，式（5）、（6）中的三维点子集可以被进一步划分，从而使式（7）中的三维点子集对被细化. 将图2中的三维点子集重新投影到像面空间，每个三维点肯定可以找到对应的像素，将三维特征投影到二维特征. 令 ${{P}}_{{a_k}}^i \in {{S}}_{\rm{C}}^i$点投影到像面空间的像素集合为 ${{I}}_{{a_k}}^i \subset {{{I}}_i}$，根据像素在横向、纵向两个维度的最大、最小值，确定覆盖 ${{I}}_{{a_k}}^i$中所有像素的矩形，作通过二维特征像素的竖直线和水平线，矩形被划分为4个象限，从而把 $I_{{a_k}}^i$中的像素划分为4个子集，如式（8）所示， ${{P}}_{{a_k}}^i \in {{S}}_{\rm{P}}^i$相应的被划分为4个子集，如式（9）所示.

(8) ${{I}}_{a_k^{\rm{1}}}^i \cup {{I}}_{a_k^{\rm{2}}}^i \cup {{I}}_{a_k^{\rm{3}}}^i \cup {{I}}_{a_k^{\rm{4}}}^i = {{I}}_{{a_k}}^i,$

(9) ${{P}}_{a_k^{\rm{1}}}^i \cup {{P}}_{a_k^{\rm{2}}}^i \cup {{P}}_{a_k^{\rm{3}}}^i \cup {{P}}_{a_k^{\rm{4}}}^i = {{P}}_{{a_k}}^i.$

式（5）~（7）中的元素均被进一步划分，分别如式（10）~（12）所示.

(10) $\begin{gathered} {{S}}_{{\rm{RC}}}^i = \left\{ {{{P}}_{a_{\rm{1}}^{\rm{1}}}^i,\;{{P}}_{a_{\rm{1}}^{\rm{2}}}^i,\;\cdots,\;{{P}}_{a_{\rm{1}}^n}^i,} \right\} \cup \left\{ {{{P}}_{a_m^{\rm{1}}}^i,\;{{P}}_{a_m^{\rm{2}}}^i,\;{{P}}_{a_m^{\rm{3}}}^i,\;{{P}}_{a_m^{\rm{4}}}^i} \right\}, \end{gathered} $

(11) $\begin{gathered} \! \!{{S}}_{{\rm{RC}}}^j = \left\{ {{{P}}_{a_{\rm{1}}^{\rm{1}}}^j,\;{{P}}_{a_{\rm{1}}^{\rm{2}}}^j,\;\cdots,\;{{P}}_{a_{\rm{1}}^n}^j} \right\} \cup \left\{ {{{P}}_{a_m^{\rm{1}}}^j,\;{{P}}_{a_m^{\rm{2}}}^j,\;{{P}}_{a_m^{\rm{3}}}^j,\;{{P}}_{a_m^{\rm{4}}}^j} \right\}, \end{gathered} $

(12) $\begin{split} &\;{{S}}_{{\rm{RCP}}}^{ij} = \\ & \left\{ {\left( {{{P}}_{a_{\rm{1}}^{\rm{1}}}^i,\;{{P}}_{a_{\rm{1}}^{\rm{1}}}^j} \right),\;\left( {{{P}}_{a_{\rm{1}}^{\rm{2}}}^i,\;{{P}}_{a_{\rm{1}}^{\rm{2}}}^j} \right),\;\cdots,\;\left( {{{P}}_{a_{\rm{1}}^n}^i,\;{{P}}_{a_{\rm{1}}^n}^j} \right)} \right\} \cup \\ & \left\{ {\left( {{{P}}_{a_m^{\rm{1}}}^i,\;{{P}}_{a_m^{\rm{1}}}^j} \right),\;\left( {{{P}}_{a_m^{\rm{2}}}^i,\;{{P}}_{a_m^{\rm{2}}}^j} \right),\;\left( {{{P}}_{a_m^{\rm{3}}}^i,\;{{P}}_{a_m^{\rm{3}}}^j} \right),\;\left( {{{P}}_{a_m^{\rm{4}}}^i,\;{{P}}_{a_m^{\rm{4}}}^j} \right)} \right\}. \end{split}$

图2中三维点子集对应的像素经过进一步划分，得到如图3所示的结果，为清晰显示，图中对于每一个特征对，只标注一个象限对应的像素集合，标号相同的像素集合构成集合对. 矩形框内的像素若未出现在三维点子集中，则该像素全用黑色显示.

将球形区域内的三维点云继续划分，进一步缩小搜索空间，提高搜索效率. 矩形被划分为4个象限，即根据三维点对应的像素在图像上的位置关系，将球形区域内的三维点划分为4份，进一步缩小匹配点搜索空间，提高匹配效率. 三维点子集 ${{P}}_{{a_k}}^i$和 ${{P}}_{{a_k}}^j$均被划分为4个子集，其中子集 ${{P}}_{{a_k}}^i$内的任意三维点的匹配点一定在子集 ${{P}}_{{a_k}}^j$中.

根据前述方法，设计帧间点云数据配准流程，伪代码算法形式如算法1所示.

算法1：帧间点云配准

输入：2帧数据 ${{{D}}_i}\left( {{{{I}}_i},\;{{{P}}_i}} \right)$ ${{{D}}_j}\left( {{{{I}}_j},\;{{{P}}_j}} \right)$

输出：三维点对集合 ${{S}}_{{\rm{PP}}}^{ij}$

匹配尺度不变特征点对

IF特征点对的数量小于3 THEN

　　 RETURN Ø

END IF

根据特征点对估计相对位姿 ${{{T}}_{ij}}$

生成三维点子集对：

${{S}}_{{\rm{CP}}}^{ij} = \left\{ {\left( {{{P}}_{{a_{\rm{1}}}}^i,\;{{P}}_{{a_{\rm{1}}}}^j} \right),\;\left( {{{P}}_{{a_{\rm{2}}}}^i,\;{{P}}_{{a_{\rm{2}}}}^j} \right),\;\cdots,\;\left( {{{P}}_{{a_m}}^i,\;{{P}}_{{a_m}}^j} \right)} \right\}$

IF需要划分三维点子集THEN

进一步划分：

${{S}}_{{\rm{RCP}}}^{ij} \leftarrow \text{细化}{{S}}_{{\rm{CP}}}^{ij}$

　　 FOR任意一对三维点子集

${{S}}_{{\rm{RCP}}}^{ij} \leftarrow \text{细化}{{S}}_{{\rm{CP}}}^{ij}$

　　　　 FOR任意一个三维点 ${{{p}}_i} \in {{P}}_{a_k^{\rm{e}}}^i$

经过 ${{{T}}_{ij}}$位姿变换后，从 ${{P}}_{a_k^{\rm{e}}}^j$搜索与 ${{{p}}_i}$距离最近的点，记作 ${{{p}}_j}$

${{S}}_{{\rm{PP}}}^{ij} \leftarrow {{S}}_{{\rm{PP}}}^{ij} \cup \left\{ {\left( {{{{p}}_i},\;{{{p}}_j}} \right)} \right\}$

　　　　 END FOR

　　 END FOR

ELSE

　　 FOR任意一对三维点子集

$\left( {{{P}}_{{a_k}}^i,\;{{P}}_{{a_k}}^j} \right) \in {{S}}_{{\rm{CP}}}^{{{ij}}}$

　　　　 FOR任意一个三维点 ${{{p}}_i} \in {{P}}_{{a_k}}^i$

经过 ${{{T}}_{ij}}$位姿变换后，从 ${{P}}_{{a_k}}^j$搜索与 ${{{p}}_i}$距离最近的点，记作 ${{{p}}_j}$

${{S}}_{{\rm{PP}}}^{ij} \leftarrow {{S}}_{{\rm{PP}}}^{ij} \cup \left\{ {\left( {{{{p}}_i},\;{{{p}}_j}} \right)} \right\}$

　　　　 END FOR

　　 END FOR

END IF

RETURN ${{S}}_{{\rm{PP}}}^{ij}$

END

算法1以2帧数据为输入，从2帧点云中生成匹配的三维点对集合，用于优化TOF相机的位姿. 算法首先从2幅图像中提取并匹配尺度不变特征点，若特征点对数量小于3，则算法终止，说明2帧数据的重叠部分较少，对位姿优化贡献比较有限. 算法应用匹配的三维特征对估计^[22]2帧数据之间对应的相对位姿 ${{{T}}_{ij}}$，应用该相对位姿把点云变换到同一个坐标系下再搜索最近点作为匹配点. 如果在相机连续拍摄过程中，横滚角变化较大，那么省去第1.2节叙述的三维点集合进一步划分的环节，否则进一步划分，使得搜索空间再次缩小. 针对集合（ ${{S}}_{{\rm{CP}}}^{ij}$或 ${{S}}_{{\rm{RCP}}}^{ij}$）中的每一对三维点子集，从2个子集中生成匹配的三维点对，如果子集的规模较大，还需要将来自数据帧 ${{{P}}_j}$的子集中的点组织到KD-Tree中，以提高搜索效率.

根据算法1的流程，只有三维特征点周围的三维点才参与生成三维点对，因此生成的点对数量一定小于原始点云（ ${{{P}}_i}$或 ${{{P}}_j}$）中点的数量，此为本文算法的帧间配准效率高的原因之一；另外一个原因是本文算法将匹配三维点锁定在较小的空间，保证了搜索效率和精度.

图结构调整算法^[16]的核心思想是将绝对位姿和相对位姿分别抽象为图中的顶点和边，通过调整顶点的位置达到优化位姿的目的.

构建如式（13）所示的马氏距离等式，其中 $\overline {{{{T}}_{ij}}} $表示由第 $i,j$帧数据估计出来的相对位姿， ${{{M}}_{ij}}$是 $\overline {{{{T}}_{ij}}} $的精度，用协方差矩阵表示^[23]， ${{{T}}_{ij}}$表示期望相对位姿，位姿优化的实质是式（16），使得期望相对位姿最大程度接近估计相对位姿，即满足图中边的约束.

(13) ${{W}} = \sum\limits_{i,j} {{{\left( {{{{T}}_{ij}} - \overline {{{{T}}_{ij}}} } \right)}^{\rm{T}}}{{M}}_{ij}^{ - 1}} \left( {{{{T}}_{ij}} - \overline {{{{T}}_{ij}}} } \right).$

相对位姿可以表示为绝对位姿“差”的形式，如式（14）所示，其中 ${{{X}}_i} = {\left[ {{x_i},{y_i},{z_i},{\alpha _i},{\beta _i},{\gamma _i}} \right]^{\rm{T}}}$，代入式（13），得到式（15）：

(14) ${{{T}}_{ij}} = {{{X}}_i} - {{{X}}_j},$

(15) ${{W}} = \sum\limits_{i,j} {{{\left( {{{{X}}_i} - {{{X}}_j} - \overline {{{{T}}_{ij}}} } \right)}^{\rm{T}}}{{M}}_{ij}^{ - 1}} \left( {{{{X}}_i} - {{{X}}_j} - \overline {{{{T}}_{ij}}} } \right).$

假设有 $Q$帧数据参与优化，把所有 ${{{X}}_i}$拼接为一列向量 ${{X}} = \left[ {{{X}}_{\rm{1}}};{{{X}}_{\rm{2}}};\cdots;{{{X}}_{\rm{Q}}} \right]$，所有有效 ${{{T}}_{ij}}$拼接为一列向量 ${{T}} = \left[ {{{{T}}_{12}};{{{T}}_{13}};\cdots;\;{{T}}_{ij}} \right]$，构造元素值只有0、1、−1的关联矩阵 ${{{H}}_{{\rm{6}} c \times {\rm{6}}Q}}$，满足 ${{T}} = {{HX}}$. 其中， $c$为有重叠区域的数据帧对数量. 式（15）中的等式用矩阵形式表示如下：

(16) ${{W}} = {\left( {{{HX}} - \overline {{T}} } \right)^{\rm{T}}}{{{M}}^{ - 1}}\left( {{{HX}} - \overline {{T}} } \right).$

式中：矩阵M的维度为 $6 c \times 6 c$，所有 ${{{M}}_{ij}}$位于M的主对角元上， $\overline {{T}} $表示所有根据有效数据帧对计算出来的相对位姿估计值 $\overline {{{{T}}_{ij}}} $的纵向拼接. 以最小化式（15）中的 ${{W}}$为目标，应用最大似然法优化得绝对位姿向量：

(17) ${{X}} = {\left( {{{{H}}^T}{{{M}}^{ - 1}}{{H}}} \right)^{ - 1}}{{{H}}^{\rm{T}}}{{{M}}^{ - 1}}\overline {{T}}. $

表征X精度的协方差矩阵如下：

(18) $\begin{split} {{{M}}_X} = &\left( {{{\left( {{{{H}}^{\rm{T}}}{{{M}}^{ - 1}}{{H}}} \right)}^{ - 1}}{{{H}}^{\rm{T}}}{{{M}}^{ - 1}}} \right){{M}}\\ & {\left( {{{\left( {{{{H}}^{\rm{T}}}{{{M}}^{ - 1}}{{H}}} \right)}^{ - 1}}{{{H}}^{\rm{T}}}{{{M}}^{ - 1}}} \right)^{\rm{T}}} =\\ & {\left( {{{{H}}^{\rm{T}}}{{{M}}^{ - 1}}{{H}}} \right)^{ - 1}}. \end{split}$

式（14）为表述图结构位姿优化算法的简洁性，相对位姿直接表示为2个绝对位姿的线性化形式，而实际上六自由度相对位姿并不是线性的，因此必须将其进行线性化处理^[16-17]，受篇幅限制，本文暂且不展开讨论.

根据第2.1节简述的基于图结构的六自由度位姿优化原理，将算法1所述的帧间配准过程融入到图结构优化算法中，其算法流程的伪代码形式如算法2所示. 算法2通过配准所有的2帧组合对，选用有效配准的帧对，所谓有效配准即是2帧数据有重叠区域，并且应用算法1能够获得足够数量的正确三维点对，搜寻所有可能对优化产生贡献的数据帧对. 综上，算法1与算法2之间的逻辑关系可以用图4中的流程图表示，左侧是优化主流程，其中2帧点云数据的帧间配准是优化过程的关键环节，用右侧虚线框标出. 算法2实现了TOF相机位姿优化主流程，算法1实现了2帧点云数据的帧间配准.

算法2：TOF相机位姿优化

输入： $Q$帧在不同位置采集的数据

${{{D}}_1}\left( {{{{I}}_1},{{{P}}_1}} \right),{{{D}}_2}\left( {{{{I}}_2},{{{P}}_2}} \right),\cdots,{{{D}}_Q}\left( {{{{I}}_Q},{{{P}}_Q}} \right)$

输出：W帧数据采集时的绝对位姿

${{{X}}_1},{{{X}}_2},\cdots,{{{X}}_Q}$

初始化有效数据帧对的数量 $c = 0$

FOR EACH $1 \leqslant i \leqslant \left( {Q - 1} \right)$

　　 FOR EACH $\left( {i + 1} \right) \leqslant j \leqslant Q$

应用算法1配准数据帧

${{{D}}_i}\left( {{{{I}}_i},{{{P}}_i}} \right),{{{D}}_j}\left( {{{{I}}_j},{{{P}}_j}} \right)$

　　 IF配准结果不为Ø的THEN

根据配准的三维点对计算相对位姿估计值 $\overline {{{{T}}_{ij}}} $及其精度 ${{{M}}_{ij}}$

$c \leftarrow c + 1$

　　 END IF

　　 END FOR

END FOR

构造关联矩阵 ${{{H}}_{6 c \times 6Q}}$，协方差矩阵 ${{M}}_{6 c \times 6 c}$

应用最大似然法优化TOF相机位姿：

${{X}} \leftarrow {\left( {{{{H}}^{\rm{T}}}{{{M}}^{ - {\rm{1}}}}{{H}}} \right)^{ - {\rm{1}}}}{{{H}}^{\rm{T}}}{{{M}}^{ - {\rm{1}}}}\overline {{T}} $

RETURN ${{X}}$

END

从TOF相机位姿优化精度和效率2个方面分别说明本文的图结构优化算法的有效性，将优化得到的TOF相机位姿与真实位姿进行比较，计算误差值来度量精度，运动捕捉系统实时记录TOF相机的运动，将记录值作为TOF相机的真实位姿. 通过比较应用本文帧间配准算法前、后的图结构优化算法执行时间来说明本文算法在效率上的优势.

令 ${{{T}}_{{\rm{abs}}}}$为优化后某一帧数据对应的绝对位姿， ${{T}}_{{\rm{abs}}}^*$为采集该数据时TOF相机的实际位姿，定义位姿误差矩阵：

19) ${{E}} = {{T}}_{{\rm{abs}}}^*{\rm{\Theta}} {{{T}}_{{\rm{abs}}}} = {{T}}_{{\rm{abs}}}^{ - 1}{{T}}_{{\rm{abs}}}^*.$

式中： $\Theta $为位姿复合减法运算，E为齐次变换矩阵，包括 $3 \times 3$的旋转矩阵 ${{{E}}_{\rm{r}}}$， $3 \times 1$的平移向量 ${{{E}}_{\rm{t}}}$. 旋转误差和平移误差的定义如下，

(20) ${{{e}}_{\rm{r}}} = {\cos ^{ - 1}}\left( {{\rm{trace}}\left( {{{{E}}_{\rm{r}}}} \right)} \right),$

(21) ${{{e}}_{\rm{t}}} = \left\| {{{{E}}_{\rm{t}}}} \right\|.$

其中， ${\rm{trace}}$表示计算矩阵的迹.

改进前、后的图结构位姿优化算法均用C++语言编写，在同一台装有64位Ubuntu16.04的笔记本电脑上执行，电脑CPU是Intel（R） Core（TM） i3-2350，主频2.3 GHz，内存4 G. 在实现本文提出的帧间配准算法时，降采样选用的三维网格的分辨率设置为（0.3 m，0.3 m，0.3 m）.

原始图结构位姿优化算法中的帧间配准通过ICP迭代过程实现，若能够从2幅图像中提取出匹配特征点对，则应用该特征对计算ICP迭代过程的初始相对位姿；反之，借助中间数据帧计算迭代的初始相对位姿.

如图5所示，应用Motion Analysis运动捕捉系统实时记录TOF相机的运动过程，根据时间戳确定采集数据时的“真实”位姿，该运动捕捉系统的最高定位频率为500 Hz，本文离线采集TOF相机数据，每秒抓拍2次，定位频率远高于TOF相机拍照频率，因此利用该运动捕捉系统对TOF相机进行定位是合理的. 标定系统后，经过多次测试，TOF相机直线运动2 m，运动捕捉系统测量误差能够控制在1 mm以内，因此将运动捕捉系统提供的定位数值作为TOF相机的“真实”位姿是可以接受的. 运动捕捉系统只识别由强反光材质制作的Marker点，实验中，在TOF相机上粘贴8个Marker点，采集数据之前需要标定其在TOF相机坐标系下的三维坐标. TOF相机选用的是PMD Camcube3.0，4 m测量范围内精度可以达到1 cm，分辨率是200×200.

经多方向移动、多角度旋转TOF相机，连续采集1 000帧数据，如式（22）所示，运动捕捉系统同时记录下数据采集时的1 000个绝对位姿：

(22) ${{D}} = \left\{ {{{{D}}_1}\left( {{{{I}}_1},{{{P}}_1}} \right),{{{D}}_2}\left( {{{{I}}_2},{{{P}}_2}} \right),\cdots,{{{D}}_{{\rm{1\;000}}}}\left( {{{{I}}_{{\rm{1\;000}}}},{{{P}}_{{\rm{1\;000}}}}} \right)} \right\}.$

以第一帧 ${{{D}}_1}\left( {{{{I}}_1},{{{P}}_1}} \right)$数据为“基准”，利用图结构优化算法计算D中其他数据帧相对于基准的位姿. 若图结构位姿优化1次，有 $Q$帧数据参与，则第一次优化基于以下数据帧，

(23) ${{{G}}_{\rm{1}}} = \left\{ {{{{D}}_{\rm{1}}},{{{D}}_{\rm{2}}},\cdots,{{{D}}_Q}} \right\}.$

第二次优化基于以下数据帧，

(24) ${{{G}}_2} = \left\{ {{{{D}}_Q},{{{D}}_{Q + 1}},\cdots,{{D}}_{2Q - 1}} \right\}.$

依次类推，假设共执行 $\lambda $次图结构优化算法，则 $\lambda $满足如下不等式约束：

(25) $\lambda (Q - 1) + 1 \leqslant 1\;000.$

令 ${\lambda _Q}$为使不等式（25）成立的最大 $\lambda $值，经过 ${\lambda _Q}$次优化算法执行，计算出 ${\lambda _Q}(Q - 1) + 1$帧数据采集时的TOF相机相对于“基准”的位姿，然后应用式（20）、（21）计算各个位姿与真实位姿的旋转误差和平移误差，取所有旋转误差的平均值和所有平移误差的平均值，结果分别如图6和7所示. 图中“原算法”代表采用ICP迭代实现帧间配准的图结构优化算法，而“改进算法”代表采用基于特征点的帧间配准的优化算法. 可以看出，改进后的图结构优化算法的优化精度明显高于原始算法. 针对每一个Q值，分别计算精度提高倍数，得到旋转量精度平均提高了1.43倍，平移量精度平均提高了1.76倍.

从图中还可以看出，当每次优化参与数据帧数 $Q \in \left[ {3,7} \right]$时，随着 $Q$值的增加，改进前、后的图结构优化算法均使得误差递减. 但是当 $Q \geqslant 8$时，原始算法的计算误差反而增加，原因是本没有重叠区域的2帧数据，在应用ICP算法进行帧间配准时产生较多的误匹配点对，从而导致误差升高. 改进后的算法直接将无重叠的数据帧对滤除，保证误差不会升高，但是当 $Q \geqslant 8$时，误差并未随着 $Q$的增加而显著降低. 一般来说，时间戳相距较大的数据帧之间无重叠区域，改进后的算法不对此类数据帧对建立图结构中“边”的约束，因此对TOF相机位姿的优化无明显的贡献.

原始图结构优化算法需要把任意2帧数据进行配准，即便无重叠区域，并且所有三维点参与到基于ICP的帧间配准过程中；算法改进后，无重叠区域的2帧数据不参与位姿优化，并且只有特征点周围的三维点才参与帧间配准过程，因此优化效率提高比较明显. 原始算法执行时间与参与优化的数据帧对数量成大致正比的关系，数据帧对的数量即是 $Q$的二组合数 $C_Q^2$. 对于 $Q$帧数据参与的图结构优化，共执行 ${\lambda _{\rm{Q}}}$次，记录每次的执行时间，平均执行时间 ${t_{{\rm{ave}}}}$如图8所示. 实验结果印证了前述理论分析.

TOF相机随机移动5次，共采集6帧数据，6幅灰度图像如图9所示. 应用本文的改进图结构优化算法，执行2次位姿优化算法，第一次基于第1、2、3、4帧数据，第二次基于第4、5、6帧数据. 以第1帧数据采集时的TOF相机位姿为“基准”，其他5帧数据根据其位姿分别变换到“基准”坐标系下，最终将6帧数据拼接在一起，结果如图10所示. 位姿优化的精度直接决定点云拼接的质量，对比图10中的上、下2幅点云拼接效果图，不难看出，改进后的算法使得6帧数据拼接效果较好，说明位姿优化精度较高. 其相机在不同位姿条件下拍摄同一场景时，灰度值有所不同，而三维点用对应像素的灰度值赋予颜色信息，因此不同数据帧的三维点存在色差.

本文以应用图结构位姿优化算法对TOF相机采集数据时的位姿进行优化为研究背景，提出了基于图像及尺度不变特征的帧间数据配准算法，帧间配准效率和精度均得到较大程度的改善. 帧间配准是图结构优化算法的关键操作步骤，实验结果显示，融合本文提出的帧间配准策略的图结构优化算法的优化效率和精度与原始算法相比均显著提高. 从优化效率上来说，本文算法比原算法平均提升39.2%；从优化精度上来说，平移精度平均提升57.7%，旋转精度平均提升65.7%.

本文的帧间配准算法不仅能够用于TOF等三维相机，也为应用双目、单目等其他相机进行定位和环境重建提供了新思路. 下一步研究是将此算法应用于移动机器人平台，实时定位机器人的位姿，同时构建周围环境的准确地图.