1、中国民航大学学报 2017,05(35),47-51 视觉 SLAM 中三维点云配准的 Cayley 方法 张鸿燕 王婧妍 周璐莎 王炳辉 中国民航大学中欧航空工程师学院 导出/参考文献 关注 分享 收藏 打印 摘 要： 利用三维点对应关系估计摄像机的位姿矩阵是同步定位与地图构建 (SLAM) 系统的一个基本问题。由于系统的误差累积会导致点云尺度的漂移, 三维点对应关系在误差干扰下从欧式变换退化为相似变换, 在计算过程中需考虑尺度因子。利用 Cayley 变换来估计三维对应点之间相似变换的闭式算法, 可方便地确定定位与构图过程中摄像机的位姿变化。仿真实验和误差分析验证了基于 Cayley 变换
2、的配准算法的准确性与鲁棒性。该算法不仅可用于 SLAM 系统, 还适用于计算机视觉中的三维重建与三维拼接问题。关键词： 同步定位与地图构建; 点云配准; 相似变换; Cayley 变换; 作者简介：张鸿燕 (1978) , 男, 湖南岳阳人, 讲师, 博士, 研究方向为计算机视觉.收稿日期：2017-01-10基金：国家自然科学基金项目 (U1633101) Cayley approach for 3D points registration in visual SLAMZHANG Hongyan WANG Jingyan ZHOU Lusha WANG Binghui Sino-Europe
3、an Institute of Aviation Engineering, CAUC; Abstract： An essential problem in SLAM system is to compute the 3D rigid body transformation of cameras, which aligns two sets of points when correspondences are known. The errors accumulated during the locating and mapping process result in the drift of s
4、cale for the pairs of points. The Euclidean transformation will be degraded into the similarity transformation due to the drift. A closed-form solution using Cayley transform is presented to estimate the similarity transformation between 3D corresponding points. The accuracy and robustness of the pr
5、oposed Cayley approach are verified by simulation and error analysis. Apart from SLAM system, this approach is also suitable for 3D reconstruction and registration in computer vision.Keyword： SLAM; 3D registration; similarity transformation; Cayley transform; Received： 2017-01-10由三维对应点估计刚体的三维运动1-3,
6、即刚体运动估计问题, 被广泛应用在姿态估计、图像配准、目标运动识别、机器人学等领域。该问题的本质就是估计运动过程中的姿态变换矩阵4其中:R 为旋转矩阵;SO (3) 为三维旋转群;t 为平移向量;s 为尺度因子。一般地, s 为正数, T 矩阵是摄像机姿态变换 (相似变换) 矩阵;特殊地, s=1 时, T矩阵是刚体运动变换 (欧式变换) 矩阵。在视觉同步定位与地图构建 (SLAM, simultaneous localisation and mapping) 系统中, 摄像机运动姿态 (R 与 t) 实时估计是关键环节5-6。当视觉系统获得不同局部坐标系下的三维点云, 即可估计摄像机的姿态变
7、换矩阵, 进而将点云配准到全局坐标系下。若系统所得点云无先验对应关系, 通常选用迭代最近点 (ICP) 1,7算法进行配准, 该算法计算复杂度高, 用于实时处理时需硬件加速。对于无需硬件加速的 SLAM 系统, 如 ORB-SLAM8, 一般根据稀疏点云之间的对应关系估计三维点云配准矩阵。针对三维对应点的配准矩阵计算, 经典的估计算法包括 Arun9提出的奇异值分解法、Horn10提出的正交分解法和单位四元数法11以及 Walker12提出的对偶四元数法。Eggert 等13分析了这 4 种算法的准确性、稳定性及效率, 同时指出, 无论从鲁棒性、效率还是收敛性考虑, 闭式解 (解析解) 都优于
8、迭代解。运动状态估计问题必然会涉及到旋转矩阵的计算。这些经典算法均采用Euler 角、四元数或 Rodrigues 公式来表达三阶旋转矩阵。当数据误差较大时, 得到的姿态可能是一个正交矩阵而非旋转矩阵。吴福朝14提出将 Cayley 变换应用于欧式变换估计, 揭示了 Cayley 变换在摄像机姿态估计中的优越性。Cayley 变换及旋转矩阵的 Cayley 表达是数学家 Cayley15于 1846 年提出的, 但在计算机视觉领域一直未引起足够的注意。Cayley 方法的优点在于直接提供了一个旋转矩阵, 无须校正与近似, 而且只需 3 个对应点就能提供线性解16。对于多数单目 SLAM 系统,
9、 由于缺乏绝对尺度信息, 系统运行的累积误差会导致点云尺度发生漂移8, 从而使得对应点无法正常匹配。此时, 三维点对应关系在误差干扰下已不是欧式变换而是相似变换。针对尺度漂移问题, ORB-SLAM 系统采用 Horn11提出的基于单位四元数的闭式算法, 用四元数表达旋转矩阵, 转化过程复杂, 且在数据误差较大的情况下给出的可能不是旋转矩阵。针对三维对应点的相似变换问题, 提出基于 Cayley 变换的闭式配准算法, 避免了复杂的迭代过程, 并直接给出旋转矩阵, 使得不同局部坐标系下的点云配准在统一的坐标系与尺度之下。1 点云配准与旋转矩阵的 Cayley 表达已知 SLAM 系统在相邻 k-
10、1 和 k 时刻构建的一组三维对应点为x k-1Ni=1和x kNi=1, 点云配准算法根据点对应关系来计算两帧图像 Ik-1和 Ik所确定的摄像机姿态变换矩阵4,17其中:R kSO (3) 为旋转矩阵;t kR31 为平移向量;s k为尺度因子。考虑无系统误差的刚体运动, s k=1, 三维对应点 xk-1和 xk满足如下关系4然而实际系统运行时存在累积误差, 这会导致点云的相对尺度发生漂移, 此时尺度因子 sk1, 点对应关系转化为18令 Xk是三维空间点 xk的齐次坐标, 点云之间的关系也可表示成此时 Tk为点云配准矩阵, 其为相似变换矩阵。为了加以区别, 采用矩阵符号 Sk代替 Tk
11、。三维点云坐标的配准如图 1 所示。图 1 三维点云坐标配准 Fig.1 3D points registration 下载原图求解摄像机的位姿变换矩阵时, 采用 Cayley 表达来表示旋转矩阵。Cayley 变换是反对称矩阵与旋转矩阵之间的一对一映射。给定反对称矩阵 W, 则存在一个唯一的三维向量 w=w1, w2, w3满足旋转矩阵可由三维向量 w 表示成15-16其中:向量 w 称为旋转矩阵 R 的 Cayley 表达。由于本文只考虑相邻两帧图像确定的摄像机姿态变换矩阵, 也就是计算一组三维对应点的坐标配准矩阵, 为简化符号描述, 省去上下标 k-1 和 k, 并记 xk-1为 xi,
12、 xk为 yi, Si为 S。2 点云配准的 Cayley 方法假设一组三维对应点 , 其关系为其中:s 为尺度因子;t 为平移向量;R 为旋转矩阵。由于实际系统存在噪声, 式 (8) 并不成立, 而变成一个超定方程组。假设误差为位姿变换参数 (R, t, s) 的估计问题转化为误差函数最小化问题其中: 是 Frobenius 范数, vR。为得到较好的三维点云配准估计算法, 先对原始三维点云数据进行 0 均值化, 以简化误差函数的描述。2.1 数据的 0 均值化数据的 0 均值化操作主要分为两步:首先计算点云的质心坐标 , 分别表示为然后将点集中的各个元素减去质心坐标, 得到各个点与质心之间
13、的偏移距离, 形成新的点云集合X ii=1和Y ii=1, 其中新生成的对应点之间的几何约束关系为2.2 误差函数的等价描述对于新的对应点集, 其误差表示为令 , 那么误差函数可转化为由于 , 式 (15) 的第 2 项为 0。为使误差最小化, 需令式 (15) 的第 3 项也为 0, 即因此, 可得平移向量的估计公式误差函数可简化如下2.3 姿态估计的 Cayley 方法由于旋转矩阵给出等距变换, 可得RX i=X i。因此当范数X i和Y i已知时, 可按下式来估计尺度因子然后将式 (7) 代入式 (13) , 可得已知尺度因子的估计值, 式 (19) 便可写成令 , 然后应用等式由式 (
14、20) 与式 (21) 可得方程组解得 w 的最小二乘解为最后根据式 (7) 估计旋转矩阵 R。于是可得姿态变换参数的最优解为基于 Cayley 变换的三维点云配准算法总结如下:算法 1 三维点云配准矩阵估计的闭式算法。Require:一组三维对应点Ensure:摄像机运动的位姿变换参数 (R, t, s) 1) 根据式 (11) 和式 (12) 对三维点云进行 0 均值化4) 利用式 (23) 计算 R 的 Cayley 向量 wLS3 实验结果及分析仿真实验采用的数据来自于 Vtk 库19中的三维恐龙状点云数据集x ii=1, 点数目 N=10 755。随机生成姿态变换参数 (R, t,
15、s) , 对原始数据集进行三维相似变换, 变换后的点云集记为y ii=1。为验证算法 1, 姿态变换参数设定如下原始点云如图 2 (a) 所示。仿真中, 为两组点云添加均值为 0, 标准差分别为3.721 1 m 和 3.490 8 m 的高斯噪声, 得到新点云x i i=1和y i i=1, 如图 2 (b) 所示。图 2 加噪前后的三维对应点表示 Fig.2 Description of point pairs 下载原图针对添加噪声后的三维对应点 , 由算法 1 估计点云配准矩阵 , 得到姿态变换参数此时, 将相似变换后的点云y i i=1通过矩阵 映射到x i i=1所在的坐标系, 可得
16、点云集x i i=1, 然后观察配准后的两组点云集的重合情况。从图 3 可看出三维对应点 点云配准矩阵进行变换后, 两组点云集基本保持重合, 从而验证了基于 Cayley 变换的闭式配准算法的有效性。从旋转矩阵的误差和平移距离误差来进一步分析算法 1 的位姿估计精度。旋转矩阵角度的误差计算定义为图 3 一组含噪三维对应点的匹配结果 Fig.3 Registration of 3D correspondence with noise 下载原图其中: k是旋转矩阵 R 的第 k 列。平移向量估计值与真实平移向量的距离被定义为平移距离误差, 即考虑 Cayley 方法的鲁棒性和准确性, 进行多组独立
17、的实验。设定不同尺度因子s 并随机生成变换参数 (R, t) , 然后对初始点云进行相似变换并生成多组三维对应点, 添加与实验示例相同强度的高斯噪声。接着用 Cayley 方法估计姿态变换参数, 最后计算估计误差, 所得结果如图 4 所示。图 4 Cayley 方法的姿态估计误差 Fig.4 Error of estimated pose via Cayley approach 下载原图由图 4 可知, 在不同变换参数条件下, 利用 Cayley 方法计算出的旋转角度误差不超过 2, 平移估计相对误差小于 0.05%, 这说明 Cayley 方法的位姿变换参数估计误差小、精度高。考虑点云配准的
18、准确性, 计算配准后的两组点云的平均距离为同样, 随机设定变换参数 (R, t, s) 生成 50 组三维对应点, 添加的噪声强度保持不变。然后采用算法 1 对三维点云进行配准, 最后计算配准后的点云集之间的平均绝对误差 edistance=5.734 m。已知实验采用的点云集规模是 89.64 m197.62 m166.94 m, 且每组对应点分别加了标准差为 3.721 1 m 和 3.490 8 m 的高斯噪声。在此实验条件下, 配准后的点云集之间的绝对误差在可接受范围内。4 结语针对 SLAM 中的地图点云尺度漂移问题, 本文提出了利用 Cayley 变换来估计三维对应点之间的相似变换
19、的闭式配准算法, 从而确定相邻两帧图像的摄像机姿态变换矩阵。该算法是一个直接提供位姿变换参数最优解的闭式算法, 无须迭代与校正, 计算效率高, 对 SLAM 的实时性至关重要。另外, 该方法采用三维向量来表达姿态矩阵中的旋转矩阵, 在仅有 3 个对应点的情况下便可提供线性解, 而且其算法结构比较简单, 容易实现。同时, 本文进行了多组随机并独立的仿真实验, 通过误差分析来说明基于Cayley 变换的闭式算法在三维点云配准问题中的优越性。实验结果表明, 无论是旋转矩阵误差、相对平移误差还是配准后的点云集之间的距离误差都比较小, 进而证实了该算法具有较高精度和较强的鲁棒性。最后要强调的是, 基于
20、Cayley 变换的闭式配准算法的适用性较广, 不仅可以解决 SLAM 系统的点云配准问题, 而且适用于计算机视觉和机器人学中的三维重建与三维拼接问题17-18。参考文献 2BESL P J, MCKAY N D.Method for registration of 3D shapesJ.IEEE Transactions on Pattern Analysis and Machine Intelligence, 1992, 14 (2) :239-256. 3HENRY P, KRAININ M, HERBST E, et al.RGB-D mapping:using Kinect-styl
21、e depth cameras for dense 3D modeling of indoor environmentsJ.International Journal of Robotics Research, 2012, 31 (5) :647-663. 4HARTLEY R, ZISSERMAN A.Multiple View Geometry in Computer VisionM.2nd ed.Cambridge:Cambridge University Press, 2003. 5王忠立, 赵杰, 蔡鹤皋.大规模环境下基于图优化 SLAM 的图构建方法J.哈尔滨工业大学学报, 201
22、5, 47 (1) :75-85. 6顾照鹏, 刘宏.单目视觉同步定位与地图创建方法综述J.智能系统学报, 2015 (4) :499-507. 7IZADI S, NEWCOMBE R A, KIM D, et al.Kinect Fusion:Real-time Dynamic 3D Surface Reconstruction and Interaction Using a Moving depth CameraC/International Conference on Computer Graphics and Interactive Techniques, Santa Barbara
23、, CA, Oct 16-19, 2011:1175-1184. 8RAUL M A, MONTIEL J M M, JUAN D T.ORB-SLAM:a versatile and accurate monocular SLAM systemJ.IEEE Transaction on Robotics, 2015, 31 (5) :1147-1163. 9ARUN K S, HUANG T S, BLOSTEIN S D.Least-squares fitting of two3-D point setsJ.IEEE Transactions on Pattern Analysis and
24、 Machine Intelligence, 1987, 9 (5) :698-700. 10HORN B K P, HILDEN H M, NEGAHDARIPOUR S.Closed-form solution of absolute orientation using orthonormal matricesJ.Journal of the Optical Society of America A, 1988, 5 (7) :1127-1135. 11HORN B K P.Closed-form solution of absolute orientation using unit qu
25、aternionsJ.Journal of the Optical Society of America A, 1988, 4 (4) :629-642. 12WALKER M W, SHAO L, VOLZ R A.Estimating 3D location parameters using dual number quaternionsJ.CVGIP:Image Understanding, 1991, 54 (3) :358-367. 13EGGERT D W, LORUSSO A, FISHER R B.Estimating 3D rigid body transformations
26、:a comparison of four major algorithmsJ.Machine Vision and Applications, 1997, 9:272-290. 14WU Fuchao, WANG Zhiheng, HU Zhanyi.Cayley transformation and numerical stability of calibration equationJ.International Journal of Computer Vision, 2009, 82 (4) :156-184. 15CAYLEY A.Sur quelques proprietes de
27、s determinant gauchesJ.Journal Fur Die Reine Und Angewandte Mathematik, 1846 (32) :119-123. 16吴福朝.计算机视觉:Cayley 变换与度量重构M.北京:科学出版社, 2011. 17张鸿燕.基于视频图像的三维建模方法与系统消化道三维建模方法与应用研究D.北京:中国科学院研究生院, 2011. 18周璐莎.大尺度场景的同步定位与地图构建D.天津:中国民航大学, 2016. 19A Small Program for Rendering a Point Cloud Using VTKEB/OL.2017-06-14.http:/
