CN102510512B - 基于双目视觉节点的动态三维多媒体传感网络自定位方法 - Google Patents

Abstract

本发明涉及一种基于双目视觉节点的动态三维多媒体传感网络自定位方法。目前的方法通常都是基于单目视觉节点。本发明方法各节点独立提取基于SIFT特征描述符的三维点云；并根据两两节点间匹配信息，采用基于单位四元数的运动估计方法获得相对位姿；最后分布式定位策略实现所有节点的自定位。其最主要优势在于：在三维网络部署下，仅在两节点间即可完成其相对位姿估计，进而可有效避免不必要的“洪泛”风险和能量损失。该优势在大规模动态多媒体传感网络中尤为重要。

Description

基于双目视觉节点的动态三维多媒体传感网络自定位方法

技术领域

本发明属于多媒体传感网络领域，具体涉及一种基于双目视觉节点的动态三维多媒体传感网络分布式自定位方法。

背景技术

由于视觉传感器可获取丰富且有价值的环境上下文信息，多媒体传感网络受到越来越广泛的关注。与此同时，近年来国内外在嵌入式系统及分布式视觉算法上取得的显著进展也为其应用和实现铺平道路。不同于传统传感器网络中传感器节点采用的全向感知模型，多媒体传感网络中的视觉节点存在视角受限且易被遮挡等问题，面临着有向感知和视距覆盖的新挑战。因此视觉节点定位是实现多媒体传感网络部署和分析的前提和关键。

目前已提出了许多多媒体传感网络自定位方法。但这些方法通常都是基于单目视觉节点，O. Faugeras等人研究表明两个已标定像机只能实现尺度上的相对位姿估计[1]，为获取节点位姿的唯一解，一种方法是需假定场景为近似平坦的表面，通过计算单应性矩阵实现位姿估计。另一种则是采用类似Bundle Adjustment方法将多个甚至全部节点共同参与定位估计，即要求每个节点都要通过“洪泛”将其特征集合传递给整个网络。如W. Mantzel等人提出的分布交替式三角定位技术（DALT）[2]，Z. Cheng等人提出的基于视觉SfM方法的视觉图模型[3]等。然而这种需要大量“洪泛”过程的方法，在网络规模较大或是动态网络下难以有效实现。

为避免单目视觉固有的歧义性问题，国内外学者提出了基于立体视觉节点的多媒体传感网络。S. Hengstler等人设计并实现了MeshEye立体视觉节点的原型系统[4]。S. Kumar等人提出一种双PTZ像机的视觉节点，通过SIFT特征匹配并估计单应性矩阵实现运动目标定位[5]。A. Mavrinac等人采用双目视觉节点经ZNCC算法匹配Harris或FAST特征，实现多媒体传感网络的自标定[6]。

参考文献

[1] Q. Luong, O. Faugeras. Camera Calibration, Scene Motion and Structure Recovery From Point Correspondences and Fundamental Matrices. International Journal of Computer Vision. 22(3): 261-289, 1997.

[2] W. Mantzel, H. Choi, R. Baraniuk. Distributed Camera Network Localization. Thirty-eighth Asilomar Conference on Signals, Systems & Computers. Pacific Grove, CA.2:1381-1386, 2004.

[3] Z. Cheng, D. Devarajan, R. Radke. Determining Vision Graphs for Distributed Camera Networks Using Feature Digests. EURASIP Journal on Applied Signal Processing. (1): 220-220. 2007.

[4] S. Hengstler, D. Prashanth, S. Fonget, et al. MeshEye: A Hybrid-Resolution Smart Camera Mote for Applications in Distributed Intelligent Surveillance. Information Processing in Sensor Networks (IPSN-SPOTS). 2007.

[5] S.Kumar, C. Micheloni, C. Piciarelli. Stereo Localization Using Dual PTZ Cameras. International Conference on Computer Analysis of Images and Patterns. 2009.

