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数据，就可以求得每个点的空间位