[发明专利]一种融合双目相机与IMU的定位与三维线框结构重建方法与系统

权利要求书

1.一种融合双目相机与IMU的定位与三维线框结构重建方法，其特征在于，包括如下步骤：

步骤1，读取惯性测量信息流和双目视频流数据，并根据相机的参数，对图像进行几何校正，消除左右视图的畸变，使得行对准；同时在时间维度上对齐读入的惯性测量数据与双目图像数据；

步骤2，在纯双目视觉模式下运行，提取特征点和关键帧中的LSD特征线段，进行左右目图像特征匹配，并基于关键帧进行跟踪定位与建图；

步骤3，利用步骤2中估计的关键帧的位置与姿态，初始化惯性测量单元；

步骤4，完成步骤3的IMU初始化以后，重新进行特征提取与图像匹配，而后联合双目视觉与惯性测量单元进行跟踪定位与建图；

步骤5，根据步骤4中的关键帧的位姿，更新步骤2中特征线段端点的坐标，得到初始的三维线框的位置；然后，遍历三维线框中的所有线段，计算线段间距离和夹角，与角度阈值和距离阈值相比较，判断线段是否表示同一条边，将同一边的线段聚为一个组，得到不同的线段的组；

步骤6，遍历步骤5得到的线段的组的集合，分别使用各组内包含的所有线段拟合出各组所对应的边；具体实现包括如下步骤，

步骤6.1，获取每个组内所有线段的端点，基于M估计的方法进行直线迭代加权拟合，分为以下几个子步骤：

步骤6.1.1，选取Huber’s M-estimators，对应的ρ函数为：

其中，r表示自变量；

步骤6.1.2，使用选取的ρ函数，构造参数求解模型F：

其中n为每组线段的总数，ρ为选择的Distance function，a_i、L为观测值，X为直线方程中的未知量，空间直线可以表示为：

待求参数为X₀、Y₀、Z₀、m、n，观测值，即每条线段的端点坐标为X、Y、Z；

将空间直线转化为矩阵形式可得：

带入上面的求解模型：

n条线段得到2n个M-估计方程，通过模型残差倒数加权最小二乘迭代拟合求解(X₀,Y₀,Z₀),(m,n,1)；

步骤6.1.3，解得直线通过的三维点坐标P:(X₀,Y₀,Z₀)以及直线的方向向量l_direction:(m,n,1)；

步骤6.2，在步骤6.1将同组线段进行直线拟合，确定直线的范围，得到表示边的线段，分为以下子步骤：

步骤6.2.1，遍历同组内所有线段的端点，找到距离最远的两个点P1、P2；

步骤6.2.2，根据拟合直线的方向向量l_direction，提取P1、P2对应方向向量主方向的值，带入直线方程中，解得表示边框线段的两个端点，从而确定线段的空间位置；

步骤7，所有组的线段拟合完成后，统计拟合结果数量N₂，设定拟合阈值N_delta，若|N-N₂|N_delta，N为线段总数，则说明所有线段拟合完毕，不存在表示同一条边的线段组合；若|N-N₂|≥N_delta，N＝N₂，返回步骤5中重新计算线段间的距离；

步骤8，输出三维线框结构。