[6] A. Mavrinac, X. Chen, K. Tepe. An Automatic Calibration Method for Stereo-based 3D Distributed Smart Camera Networks. Computer Vision and Image Understanding. 114(8): 952-962. 2010.

[7] D. Lowe. Distinctive Image Features from Scale Invariant Keypoints. International Journal of Computer Vision. 60: 91-110. 2004.

[8] Y. Ke, R. Sukthankar. PCA-SIFT: A More Distinctive Representation for Local Image Descriptors. Computer Vision and Pattern Recognition, 2004

[9] B. Horn. Closed-form Solution of Absolute Orientation Using Unit Quaternions. Journal of the Optical Society of America. 4(4): 629-642. 1987。

发明内容

本发明针对视觉节点可在任意三维空间中随机部署，且可任意旋转确保目标连续跟踪的更一般情况，提供一种无任何假定约束的动态三维多媒体传感网络的分布式自定位方法。

本发明中各节点独立提取基于SIFT特征描述符[7, 8]的三维点云；并根据两两节点间匹配信息，采用基于单位四元数的运动估计方法[9]获得相对位姿；最后分布式定位策略实现所有节点的自定位。其最主要优势在于：在三维网络部署下，仅在两节点间即可完成其相对位姿估计，进而可有效避免不必要的“洪泛”风险和能量损失。

本发明方法的具体步骤如下：

步骤(1)网络中各视觉节点独立地提取该节点视野范围内的视觉特征。

所述的视觉特征提取具体方法为：

每个视觉节点采用一个已标定的双目立体像机，包含了左、右两个像机，可实时获取左、右立体图像；然后采用Y. Ke等人提出的PCA-SIFT[9]方法，分别提取左、右图像的SIFT特征描述符，每个特征描述符为36维矢量，每一维矢量用8位来表示；所述的双目立体像机标定方法为成熟技术。

采用KD-Tree方法实现左、右图像间的SIFT特征匹配，并记录每对SIFT特征匹配的匹配代价。所述的SIFT特征匹配方法为成熟技术。

采用外极线约束滤除误匹配点；对于已标定的双目立体像机，外极线约束要求正确匹配点对在左、右图像中的垂直坐标差小于1。

以左图像为视觉特征提取的基准图像，将左图像划分为20×20个大小相等的区域，在每个区域中只选取一个具有最大匹配代价的SIFT特征作为该区域的视觉特征，即从左图像中最多可提取出400个视觉特征，并将这些视觉特征作为节点当前帧的视觉特征；因此每一个视觉节点从每一帧立体图像对中提取视觉特征的描述符最大数据量为400×36×8/1024=112.5kb。

步骤(2)网络中各视觉节点独立计算步骤(1)中所提取视觉特征的三维坐标。

所述的视觉特征三维坐标计算具体方法为：

步骤(1)中所提取视觉特征在左、右图像中的垂直坐标差小于1，而水平坐标差称为视差d，根据双目立体视觉三维重建公式，可得视觉特征i在左像机坐标系下的三维坐标                                                

：

                      

                        (1)

其中b，f，(u ₀, v ₀)分别为双目立体像机标定中获得的基线、焦距和左图像光心坐标。(u _i , v _i )为视觉特征i在左图像中的坐标，d _i 为视觉特征i的视差。

步骤(3)网络中各视觉节点的网络定位数据包生成，具体为：

将步骤(1)和步骤(2)中提取的视觉特征描述符及其三维坐标打包作为视觉节点的网络定位数据包，并根据各视觉节点内的邻域关系表传递给邻域节点。

初始时各视觉节点内的邻域关系表为空，即各视觉节点间邻域关系未知，此时各视觉节点将其网络定位数据包通过“洪泛”传递给网络中所有视觉节点。

