[发明专利]一种基于点线综合特征的视觉同时建图与定位方法

权利要求书

1.一种基于点线综合特征的视觉同时建图与定位方法，其特征在于，包括离线建立视觉词典和在线建立稀疏视觉特征地图两个部分：

首先，利用聚类方法离线建立树状的视觉词典，即描述子空间的KD树，并确定树状视觉词典中每个节点的逆文本频率，所述每个节点即为描述子的聚类中心：

将每帧图像包含的特征转化为视觉词汇，即特征描述子；对所述视觉词汇进行分层聚类，建立描述子空间的KD树，该KD树称为视觉词典；使用特征描述子离线建立树状的视觉词典，将特征描述子的训练图像集中提取出来；所述描述子是ORB，定向快速角点检测和二进制鲁棒独立基本特征描述子点特征描述子和LBD直线特征描述子；所述ORB点特征描述子和LBD直线特征描述子均为二进制描述子，把两种二进制描述子分别进行拓展：为ORB点特征添加标志位0，为LBD线特征添加标志位1，所述标志位0和标志位1可区分直线特征与点特征；获取LBD直线特征描述子之前首先要用LSD检测直线，再用LBD描述子对该直线进行描述；

视觉词典中每个节点的权重由该节点包含的所有特征描述子的逆文本频率确定；

然后，在线建立稀疏的视觉特征地图，步骤如下：

步骤一，从双目相机中获取校正后的图像，并对校正后的图像的特征提取及描述：

提取校正后的图像中的点线特征及其描述子，在线提取ORB点特征描述子和LBD线特征描述子；

步骤二，双目相机中校正后的图像进行特征匹配及三维重建：

匹配校正后的图像中的特征点和特征直线，建立匹配对，利用双目视觉成像模型把特征点和特征直线进行三维重建，在重建中用普吕克坐标来表示特征直线，并维护直线的端点，用特征点和特征直线建立综合点线特征的稀疏特征地图，采用普吕克坐标用于直线的表示和计算；

步骤三，前后帧图像匹配、局部地图匹配及相机的运动估计：

在重建出三维空间中的特征点和特征直线后，对这些点和线进行跟踪匹配，匹配包括两部分：前后图像匹配以及局部地图匹配，所述前后帧图像匹配用于估计当前时刻相机的位姿，

求解位姿的过程如下，假设当前时刻左相机坐标系O_c在世界坐标系O_w中旋转和平移分别为R_wc和t_wc，重构的特征点j在世界坐标系O_w的坐标为Pj_w，则该特征点在当前时刻左相机坐标系O_c下的坐标Pj_c为：

Pj_c＝R_cwPj_w+t_cw

重构的特征直线i在世界坐标系O_w的坐标为Li_w，Li_w＝[n^T,v^T]^T，则该特征直线在当前时刻左相机坐标系O_c下的坐标为：

其中R_cw＝R_wc^T，t_cw＝-R_wct_wc，分别为世界坐标系在左相机坐标系中的旋转和平移，[t_cw]_×是由向量t_cw构成的3×3的反对称矩阵；将特征点Pj_c通过针孔相机模型投影到当前左相机中，得到其投影的图像坐标将特征直线Li_c投影到当前左相机中，得到其投影直线方程为l_i；分别定义点和线特征的误差，点的误差为重投影误差，即特征点投影坐标与观测坐标p_j之间的距离e_pj；线的误差为观测线段的两个端点ep1_i、ep2_i到投影直线方程的几何距离e_li；运动估计的目标是求解以下非线性最小二乘问题：

即求解当前相机的姿态使得特征点和特征直线的重投影误差最小；其中α、β为点特征重投影误差和线特征重投影误差的权重值，α、β是两个常数为了剔除错误图像特征匹配的影响，在优化过程中可采用Ransac方法得到运动估计的解；

步骤四，利用步骤一中得到的视觉词典做回环检测：

对视觉关键帧提取得到的点、线特征描述子，建立图像数据库，所述图像数据库包含每一关键帧中的点、线特征描述子，根据建立的视觉词典，把图像的特征转换成词包向量，其中词包向量包含图像中每个视觉词汇的TF-IDF分数，TF表示词条在一帧图片中出现的频率，IDF为上文所述的逆文本频率，TF-IDF表示TF与IDF的乘积，如果一个视觉词汇在同一帧图像中出现的频率越高TF-IDF分数就越高，但是在整个图像数据库中出现频率越高TF-IDF分数会越低；

评价两幅图像的相似性时，根据提取到的特征把图像转换成词包向量，然后根据词包向量计算相似度得分，对当前相机采集的图像与图像数据库中的图像进行比较，得分较高为同一个位置采集得到的图像，即为一个闭环，表示之前已经访问过这个位置，再利用几何一致性，即两帧图像中有足够多的匹配对支持欧氏变换，时间一致性，即两帧图像前后的若干图像序列也应该很相似，进一步判断是否为闭环；

步骤五，把步骤二到步骤四得到的点线特征、相机运动估计、闭环检测放入基于关键帧的图优化框架中，在后端的优化中采用直线的正交表示来最小化直线的参数个数；在图优化框架中优化相机的位姿和特征点和线的位姿，实现相机的定位与在线稀疏视觉特征地图的构建。