[发明专利]一种基于直线特征与点云特征结合的位姿估计方法

摘要

本发明公开了一种基于直线特征与点云特征结合的位姿估计方法，包括：(1)融合先验知识的直线特征提取；(2)双目图像中的直线匹配；(3)直线特征的三维重建；(4)位姿计算。本发明的点云来自边缘特征，具有良好的抗干扰能力和准确的定位能力，且用点云匹配代替线段匹配可充分发挥点云匹配的鲁棒性优势，即使线段出现长度变换或断裂等现象，变成点云后仍能有效匹配；点云数量有限，覆盖区域是空间有限的线段集，这使得点云数量大大减小，提高了匹配速度，但同时点云又都是来自对定位贡献最大的物体边缘，定位精度并不会明显下降；直线特征提取及匹配过程无需致密深度场信息，针对复杂纹理以及简单纹理的物体均能保证精度。

主权项

1.一种基于直线特征与点云特征结合的位姿估计方法，其特征在于，包括如下步骤：步骤1：融合先验知识的直线特征提取与合并；步骤2：双目图像中的直线匹配：左右目图像中直线集合和取左图中的直线，从到依次与右目中的直线计算，则有公式：其中，Mark为两条直线匹配的评分值，在初始的Mark_final＝Mark₀＝100，Mark_i为某一过程中的评分值，f为指定的各项评分条件，包括直线的夹角，水平约束，左右目视差；在左右目图像的直线集合中分别取两条直线(l代表左目图像)(r代表右目图像)，带入直线夹角约束、水平约束和左右目视差约束中；通过以上步骤，得到Mark的最终值，比较Mark_i与Mark_i‑1的值，Mark_final＝max(Mark_i,Mark_i‑1)，同时并记下该评分值所对应的左右目图像图片中直线的编号k_tempL，j_tempR；在左目图像第k条直线依次遍历右目中所有直线之后，再用右目图像图片中直线的编号j_tempR去依次遍历左目中所有直线，执行公式最终迭代出最大的Mark值以及对应的i_tempR，如果i_tempL＝i_tempR，这左目图像直线编号为i_tempL的直线与右目图像直线编号为j_tempL成功匹配，存入匹配直线集合M中；否则匹配失败，重复上述步骤，依次迭代直到算法终止，最终得到步骤3：直线特征的三维重建3.1:空间直线的求解设图像直线方程为ax+by+c＝0，根据中心投影原理可知，图像上的一条直线对应于空间一个平面，设空间平面方程为：AX+BY+CZ+D＝0由点的中心成像模型有：其中m₁₁到m₃₄为摄像机成像的内参矩阵的乘积，比较方程可得：将图像上匹配的像直线确定的两个共面方程联立求解,交会得到像直线对应的空间直线，设空间直线L_EF对应的像直线为列出空间直线的方程为：采用空间直线的参数式方程，取任意x＝x₀代入上述方程中，解出空间直线上某一点的坐标(X₀,Y₀,Z₀)；己知垂直两平面的法矢量确定直线的方向矢量空间直线的方程为：3.2求解空间线段在左目中由光心点O_l与投影到左目成像平面的直线段两端，分别构成两条空间直线与求出空间直线l_EF与I_EF与的空间交点(若两直线异面，则求出距离两直线最短距离的点)分别为E_left与F_left。同理在右目中可以求出空间直线的交点E_right与F_right。为保证直线信息得到最大可能的保留，在所得的四点{E_left,E_right,F_left,F_right}中，取长度最大的两点当做还原的线段起点与终点。从而实现了空间直线段EF的还原。将所得到的空间直线依次算出得到空间的直线段集合N＝{L₁,L₂…L_n}。步骤4：位姿计算4.1选择左目的图像作为粗匹配的二维平面：在步骤3中得到线段集合获取集合中两两线段之间的交点，得到交点的集合为M_pnp＝{(x¹,y¹),(x²,y²)…(xⁿ,yⁿ)}；测量、建立现实坐标系中的三维点，存入点集G_3dpot＝{(X¹,Y¹,Z¹),(X²,Y²,Z²)…(Xⁿ,Yⁿ,Zⁿ)}中，将点集M_pnp中的所有的点按一定的顺序排列，使其顺序与对应的点集G_3dpot中的顺序保持一致，代入摄像机成像模型公式中：其中i∈[1,n]其中K为已知的相机内参，λ为成像模型的比例系数，将M_pnp和G_3dpot的点分别代入公式中，应用EPNP算法，最终得到粗匹配的旋转变量R_pnp、平移变量T_pnp；4.2获取点云：将空间直线，按照一定的阈值进行分割采样，最终获得基于直线特征的点云数据，取空间的某一条直线，具体方法如下：其中θ为直线与x轴正方向的夹角，len为该直线的长度，k为该直线上点云的数量，该直线所离散的点集集合为对步骤3中对所得到的三维直线，按照上述算法依次离散成点集P，对于人工建立的模板库按照同样的方法，形成点集Q；4.3将4.1中的结果设为初始化的旋转矩阵和平移向量，即R＝R_pnp，T＝T_pnp，更新数据点集，对P使用4.1求得的平移和旋转参数，得到新的变换点集,并计算误差其中，p_i∈P，q_i∈Q，若迭代估计误差之差小于给定阈值，即则计算完毕；若不满足，重复上述迭代过程，得出旋转变量R_icp和平移变量T_icp；最终，摄像机坐标系与物理坐标系对应的旋转变量R＝R_pnp·R_icp，平移变量T＝T_pnp+T_icp。