各视觉节点在接收到其他视觉节点的网络定位数据包后，更新其内部邻域关系表。随着各视觉节点内的邻域关系表逐步完善，即可通过邻域关系表估计出可能相邻的节点或仍未知关系的节点，则网络定位数据包将会有目的性地传递给这些相邻或仍未知关系的节点。最终待各节点内的邻域关系表完善后，每次网络定位数据包发送都将具有明确的目的节点，从而有效避免不必要的“洪泛”风险和能量损失。所述的邻域关系表构建和更新的具体表述在步骤(4)到步骤(7)中。

步骤(4)网络中两个视觉节点间视觉特征匹配，具体为：

当一个视觉节点接收到其他视觉节点的网络定位数据包后，首先进行两视觉节点间的视觉特征匹配。采用与步骤(1)中相同的SIFT特征匹配方法。由于步骤(1)中视觉特征提取都是基于左图像的，因此类似于实现两视觉节点左图像间的SIFT特征匹配。

步骤(5)根据步骤(4)中得到的两个视觉节点间视觉特征匹配关系，估计两个视觉节点间的相对位姿，即获取两个视觉节点间的旋转矩阵和平移矩阵。

所述的两个视觉节点间相对位姿估计具体方法为：

令两个视觉节点m和n的视觉特征匹配点对的三维坐标分别表示为

和

，根据视觉成像关系，有：

                      

                        (2)

其中矩阵

和

分别代表两个视觉节点相对位姿中的旋转矩阵和平移矩阵。相对位姿估计可转换为采用最小二乘法求解下式最小：

                         

                      (3)

采用单位四元数

表示旋转矩阵

，用四元数加法和乘法代替旋转矩阵中的三角计算，可简化计算，并提高求解精度，有：

           

         (4)

从而相对位姿估计中包含了7个自由度，理论上只需3对匹配点即可实现估计。为确保估计结果的鲁棒性，通常要求匹配点数大于某个阈值N（通常采用20）。构建四元数的共生对称矩阵

：

(5)

其中

和

为

和

经数值解归一化，减去质心后的三维坐标。i为参与相对位姿估计的匹配点标号。

共生对称矩阵

经SVD分解，其最大特征根所对应的特征向量即为单位四元数的最优解。根据公式(4)恢复旋转矩阵

，并代入公式(2)，可获得平移矩阵

；

为提高估计精度和鲁棒性，滤除偏离较大的野值，采用归一化RANSAC算法，从两个视觉节点间所有视觉特征匹配点对中估计出

和

。所述的归一化RANSAC算法为成熟技术。

最后采用非线性最小二乘LM（Levenberg-Marquardt）算法同时优化

和

的估计值。所述的非线性最小二乘LM算法为成熟技术。

步骤(6)根据步骤(5)中获得的两个视觉节点间相对位姿，更新视觉节点内邻域关系表。

所述的节点内邻域关系表更新具体方法为：

每个节点内都建有一张二维的邻域关系表

，令

表示视觉节点m和n间的邻域关系，

和

为步骤(5)中估计结果，有

。则

更新为：

                                              (6)

搜索整个邻域关系表，若邻域关系表中已知

，则可通过矩阵推导，同时更新

：

                  

               (7)

步骤(7)当某个视觉节点m发生运动，则该视觉节点进行自运动估计，确定视觉节点m运动后的新邻域节点，并根据新的邻域关系发送该视觉节点的网络定位数据包，更新邻域关系表。

所述的视觉节点自运动估计具体方法为：

视觉节点m根据步骤(1)到步骤(3)提取当前帧的网络定位数据包；

在视觉节点m中，采用与步骤(1)中相同的SIFT特征匹配方法，实现视觉节点m当前帧视觉特征和视觉节点m前一帧视觉特征的SIFT特征匹配；

采用与步骤(5)中相同的相对位姿估计方法，估计视觉节点m当前帧和前一帧间的相对位姿，即估计出视觉节点m在当前帧和前一帧间自运动的旋转矩阵

和平移矩阵

，有

，其中为视觉节点m当前帧视觉特征的三维坐标，

