python点云快速配准_分享一个V-SLAM中点云配准算法改进的方法

近年来，随着自主导航的广泛应用，视觉同时定位与地图构建(Visual simultaneous localization and mapping, V-SLAM)通过自身携带的视觉传感器对周围环境进行重构并且估计出自身的位置，准确定位与高精度的V-SLAM算法是实现自主导航的关键技术。

V-SLAM一般分为前端视觉部分和优化的后端部分基于特征点法的前端视觉部分，一直以来都是视觉里程计(Visual Odometry，VO)的主流方法，它的任务是估算相邻图像之间的相机运动，给后端优化提供较好的初值，后端则处理地图模型的优化问题，依据前端视觉里程计测量的相机位姿，进行优化处理得到全局一致的轨迹和地图，视觉里程计的实现方法，应用最广泛的是通过ICP算法对相机进行运动估计，传统ICP算法是建立在配准点集间完全包含的理论假设，然而实际点云配准中待配准点集间具有初始位姿偏差大，导致迭代精度不高，使得ICP算法陷入局部最优；匹配过程中随着待匹配点对数量的增多，导致误匹配累积增大，严重影响配准效果针对ICP存在的问题，研究者们进行了多种算法的改进，文献提出一种针对家庭室内环境常见物体的三维建模方法，该方法通过手持相机环绕进行采集帧数，可以进行三维模型的重构，但是无法估计相机的位姿，后有文献提出了一种快速三维SLAM方法，在对应点对的筛选策略上仅使用RANSAC算法，对特征点进行随机配准，提高了算法的实时性和效率，但配准结果无法满足全局要求，易陷入局部最优使配准效果不好，届时有文献提出了一种通过图像配准的3D室内环境重建方法，采用尺度不变特征变换和随机采样一致性算法确定相邻帧，将得到的变换矩阵作为精配准的初始点云数据该方法可以进行环境地图重构，但是过度依赖图像的帧序列，难以满足真实环境下智能体的需求，而且无法估计移动机器人的位置和姿态。

针对以上研究现状，本文提出了一种三阶段点云配准的视觉SLAM方法，通过预处理、初配准、精配准三个阶段对点云的重构和位姿估计进行处理，在预处理阶段，选用RANSAC算法通过设定欧氏距离阈值筛选可靠内点来进行初始位置估计；将预处理的结果作为初值，通过基于刚体变换一致性的双重距离阈值法完成初配准；在具有良好点云初始位置的条件下进行精配准，引入动态角度迭代因子，逐步减小误匹配，在后端优化的关键帧选取中采用滑动窗口法和随机采样法相结合的方法，实现三维地图的更新优化。

首先来看一下三维点云生成——Kinect标定

为了生成三维点云，需要获取Kinect的内参矩阵来进行RGB相机与深度相机之间的刚体变换，对RGB摄像机和深度相机进行标定，利用视差补偿模型对深度相机的畸变进行校正，RGB图像与Depth图像联合标定前如下图 (a)、标定后下图(b)比较情况如下图 所示：

RGB和Depth数据对准与点云生成

在Kinect相机完成标定后，通过RGB图的像素坐标结合该点的Depth数据，就可以求得每个点的空间位置，从而可生成三维点云数据.空间点[x, y, z]和其在图像中的像素坐标[u, v, d] (d指Depth数据)具有如下对应关系：

式中：fx、fy分别代表相机在x、y轴线上的焦距；cx、cy代表相机的光圈中心，s是深度图的缩放比例因子(Depth数据与实际距离的比值)，本文取值为1000.通常情况下fx、fy、cx、cy被定义为相机的内参矩阵C：

再来看看三阶段点云配准

1、预处理

ORB特征是目前最具代表性的实时图像特征，它改善了快速检测子FAST不定向的问题，并使用快速二进制描述子BRIEF，加快整个图像的特征提取.提取ORB特征分为如下两个步骤：

1) FAST角点提取：找出图像中的“角点”

2) BRIEF描述子：对提取的特征点周围图像区域进行描述

在提取了ORB图像特征之后进行点集元素的特征匹配， SLAM中的数据关联问题通过特征匹配来解决，即确定当前时刻看到的路标与之前看到的路标之间的对应关系，匹配帧间的图像描述子，可以为后续的姿态估计、优化等减轻工作量.为了符合在SLAM中的实时性需求，本文采用适合匹配点数量极多的FLANN(FAST Library for Approximate Nearest Neighbour，快速近似最近邻)算法进行双向匹配。

本文采用RANSAC算法对误匹配进行剔除，对两组相邻的RGB图的特征点对进行预处理，设定一个阈值d，剔除大于阈值的匹配点对，得到符合的内点(真实对应)，针对源点集A和目标点集B上的特征点集，d1和d2分别为A上一点在目标点集上的最近和次最近点的欧氏距离，依据工程上的经验方法在本文中设定d2 < αd1(其中α=0.5), 取内点数量最小阈值为N=13，若不满足则说明当前RGB图不满足本阶段要求，进行下组RGB图预处理.通过对内点进行最小二乘法的位姿估计，可对源点集A和目标源点集B进行一个微调，预处理后的两片点云分别记为P和Q.相机第i时刻的位姿pi和第j时刻相机的位姿pj的位姿变换关系为: pj= pi Tij，其中

