[发明专利]一种基于视觉—惯性的物体三维尺寸测量方法

权利要求书

1.一种基于视觉—惯性的物体三维尺寸测量方法，其特征在于，包括如下步骤：

S1、基于视觉—惯性耦合估计相机的位姿；

S2、提取物体检测框，计算物体检测框对应的反投影平面；

S3、构建对偶椭球包络方程；

S4、计算椭球最小包络盒的三维尺寸，从而获得物体的三维占据空间尺寸信息；

所述步骤S1基于视觉—惯性耦合估计相机的位姿的具体过程如下：

采用视觉—惯性紧耦合框架，包括对图像进行SIFT特征提取和匹配关联；由于IMU和图像的数据频率不同，对图像关键帧C_i到C_j之间的IMU数据进行预积分，积分结果为帧间的IMU姿态ΔR，速度Δv和位置Δp；对于C_i的状态，融合进预积分结果可得C_j下的IMU相对于世界坐标系的姿态R，速度v和位置p；

对于已跟踪的特征点X，利用三角法估计出其三维坐标，对于已经存在于地图中，并且与当前帧特征点建立起数据关联的特征点，加入优化函数之中；

构建视觉重投影误差及IMU惯性误差作为优化代价函数，函数如下：

其中，E_proj(k,j)为视觉传感的代价函数，其定义来自特征点的三维坐标在图像上的重投影误差：

其中{R_CB,p_CB}是相机坐标系与IMU坐标系的相对变换，用于将第k个特征点在世界坐标系的坐标转换到相机坐标系下的坐标函数是相机成像模型，将三维坐标点映射到图像坐标系中，得到其在图像投影的像素坐标，ρ(·)是核函数Huber函数，用于将原始误差的平方项替换为一个增长较缓的函数，同时保证误差函数的光滑性质；

E_IMU(i,j)为IMU误差项：

e_b＝b^j-bⁱ

E_IMU(i,j)表示第i帧与第j帧的IMU误差项，包括姿态误差e_R、速度误差e_v、位置误差e_p以及偏移误差e_h，其中∑_I是预积分的信息矩阵，而∑_R是偏移随机游走的信息矩阵；在姿态误差e_R中，ΔR_ij表示通过i、j两个帧的IMU相对变换姿态，表示姿态预积分在j帧的近似修正项，表示IMU通过预积分得到的i-j的相对变换姿态，Log(·)将正交变换群SO(3)的元素映射到李代数so(3)中；在速度误差e_v中，和分别表示j帧和i帧的IMU速度在世界坐标系下的表示，g_W表示重力矢量，Δv_ij表示ij两个帧的速度差，而和分别表示陀螺仪和加速度计在j帧的偏移近似项；在位置误差e_p中，和分别表示在j帧和i帧的IMU相对于世界坐标系位置，是i帧的IMU线速度在世界坐标系下的表达，Δp_ij是i、j两个帧的位置差；在偏移项e_b中，b^j和bⁱ分别表示j、i帧下的IMU偏移量；

通过对上述包含视觉传感和惯性传感误差的方程利用g2o优化框架进行优化求解，求出各关键帧的姿态R_j和位置P_j；

所述步骤S2的具体过程如下：

采用预训练卷积深度神经网络在目标物体数据集上进行训练、微调，得到一个目标物体检测网络模块，该模块将从输入图像中提取目标物体，输出该物体语义标签、2D包络框的坐标及尺寸信息，设该包络框四个顶点记为p₁，p₂，p₃，p₄，四条边为l₁，l₂，l₃，l₄，均在齐次像素坐标系下表示；在该坐标下，任一点p均有坐标[u,v,1]，而任一直线l_t可由其上两个点p_a及p_b的叉乘得到，即：

根据相机成像模型，图像中的直线l反投影得到过相机光心的平面π，则给定第k帧图像平面反投影方程可由相机投影矩阵P_k＝K[R_k t_k]得到，即：

其中π_t＝[π_t1,π_t2,π_t3,π_t4]^T是由l_t所得的反投影平面，对所有图像中的物体检测框的四条边均执行此操作，每个检测框均得到四个反投影平面。