为视觉节点m前一帧视觉特征的三维坐标。

所述的视觉节点自运动后邻域关系表更新具体方法为：

在视觉节点m自运动估计后，搜索整个邻域关系表，若邻域关系表中视觉节点m和n在视觉节点m运动前的邻域关系已知，则视觉节点m运动后的

为：

根据

判断视觉节点m和n是否为视觉相邻，即视觉节点m和n在视野上存在较大范围重叠。判断依据为视觉节点m和n相对位姿中旋转角度不能超过

弧度（根据双目像机的有效视角选取，通常为

），平移距离不能超过

米（根据双目像机的有效视距选取，通常为5米）。

在确定视觉节点m与其他节点的邻域关系后，视觉节点m将当前帧的网络定位数据包发送给所有可能与视觉节点m存在视觉相邻的节点，并采用步骤(4)到步骤(6)的过程，更新邻域关系表。

本发明的有益效果：本发明提供了一种基于双目视觉节点的动态三维多媒体传感网络自定位方法。该方法采用双目立体像机作为视觉节点，每个视觉节点独立提取有限个数SIFT特征描述符及其三维坐标，并打包为该视觉节点的网络定位数据包；两个视觉节点间仅需存在3个及以上的特征匹配点对即可估计出这两节点的相对位姿，可有效降低网络通信开销和节点存储容量需求。同时采用基于四元数的位姿估计方法，在提高数值解精度的同时还具有计算简便等特点。结合视觉节点自运动估计和节点间相对位姿估计，在每个视觉节点内建立和更新邻域关系表，以此确定各视觉节点数据包发送的目标地址，避免不必要的“洪泛”风险和能量损失。

附图说明

图1为本发明中的方法框图。

具体实施方式

以下结合附图对本发明作进一步说明。

    如图1所示，基于双目视觉节点的动态三维多媒体传感网络自定位方法的具体步骤如下：

步骤(1)网络中各视觉节点独立地提取该节点视野范围内的视觉特征。

所述的视觉特征提取方法具体为：

每个视觉节点采用一个已标定的双目立体像机，包含了左、右两个像机，可实时获取左、右立体图像；然后采用Y. Ke等人提出的PCA-SIFT[9]方法，分别提取左、右图像的SIFT特征描述符，每个特征描述符为36维矢量，每一维矢量用8位来表示；所述的双目立体像机标定方法为成熟技术。

采用KD-Tree方法实现左、右图像间的SIFT特征匹配，并记录每对SIFT特征匹配的匹配代价。所述的SIFT特征匹配方法为成熟技术。

采用外极线约束滤除误匹配点；对于已标定的双目立体像机，外极线约束要求正确匹配点对在左、右图像中的垂直坐标差小于1。

以左图像为视觉特征提取的基准图像，将左图像划分为20×20个大小相等的区域，在每个区域中只选取一个具有最大匹配代价的SIFT特征作为该区域的视觉特征，即从左图像中最多可提取出400个视觉特征，并将这些视觉特征作为节点当前帧的视觉特征；因此每一个视觉节点从每一帧立体图像对中提取视觉特征的描述符最大数据量为400×36×8/1024=112.5kb。

步骤(2)网络中各视觉节点独立计算步骤(1)中所提取视觉特征的三维坐标。

所述的视觉特征三维坐标计算方法具体为：

步骤(1)中所提取视觉特征在左、右图像中的垂直坐标差小于1，而水平坐标差称为视差d，根据双目立体视觉三维重建公式，可得视觉特征i在左像机坐标系下的三维坐标

：

                      

                        (1)

其中b，f，(u ₀, v ₀)分别为双目立体像机标定中获得的基线、焦距和左图像光心坐标。(u _i , v _i )为视觉特征i在左图像中的坐标，d _i 为视觉特征i的视差。

步骤(3)网络中各视觉节点的网络定位数据包生成。