2、改进点云初配准

ICP算法用于精配准，需要满足配准点云之间初始位置相近的条件，否则由于其对初值的极大依赖使得迭代易陷入局部最优，仅仅依靠第一步的预处理，并不能保证待配准点云初始状态比较接近，所以不能直接使用ICP算法进行精配准，需要在预处理的基础上对点云进行一次初配准，初配准的结果作为精配准的初始值使其具有良好的初始状态，初配准的原理图如下图所示

初配准原理示意图

同一片点云任一点与其邻点的拓扑结构保持不变，具有刚体变换一致性.因此对于点云P中任意一点pi与其近邻点p′i，在配对点云Q中也应是近邻点.在预处理的基础上获取良好的初始状态，若获取的匹配点对是正确的匹配点对，则对于任意两点对(pi，qj)和(p′i，q′j)应满足点对间的距离相等，如式两个约束条件：

式中：pi与p′i分别表示点云P中的任意一点与其近邻点，qj与q′j分别表示pi与p′i在其配对点云Q中的对应匹配点. u1、u2为距离阈值，经过初配准可以剔除大部分噪声点，获得具有良好初始状态的配对点云S与L。

3、 改进点云精配准

ICP匹配算法基于两个点云完全重合的假设，采用全局搜索实现匹配点搜索.随着地图规模的扩大，全局搜索将带来计算量的增加.另一方面实际匹配过程中两片点云并不是全部点一一对应，存在大量的误匹配点对，因此会导致容易陷入局部最优，甚至收敛失败，所以比较好的点云初始位置，能更好地实现精配准.本文采用ICP算法的精配准目标是为了高效地配准具有良好初始状态的三维点云，从而得到一个旋转矩阵和平移向量.传统的ICP算法通过最小二乘优化思想求解式，使其中的R和T最小化。

式中：n为匹配点对的数量，R为3×3的旋转矩阵，T为×1的平移矩阵，Si和Li分别为初始和目标点集。

本文采用改进ICP算法完成精配准，其原理图如下图所示，完成匹配点对的搜索后，通过最小二乘优化误差方程得到刚体变换矩阵，当ICP算法完成一次迭代之后，初始点云和目标点云的匹配可以更加准确。

精配准原理示意图

这意味着点云S与其匹配点云L之间的偏移角减小，因此下一次ICP匹配点搜索可以缩小搜索范围，不但减少了误匹配，而且减少了搜索计算量.经过初配准的处理，待匹配点云之间的初始位置良好，再结合动态角度迭代因子，ICP算法每实现一次迭代，点对之间的法向量夹角会更小，通过动态阈值减小错误迭代次数，避免局部最优和获得更好的位姿估计精度.本文将选择匹配点对之间的方向向量夹角的正弦值作为匹配点误差方程，匹配点的正弦值如式:

式中: ni和nj分别为点si与lj的近似法向量，θ为两匹配点对的法向量夹角.匹配点的权重由式计算：

式中E为动态角度迭代阈值匹配点的数量为

本文改进后的ICP精配准步骤如下：

Step 1 初配准获得初始对应点集Si0与Li0；

Step 2 由动态角度迭代阈值法进一步获得精配准点集Si1={s1, …, sn1}与Li1={l1, …, ln1}；

Step 3 采取SVD求解点集Si1与Li1之间的旋转矩阵R和平移向量t；

Step 4 根据式Si2=R1Si1+t1, 计算点集Si1经过一次迭代变换后的数据点集Si2；

Step 5 完成一次迭代后，E通过下式被缩小。

然后重复Step 2~Step 5，直到满足

最后我们来看看它的后端优化

1、关键帧筛选

相机行进中会获取室内的环境信息，若将全部采集帧的图像位姿估计都作为状态变量传递给后端优化，将导致后端优化中重建模型的规模急剧变大，程序占用的内存也会越来越高.于是需要从采集帧中选取代表性的图像作为关键帧，而系统的精度与关键帧的选取有很大关系，当关键帧的选取十分稠密时，会增加回环检测与全局优化的计算量，并且会增加很多的冗余信息，无法满足实时性的要求；而当关键帧的选取比较稀疏时，则帧间配准容易失败的情况增加，甚至导致跟踪失败.为了满足帧间配准的成功率和系统实时性要求，对于当前帧，本文将采用下图所示的滑动窗口法与随机采样法相结合的策略，选取相邻的m帧，以及随机采样的n帧作为关键帧。