所述的视觉节点网络定位数据包生成方法具体为：

将步骤(1)和步骤(2)中提取的视觉特征描述符及其三维坐标打包作为视觉节点的网络定位数据包，并根据各视觉节点内的邻域关系表传递给邻域节点。

初始时各视觉节点内的邻域关系表为空，即各视觉节点间邻域关系未知，此时各视觉节点将其网络定位数据包通过“洪泛”传递给网络中所有视觉节点。

各视觉节点在接收到其他视觉节点的网络定位数据包后，更新其内部邻域关系表。随着各视觉节点内的邻域关系表逐步完善，即可通过邻域关系表估计出可能相邻的节点或仍未知关系的节点，则网络定位数据包将会有目的性地传递给这些相邻或仍未知关系的节点。最终待各节点内的邻域关系表完善后，每次网络定位数据包发送都将具有明确的目的节点，从而有效避免不必要的“洪泛”风险和能量损失。所述的邻域关系表构建和更新的具体表述在步骤(4)到步骤(7)中。

步骤(4)网络中两个视觉节点间视觉特征匹配

所述的两个视觉节点间视觉特征匹配方法具体为：

当一个视觉节点接收到其他视觉节点的网络定位数据包后，首先进行两视觉节点间的视觉特征匹配。采用与步骤(1)中相同的SIFT特征匹配方法。由于步骤(1)中视觉特征提取都是基于左图像的，因此类似于实现两视觉节点左图像间的SIFT特征匹配。

步骤(5)根据步骤(4)中得到的两个视觉节点间视觉特征匹配关系，估计两个视觉节点间的相对位姿，即获取两个视觉节点间的旋转矩阵和平移矩阵。

所述的两个视觉节点间相对位姿估计方法具体为：

令两个视觉节点m和n的视觉特征匹配点对的三维坐标分别表示为和

，根据视觉成像关系，有：

                                              (2)

其中矩阵和

分别代表两个视觉节点相对位姿中的旋转矩阵和平移矩阵。相对位姿估计可转换为采用最小二乘法求解下式最小：

                         

                      (3)

采用单位四元数表示旋转矩阵

，用四元数加法和乘法代替旋转矩阵中的三角计算，可简化计算，并提高求解精度，有：

                   (4)

从而相对位姿估计中包含了7个自由度，理论上只需3对匹配点即可实现估计。为确保估计结果的鲁棒性，通常要求匹配点数大于某个阈值N（通常采用20）。构建四元数的共生对称矩阵

：

(5)

其中和

为

和

经数值解归一化，减去质心后的三维坐标。i为参与相对位姿估计的匹配点标号。

共生对称矩阵

经SVD分解，其最大特征根所对应的特征向量即为单位四元数的最优解。根据公式(4)恢复旋转矩阵，并代入公式(2)，可获得平移矩阵

；

为提高估计精度和鲁棒性，滤除偏离较大的野值，采用归一化RANSAC算法，从两个视觉节点间所有视觉特征匹配点对中估计出

和

。所述的归一化RANSAC算法为成熟技术。

最后采用非线性最小二乘LM（Levenberg-Marquardt）算法同时优化

和

的估计值。所述的非线性最小二乘LM算法为成熟技术。

步骤(6)根据步骤(5)中获得的两个视觉节点间相对位姿，更新视觉节点内邻域关系表。

所述的节点内邻域关系表更新方法具体为：

每个节点内都建有一张二维的邻域关系表

，令

表示视觉节点m和n间的邻域关系，

和为步骤(5)中估计结果，有

。则

更新为：

                      

                        (6)

搜索整个邻域关系表，若邻域关系表中已知

，则可通过矩阵推导，同时更新

：

                  

               (7)

步骤(7)当某个视觉节点m发生运动，则该视觉节点进行自运动估计，确定视觉节点m运动后的新邻域节点，并根据新的邻域关系发送该视觉节点的网络定位数据包，更新邻域关系表。

所述的视觉节点自运动估计方法具体为：

视觉节点m根据步骤(1)到步骤(3)提取当前帧的网络定位数据包；

在视觉节点m中，采用与步骤(1)中相同的SIFT特征匹配方法，实现视觉节点m当前帧视觉特征和视觉节点m前一帧视觉特征的SIFT特征匹配；

采用与步骤(5)中相同的相对位姿估计方法，估计视觉节点m当前帧和前一帧间的相对位姿，即估计出视觉节点m在当前帧和前一帧间自运动的旋转矩阵

和平移矩阵

，有

，其中为视觉节点m当前帧视觉特征的三维坐标，

为视觉节点m前一帧视觉特征的三维坐标。

所述的视觉节点自运动后邻域关系表更新方法具体为：

在视觉节点m自运动估计后，搜索整个邻域关系表，若邻域关系表中视觉节点m和n在视觉节点m运动前的邻域关系

已知，则视觉节点m运动后的

为：

根据判断视觉节点m和n是否为视觉相邻，即视觉节点m和n在视野上存在较大范围重叠。判断依据为视觉节点m和n相对位姿中旋转角度不能超过

弧度（根据双目像机的有效视角选取，通常为

），平移距离不能超过

米（根据双目像机的有效视距选取，通常为5米）。

在确定视觉节点m与其他节点的邻域关系后，视觉节点m将当前帧的网络定位数据包发送给所有可能与视觉节点m存在视觉相邻的节点，并采用步骤(4)到步骤(6)的过程，更新邻域关系表。

Claims (1)

1.基于双目视觉节点的动态三维多媒体传感网络自定位方法，其特征在于该方法包括如下步骤：

步骤(1)网络中各视觉节点独立地提取该节点视野范围内的视觉特征；

所述的视觉特征提取具体方法为：

每个视觉节点采用一个已标定的双目立体像机，包含了左、右两个像机，可实时获取左、右立体图像；然后采用PCA-SIFT方法，分别提取左、右图像的SIFT特征描述符，每个特征描述符为36维矢量，每一维矢量用8位来表示；

采用KD-Tree方法实现左、右图像间的SIFT特征匹配，并记录每对SIFT特征匹配的匹配代价；

采用外极线约束滤除误匹配点；对于已标定的双目立体像机，外极线约束要求正确匹配点对在左、右图像中的垂直坐标差小于1；

以左图像为视觉特征提取的基准图像，将左图像划分为20×20个大小相等的区域，在每个区域中只选取一个具有最大匹配代价的SIFT特征作为该区域的视觉特征，即从左图像中最多可提取出400个视觉特征，并将这些视觉特征作为节点当前帧的视觉特征；因此每一个视觉节点从每一帧立体图像对中提取视觉特征的描述符最大数据量为400×36×8/1024=112.5kb；

步骤(2)网络中各视觉节点独立计算步骤(1)中所提取视觉特征的三维坐标；

所述的视觉特征三维坐标计算方法具体为：

步骤(1)中所提取视觉特征在左、右图像中的垂直坐标差小于1，而水平坐标差称为视差d，根据双目立体视觉三维重建公式，可得视觉特征i在左像机坐标系下的三维坐标(Xⁱ,Yⁱ,Zⁱ)：