本文在随机采样时，考虑机器人在较短时间内返回当前位置的可能性较小，因此过去时间较长的帧选取的可能性大，过去时间较短的帧选取的可能性小，若当前时刻有数量为N+m的帧，去除滑动窗口法选取的最近邻m帧，还剩N帧.那么第i帧被选中的概率为P(i)如下：

2、 位姿优化

由上文得到机器人在不同时刻的帧间变换关系，各时刻相对于初始时刻的变换关系如下式:

然而，相邻帧间变换算法得到的矩阵Tk-1k是存在误差的，所以机器人位姿会产生累积误差，随着时间的推移，累积误差会越来越大，导致无法构建全局一致的地图，本文采用g2o优化对位姿变换过程中的累积误差进行优化，它将SLAM问题求解转化为基于最小二乘法的优化问题：

式中：xi代表机器人在g2o图中第i个节点的位姿; zij为节点xi和xj之间的边，也就是位姿xi和xj之间的约束即变换矩阵Tij; Ωij表示xi和xj之间的信息矩阵; e(xi, xj, zij)是g2o图中表示定点xi和xj满足约束zij的程度，若位姿xi和xj完全满足约束zij，那么e(xi, xj, zij)的值为0，即无需调整位姿xi和xj。

不同于传统点云匹配方法，本文通过对RGB图的彩色信息进行处理来获取初始匹配内点进行微调；在不满足精匹配初始位姿的要求下，采用匹配点对的双重距离阈值法完成点云的初配准，从而获取良好初始位姿；引入动态迭代角度因子改进ICP来进行点云的精配准，通过误差的判定决定是否继续迭代下去，每实现一次迭代都会缩小搜索范围并且更加准确地剔除误匹配，从而满足V-SLAM系统对精度的需求，具有重要的现实意义。

今天分享的v-SLAM中点云配准算法，只是SLAM研究领域的一部分，如果在SLAM方面你想有更全面，更深入的研究，可以选择在已经开发好的SLAM平台上进行项目研究。

已经开发好的SLAM研究平台，操作简单，适用性强，相关配套视频教材及例程可快速帮助研发者投入项目研究。

先来看看它的系统组成方面

1)无人机平台

可提供多种无人机飞行平台，飞行平台集成光流、激光定高等传感器，均可稳定悬停。飞行平台均提供完善的二次开发接口，支持C++、Python、ROS、Matlab等多种开发接口。

2)视觉传感器

支持Intel D435i、小觅等多种双目视觉相机。

可根据客户需求选配可见光相机、激光雷达、红外相机等多种其他类型的传感器。为客户提供完善的驱动程序支持和二次开发接口。

3)机载计算机

可搭载TX2、树莓派、Intel NUC等多种机载计算机。

4)开发工作站

提供双目视觉slam实时状态可视化界面和自主导航避障轨迹显示界面。

5)视觉导航集成开发环境

提供全套基于机器视觉无人平台自主导航避障系统软件开发体系：包括VIO定位开发框架，MAVROS无人机二次开发SDK、自主导航避障框架、视觉仿真环境等。

6)实验指导书

提供航线飞行和自主导航避障案例以及常见故障解决方案。

再来看看平台具备的特点及应用价值

1) 紧扣目前无人机研究的热点，提供视觉导航、SLAM、视觉避障、人工智能等领域最新的技术。

2)该系统提供全套的学科教学课程、示例算法，包括课程PPT及例程操作指导视频等，可快速展开相关课程设计，进行实际教学应用阶段。而且平台由硬件平台和软件平台组成，可实现slam理论教学和硬件实操。

可开设实验课程如下：

⦁SLAM教学:

⦁飞思视觉slam硬件平台介绍

⦁飞思视觉slam软件平台介绍

⦁相机原理、点云原理、GPU加速原理简介。

⦁飞思视觉slam平台支持的视觉slam架构，包括VINS，ORB_SLAM和Rovio，降低学校教师或学生学习SLAM的门槛。

⦁视觉slam主流技术介绍，包括多传感器融合算法，KLT光流算法双目VIO

⦁视觉词袋技术介绍

⦁回环检测技术介绍，包括重定位技术、全局位姿估计技术

⦁无人机通信技术，包括Mavlimk介绍和Mavros介绍

⦁自主导航避障算法介绍

⦁飞思视觉slam平台应用实例介绍。

⦁航线飞行实例，自主导航避障实例

3)提供全套的机器视觉与视觉导航的开发环境、仿真环境、硬件平台，所提供的硬件均提供完善的二次开发接口和实例代码，省去用户从头搭建开发平台的繁杂工作。

4)提供完整的无人平台控制、通讯链路、视觉图像采集、图像识别、三维重建、定位解算等软件算法解决方案。提供了VIO、LocalPlanner等多种实例程序，用户只需要简单配置即可实现完整的自主定位、自主建图、自主导航、自主避障等功能。

针对SLAM技术我们后续还有更多精彩分享，欢迎大家留言关注！