$\left\{\begin{array}{c}{X}^i=b(u_i-u_0)/d_i\\ Y^i=b(v_0-v_i)/d_i\\ Z^i=\mathrm{bf}/d_i\end{array}\right.---\left(1\right)$

其中b，f，(u₀,v₀)分别为双目立体像机标定中获得的基线、焦距和左图像光心坐标；(u_i,v_i)为视觉特征i在左图像中的坐标，d_i为视觉特征i的视差；

步骤(3)网络中各视觉节点的网络定位数据包生成，具体为：

将步骤(1)和步骤(2)中提取的视觉特征描述符及其三维坐标打包作为视觉节点的网络定位数据包，并根据各视觉节点内的邻域关系表传递给邻域节点；

初始时各视觉节点内的邻域关系表为空，即各视觉节点间邻域关系未知，此时各视觉节点将其网络定位数据包通过“洪泛”传递给网络中所有视觉节点；

各视觉节点在接收到其他视觉节点的网络定位数据包后，更新其内部邻域关系表；随着各视觉节点内的邻域关系表逐步完善，即可通过邻域关系表估计出可能相邻的节点或仍未知关系的节点，则网络定位数据包将会有目的性地传递给这些相邻或仍未知关系的节点；最终待各节点内的邻域关系表完善后，每次网络定位数据包发送都将具有明确的目的节点，从而有效避免不必要的“洪泛”风险和能量损失；

步骤(4)网络中两个视觉节点间视觉特征匹配，具体为：

当一个视觉节点接收到其他视觉节点的网络定位数据包后，首先进行两视觉节点间的视觉特征匹配；采用与步骤(1)中相同的SIFT特征匹配方法；由于步骤(1)中视觉特征提取都是基于左图像的，因此类似于实现两视觉节点左图像间的SIFT特征匹配；

步骤(5)根据步骤(4)中得到的两个视觉节点间视觉特征匹配关系，估计两个视觉节点间的相对位姿，即获取两个视觉节点间的旋转矩阵和平移矩阵；

所述的两个视觉节点间相对位姿估计具体方法为：

令两个视觉节点m和n的视觉特征匹配点对的三维坐标分别表示为

和

根据视觉成像关系，有：

P_n=R_3×3P_m+T_3×1                     (2)

其中矩阵R_3×3和T_3×1分别代表两个视觉节点相对位姿中的旋转矩阵和平移矩阵；相对位姿估计可转换为采用最小二乘法求解下式最小：

${\left|\right|P_n-(R_{3\×3}{P}_m+T_{3\×1})\left|\right|}^2---\left(3\right)$

采用单位四元数q=[q₀,q₁,q₂,q₃]^T表示旋转矩阵R_3×3，用四元数加法和乘法代替旋转矩阵中的三角计算，可简化计算，并提高求解精度，有：

$R_{3\×3}=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]---\left(4\right)$

从而相对位姿估计中包含了7个自由度，理论上只需3对匹配点即可实现估计；为确保估计结果的鲁棒性，通常要求匹配点数大于某个阈值N，构建四元数的共生对称矩阵Q：

$Q=\underset{i}{\mathrm{\Σ}}\left[\begin{array}{cccc}{\tilde{x}}_m^i{\tilde{x}}_n^i+{\tilde{y}}_m^i{\tilde{y}}_n^i+{\tilde{z}}_m^i{\tilde{z}}_n^i& {\tilde{y}}_m^i{\tilde{z}}_n^i-{\tilde{z}}_m^i{\tilde{y}}_n^i& {\tilde{z}}_m^i{\tilde{x}}_n^i-{\tilde{x}}_m^i{\tilde{z}}_n^i& {\tilde{x}}_m^i{\tilde{y}}_n^i-{\tilde{y}}_m^i{\tilde{x}}_n^i\\ {\tilde{y}}_m^i{\tilde{z}}_n^i-{\tilde{z}}_m^i{\tilde{y}}_n^i& {\tilde{x}}_m^i{\tilde{x}}_n^i-{\tilde{y}}_m^i{\tilde{y}}_n^i-{\tilde{z}}_m^i{\tilde{z}}_n^i& {\tilde{x}}_m^i{\tilde{y}}_n^i+{\tilde{y}}_m^i{\tilde{x}}_n^i& {\tilde{x}}_m^i{\tilde{z}}_n^i+{\tilde{z}}_m^i{\tilde{x}}_n^i\\ {\tilde{z}}_m^i{\tilde{x}}_n^i-{\tilde{x}}_m^i{\tilde{z}}_n^i& {\tilde{x}}_m^i{\tilde{y}}_n^i+{\tilde{y}}_m^i{\tilde{x}}_n^i& {\tilde{y}}_m^i{\tilde{y}}_n^i-{\tilde{x}}_m^i{\tilde{x}}_n^i-{\tilde{z}}_m^i{\tilde{z}}_n^i& {\tilde{y}}_m^i{\tilde{z}}_n^i+{\tilde{z}}_m^i{\tilde{y}}_n^i\\ {\tilde{x}}_m^i{\tilde{y}}_n^i-{\tilde{y}}_m^i{\tilde{x}}_n^i& {\tilde{x}}_m^i{\tilde{z}}_n^i+{\tilde{z}}_m^i{\tilde{x}}_n^i& {\tilde{y}}_m^i{\tilde{z}}_n^i+{\tilde{z}}_m^i{\tilde{y}}_n^i& {\tilde{z}}_m^i{\tilde{z}}_n^i-{\tilde{x}}_m^i{\tilde{x}}_n^i-{\tilde{y}}_m^i{\tilde{y}}_n^i\end{array}\right]---\left(5\right)$ 其中

和

为P_m和P_n经数值解归一化，减去质心后的三维坐标；i为参与相对位姿估计的匹配点标号；

共生对称矩阵Q经SVD分解，其最大特征根所对应的特征向量即为单位四元数的最优解；根据公式(4)恢复旋转矩阵R_3×3，并代入公式(2)，可获得平移矩阵T_3×1；

为提高估计精度和鲁棒性，滤除偏离较大的野值，采用归一化RANSAC算法，从两个视觉节点间所有视觉特征匹配点对中估计出R_3×3和T_3×1；

最后采用非线性最小二乘LM算法同时优化R_3×3和T_3×1的估计值；

步骤(6)根据步骤(5)中获得的两个视觉节点间相对位姿，更新视觉节点内邻域关系表；

所述的节点内邻域关系表更新具体方法为：

每个节点内都建有一张二维的邻域关系表，令表示视觉节点m和n间的邻域关系，和

为步骤(5)中估计结果，有

则

更新为：

搜索整个邻域关系表，若邻域关系表中已知

则可通过矩阵推导，同时更新

步骤(7)当某个视觉节点m发生运动，则该视觉节点进行自运动估计，确定视觉节点m运动后的新邻域节点，并根据新的邻域关系发送该视觉节点的网络定位数据包，更新邻域关系表；

所述的视觉节点自运动估计具体方法为：

视觉节点m根据步骤(1)到步骤(3)提取当前帧的网络定位数据包；

在视觉节点m中，采用与步骤(1)中相同的SIFT特征匹配方法，实现视觉节点m当前帧视觉特征和视觉节点m前一帧视觉特征的SIFT特征匹配；

采用与步骤(5)中相同的相对位姿估计方法，估计视觉节点m当前帧和前一帧间的相对位姿，即估计出视觉节点m在当前帧和前一帧间自运动的旋转矩阵

和平移矩阵

有其中P′_m为视觉节点m当前帧视觉特征的三维坐标，P_m为视觉节点m前一帧视觉特征的三维坐标；

所述的视觉节点自运动后邻域关系表更新具体方法为：

在视觉节点m自运动估计后，搜索整个邻域关系表，若邻域关系表中视觉节点m和n在视觉节点m运动前的邻域关系

已知，则视觉节点m运动后的

为：

根据

判断视觉节点m和n是否为视觉相邻，即视觉节点m和n在视野上存在较大范围重叠；判断依据为视觉节点m和n相对位姿中旋转角度不能超过θ弧度，平移距离不能超过τ米；

在确定视觉节点m与其他节点的邻域关系后，视觉节点m将当前帧的网络定位数据包发送给所有可能与视觉节点m存在视觉相邻的节点，并采用步骤(4)到步骤(6)的过程，更新邻域关系表。

Images