CN115049949A - 一种基于双目视觉的物体表达方法 - Google Patents

Abstract

本发明属于机器人导航技术领域，具体涉及一种基于双目视觉的物体表达方法。本发明通过机器人搭载的双目相机获取关于场景的当前帧图像；从当前帧左右图像中检测场景里的物体；对得到的左右图像中的物体进行匹配，完成相同物体的配对；利用卡尔曼滤波与匈牙利匹配对双目帧间的物体进行跟踪，一旦检测到相同物体，利用两帧中的四张图像完成对物体的语义信息提取，包括物体的大小，位置和姿态。本发明提出了一种可靠性好、适用性广的物体表达方法，保证场景中任意形状的物体都能提取出有价值的语义信息，从而促进视觉同步定位与建图的精度和语义性。

Description

一种基于双目视觉的物体表达方法

技术领域

本发明属于机器人导航技术领域，具体涉及一种基于双目视觉的物体表达方法。

背景技术

同步定位与建图(Simultaneous Localization and Mapping，SLAM)技术首先作为一种室内定位和建图方法被提出，随后受到广泛的研究，它可以让机器人在没有GPS(Global Position System，GPS)信号的环境中，仅利用实时传感器信息在未知环境中自主导航、自主定位，并对环境进行数据采集，通过一定的技术实现对自身位置和环境地图的实时修正。

目前，机器人视觉SLAM的研究主要以点特征为主，即通过提取特征点进行匹配，从而获得数据关联，对于纹理细节较少的场景，并不利于特征点的提取。另一方面，点特征作为低等级的特征，并不能利用图像中更高级的语义信息，除了用于机器人定位外，其构建的稀疏点云地图，很难从中提取有用的环境信息。

本发明涉及的技术问题是提供一种高级的特征提取方法，使视觉SLAM能够利用场景中的物体，提高在特征纹理缺少的环境中的鲁棒性，并构建信息丰富的语义地图。

申请日为2018年12月30日、申请号为CN201811648994.2、发明名称为“一种基于语义约束的视觉SLAM方法”的专利申请，该方法的视觉语义部分虽然与本发明实现相同的目的，但该方法利用深度学习的语义分割网络进行物体时别，并利用深度相机的三维点与平面地图点作为视觉SLAM的语义约束，与本发明完全不同，此外该方法并没有说明如何进行语义的数据关联。

申请日为2019年01月15日、申请号为CN201910037102.3、发明名称为“基于物体实例匹配的机器人语义SLAM方法、处理器及机器人”的专利申请，该方法也是用深度学习的语义分割网络进行物体时别，并利用物体的点特征和编码产生的特征描述向量进行物体匹配，与本发明完全不同。

发明内容

本发明的目的在于提供一种基于双目视觉的物体表达方法。

一种基于双目视觉的物体表达方法，包括以下步骤：

步骤1：利用机器人搭载的双目相机，采集场景中的当前帧图像对；

步骤2：物体单帧识别与匹配：对左右图像分别进行物体识别，将得到的物体矩形框利用像素重叠度判别是否为同一物体，完成匹配并进入步骤3；

步骤3：物体帧间跟踪：通过步骤2得到物体集合后，对每个物体判断是否在历史帧中出现，对已出现的物体进行归类，对未出现的物体进行跟踪初始化，对跟踪失败的物体进行剔除，进入步骤4；

步骤4：物体语义信息提取：将步骤3已跟踪超过两帧的物体进行语义信息的提取，进入步骤5；

步骤5：输出场景物体的语义信息，结束。

进一步地，所述的步骤2具体为：

步骤2.1：物体信息提取；

定义图像中物体的信息为：

B＝(w,h,b,σ,c)

其中，w和h表示物体矩形框的像素长度和宽度；b＝(u,v)表示矩形框的中心像素位置；σ和c分别表示矩形框的置信度和物体类别；

对左右图像进行物体识别，得到图像中所有的物体：

O_L＝{B_Li＝(w_Li,h_Li,b_Li,σ_Li,c_Li)|c_Li∈C,σ_Li∈[0,1],i＝1,2,...,k}

O_R＝{B_Ri＝(w_Ri,h_Ri,b_Ri,σ_Ri,c_Ri)|c_Ri∈C,σ_Ri∈[0,1],i＝1,2,...,m}

其中，O_L和O_R表示当前帧左右图像中的所有物体集合；k和m表示当前帧左右图像中的所有物体数量；C表示先设置的物体类别集合；

步骤2.2：单帧物体匹配；

定义物体之间的像素重叠度为：

其中，S(·)＝w·h表示矩形框的面积；A,B表示图像上的两个物体；

将双目单帧左图上的物体依次计算与右图上物体的像素重叠度，选出重叠度最大的作为其候选匹配对，形成候选匹配对集合：M_can＝{(B_Lcan,B_Rcan)|B_Lcan∈O_L,B_Rcan∈O_R}；

步骤2.3：误匹配判别：对候选匹配对集合M_can中的任意一个匹配对(B_{Lcan_i},B_{Rcan_i})，首先利用双目特性约束判别矩形框中心b＝(u,v)的相对位置，若不满足u_Lcan＜u_Rcan和|v_Lcan-v_Rcan|＜10，则剔除该候选匹配对，进入步骤2.4；

步骤2.4：重复性检验；在完成所有候选匹配对的相对位置判别后，若集合M_can中存在两个或多个候选匹配对中有同一物体信息B，则将这些候选匹配对全部剔除，保证每个物体信息至多出现在一个候选匹配对中，进入步骤2.5；

步骤2.5：最终得到双目单帧左右图像的匹配对集合，即每个匹配对表示同一物体在左右图像上的成像信息：

M＝{(B_LM,B_RM)|B_LM∈O_L,B_RM∈O_R}。

进一步地，所述的步骤3具体为：

步骤3.1：判断当前关键帧是否为初始关键帧，即i＝0，若当前关键帧是初始关键帧，则执行步骤3.2；否则，执行步骤3.3；

步骤3.2：初始化跟踪系统，设置卡尔曼滤波的跟踪子变量为

和

表示矩形框中心位置像素坐标的速度大小；设置过程协方差矩阵P、测量协方差矩阵Q、测量矩阵R中变量的的噪声，设置转换矩阵H、卡尔曼增益矩阵K、状态转移矩阵F；根据初始关键帧配对集合M⁰中的左图结果

初始化跟踪变量

X⁰中跟踪的物体个数与

一致，所有的

和

均初始化为0，进入步骤3.3；

步骤3.3：物体运动预测：对上一关键帧的跟踪变量X^i-1进行预测，得到当前关键帧中物体的先验状态

以及先验的过程协方差矩阵

步骤3.4：物体匹配：计算物体先验信息

中每个矩形框与物体测量信息

中所有矩形框的像素重叠度，并将其数值取负，形成匹配的代价矩阵，进而得到匹配结果；

步骤3.5：更新物体运动状态：根据匹配结果从X^i-1中筛选出跟踪成功的物体集合

并更新其状态：

其中，I为单位阵；Kⁱ为卡尔曼增益；Zⁱ为测量值，根据匹配结果从

中得出；

步骤3.6：更新跟踪系统：对

中未匹配上的物体进行卡尔曼滤波初始化，初始化参数与步骤3.2中一致，并添加到跟踪成功的物体集合

中，最终形成当前关键帧的跟踪变量Xⁱ。

进一步地，所述的步骤4具体为：

步骤4.1：物体条件判别：从当前关键帧的跟踪物体Xⁱ中筛选出跟踪次数大于2且没有提取语义信息的物体，组成集合Oⁱ＝{o₁,o₂,...,o_l}，进入步骤4.2；

步骤4.2：物体语义信息恢复：

对集合Oⁱ中的任一物体oⁱ，从历史跟踪过程中得到双目两帧上的检测矩形框，即4个矩形框，根据矩形顶点坐标转换成16条直线集合

每条直线用三个参数表示，即l＝(a,b,c,1)；

空间平面与相机平面相交是一条直线：

π＝P^TL

其中，π＝(π₁,π₂,π₃,π₄)表示空间中的某一平面，由四个参数表示；P＝K[R|t]表示相机投影矩阵，由相机内参K，相机旋转矩阵R和位移向量t组成；

已知空间中椭球可以由一个4×4的对称阵Q表示，则面坐标形式的空间椭球方程为：

π^TQ^*π＝0

其中，Q^*是Q的伴随矩阵，也是一个对称阵，共有十个自由度，即：

则面坐标形式的空间椭球方程改写为：

将上式简写成aq＝0，这样16个与物体最小包络椭球相切的平面集合可组成方程组：

a_iq＝0,(i＝1,2,...,16)

利用奇异值分解可以求得一个最小二乘解：

Aq＝UDV^T

其中，A是由a_i组成的16×10的系数矩阵；U，D和V分别是奇异值奇异值分解得到的矩阵；取V的最后一列，即最小二乘解q，这样就得到最小包络椭球的伴随阵Q^*；

将矩阵Q^*对应的伴随阵Q进一步分解：

其中，s₁，s₂和s₃为椭球的三个半轴长，即大小；Q是点坐标形式的二次曲面矩阵，即

Q₃₃为矩阵Q左上角3×3的矩阵；λ₁，λ₂和λ₃为Q₃₃的特征值；椭球的旋转角θ＝(θ₁,θ₂,θ₃)则是Q₃₃的特征向量；椭球的位移对应矩阵

的最后一列的归一化坐标：t＝(q₄,q₇,q₉)/q₁₀＝(t₁,t₂,t₃)；

步骤4.3：从双目相机的两帧中，恢复出物体的语义表达

本发明的有益效果在于：

本发明通过机器人搭载的双目相机获取关于场景的当前帧图像；从当前帧左右图像中检测场景里的物体；对得到的左右图像中的物体进行匹配，完成相同物体的配对；利用卡尔曼滤波与匈牙利匹配对双目帧间的物体进行跟踪，一旦检测到相同物体，利用两帧中的四张图像完成对物体的语义信息提取，包括物体的大小，位置和姿态。本发明提出了一种可靠性好、适用性广的物体表达方法，保证场景中任意形状的物体都能提取出有价值的语义信息，从而促进视觉同步定位与建图的精度和语义性。

附图说明

图1为本发明的总体流程图。

图2为本发明中物体单帧识别与匹配的流程图。

图3为本发明中物体帧间跟踪的流程图。

图4为本发明中物体语义信息提取的流程图。

具体实施方式

下面结合附图对本发明做进一步描述。

本发明涉及的是一种基于双目视觉的物体表达方法，用于视觉同步定位与建图中环境物体的语义信息提取。该方法包括：通过机器人搭载的双目相机获取关于场景的当前帧图像；从当前帧左右图像中检测场景里的物体；对得到的左右图像中的物体进行匹配，完成相同物体的配对；利用卡尔曼滤波与匈牙利匹配对双目帧间的物体进行跟踪，一旦检测到相同物体，利用两帧中的四张图像完成对物体的语义信息提取，包括物体的大小，位置和姿态。本发明的有益效果在于提出了一种可靠性好、适用性广的物体表达方法，保证场景中任意形状的物体都能提取出有价值的语义信息，从而促进视觉同步定位与建图的精度和语义性。

本发明的内容是这样实现的：包括以下步骤：

步骤(1)：输入双目相机图像：利用机器人搭载的双目相机，采集场景中的当前帧图像对，作为本方法的输入，进入步骤(2)；

步骤(2)：物体单帧识别与匹配：对左右图像分别进行物体识别，将得到的物体矩形框利用像素重叠度判别是否为同一物体，具体步骤如下：

步骤(2.1)：物体信息提取：定义图像中物体的信息为：

B＝(w,h,b,σ,c)

其中，w和h表示物体矩形框的像素长度和宽度，b＝(u,v)表示矩形框的中心像素位置，σ和c分别表示矩形框的置信度和物体类别，利用开源的目标检测算法YOLOv5或其他目标检测方法对左右图像进行物体识别，得到图像中所有的物体：

O_L＝{B_Li＝(w_Li,h_Li,b_Li,σ_Li,c_Li)|c_Li∈C,σ_Li∈[0,1],i＝1,2,...,k}

O_R＝{B_Ri＝(w_Ri,h_Ri,b_Ri,σ_Ri,c_Ri)|c_Ri∈C,σ_Ri∈[0,1],i＝1,2,...,m}

其中，O_L和O_R表示当前帧左右图像中的所有物体集合，k和m表示当前帧左右图像中的所有物体数量，C表示算法YOLOv5预先设置的物体类别集合，进入步骤(2.2)；

步骤(2.2)：单帧物体匹配：定义物体之间的像素重叠度为：

其中，S(·)＝w·h表示矩形框的面积，A,B表示图像上的两个物体。将双目单帧左图上的物体依次计算与右图上物体的像素重叠度，选出重叠度最大的作为其候选匹配对，形成候选匹配对集合：

M_can＝{(B_Lcan,B_Rcan)|B_Lcan∈O_L,B_Rcan∈O_R}

步骤(2.3)：误匹配判别：对候选匹配对集合M_can中的任意一个匹配对(B_{Lcan_i},B_{Rcan_i})，首先利用双目特性约束判别矩形框中心b＝(u,v)的相对位置，若不满足u_Lcan＜u_Rcan和|v_Lcan-v_Rcan|＜10，则剔除该候选匹配对，进入步骤(2.4)；

步骤(2.4)：重复性检验；在完成所有候选匹配对的相对位置判别后，若集合M_can中存在两个或多个候选匹配对中有同一物体信息B，则将这些候选匹配对全部剔除，保证每个物体信息至多出现在一个候选匹配对中，进入步骤(2.5)；

步骤(2.5)：最终得到双目单帧左右图像的匹配对集合，即每个匹配对表示同一物体在左右图像上的成像信息：

M＝{(B_LM,B_RM)|B_LM∈O_L,B_RM∈O_R}

步骤(3)：物体帧间跟踪：假设当前关键帧i的匹配对集合为Mⁱ，则上一关键帧i-1的匹配对集合为M^i-1，利用卡尔曼滤波算法对场景中被识别到的物体进行帧间跟踪。具体步骤如下：

步骤(3.1)：系统是否初始化：判断当前关键帧是否为初始关键帧，即i＝0，若是进入步骤(3.2)，否则进入步骤(3.3)；

步骤(3.2)：初始化跟踪系统：卡尔曼滤波算法可由许多开源项目中如Python语言中的filterpy软件包中获得，这里简要说明必要的参数设置。设置卡尔曼滤波的跟踪子变量为6个，即：

其中，

和

表示矩形框中心位置像素坐标的速度大小。设置过程协方差矩阵P中变量u,v,w,h噪声为10，变量

的噪声为10000；设置测量协方差矩阵Q中变量u,v,w,h噪声为1，变量

的噪声为0.01；设置测量矩阵R中变量u,v误差为1，w,h误差为10；转换矩阵H设置为纬度4×7的单位对角阵；卡尔曼增益矩阵K设置为0，设置状态转移矩阵F为：

根据初始关键帧配对集合M⁰中的左图结果

初始化跟踪变量

X⁰中跟踪的物体个数与

一致，其中所有的

和

均初始化为0，进入步骤(3.3)；

步骤(3.3)：物体运动预测：系统对上一关键帧的跟踪变量X^i-1进行预测，得到当前关键帧中物体的先验状态：

以及先验的过程协方差矩阵：

步骤(3.4)：物体匹配：计算物体先验信息

中每个矩形框与物体测量信息

中所有矩形框的像素重叠度，并将其数值取负，形成匹配的代价矩阵，利用匈牙利匹配算法与代价矩阵得到匹配结果，具体算法可由许多开源项目中获取，如Python语言中的scipy软件包中的linear_sum_assignment函数。进入步骤(3.5)；

步骤(3.5)：更新物体运动状态：根据匹配结果从X^i-1中筛选出跟踪成功的物体集合

并更新其状态：

其中，I为单位阵，Kⁱ为卡尔曼增益，Zⁱ为测量值，根据匹配结果从

中得出。进入步骤(3.6)；

步骤(3.6)：更新跟踪系统：对

中未匹配上的物体进行卡尔曼滤波初始化，初始化参数与步骤(3.2)中一致，并添加到跟踪成功的物体集合

中，最终形成当前关键帧的跟踪变量Xⁱ，进入步骤(4)；

步骤(4)：物体语义信息提取：从当前关键帧的跟踪物体Xⁱ中筛选出跟踪次数大于2且没有提取语义信息的物体，通过多视图几何的理论恢复出物体的在空间中的最小包络椭球，即椭球的大小和位姿就是物体的语义信息，具体步骤如下：

步骤(4.1)：物体条件判别：从当前关键帧的跟踪物体Xⁱ中筛选出跟踪次数大于2且没有提取语义信息的物体，组成集合Oⁱ＝{o₁,o₂,...,o_l}，进入步骤(4.2)。

步骤(4.2)：物体语义信息恢复：对集合Oⁱ中的任一物体oⁱ，可以从历史跟踪过程中得到双目两帧上的检测矩形框，即4个矩形框，根据矩形顶点坐标可以转换成16条直线集合

每条直线用三个参数表示，即l＝(a,b,c,1)。由射影几何可知，空间平面与相机平面相交是一条直线：

π＝P^TL

其中，π＝(π₁,π₂,π₃,π₄)表示空间中的某一平面，由四个参数表示，P＝K[R|t]表示相机投影矩阵，由相机内参K，相机旋转矩阵R和位移向量t组成，可由SLAM的视觉里程计得到。这样可以得到16个与物体最小包络椭球相切的平面集合。已知空间中椭球可以由一个4×4的对称阵Q表示，则面坐标形式的空间椭球方程为：

π^TQ^*π＝0

其中，Q^*是Q的伴随矩阵，也是一个对称阵，共有十个自由度，即：

则面坐标形式的空间椭球方程可以改写为：

将上式简写成aq＝0，这样16个与物体最小包络椭球相切的平面集合可组成方程组：

a_iq＝0,(i＝1,2,...,16)

利用奇异值分解可以求得一个最小二乘解：

Aq＝UDV^T

其中A是由a_i组成的16×10的系数矩阵，U，D和V分别是奇异值奇异值分解得到的矩阵，取V的最后一列，即最小二乘解q，这样就得到最小包络椭球的伴随阵Q^*。

将矩阵Q^*对应的伴随阵Q进一步分解：

其中，s₁，s₂和s₃为椭球的三个半轴长，即大小。Q是点坐标形式的二次曲面矩阵，即

Q₃₃为矩阵Q左上角3×3的矩阵，λ₁，λ₂和λ₃为Q₃₃的特征值。椭球的旋转角θ＝(θ₁,θ₂,θ₃)则是Q₃₃的特征向量。而椭球的位移，则对应矩阵

的最后一列的归一化坐标：t＝(q₄,q₇,q₉)/q₁₀＝(t₁,t₂,t₃)。最终，从双目相机的两帧中，恢复出物体的语义表达

步骤(5)：输出物体的语义信息，结束。

与现有技术相比，本发明的有益效果在于提出了一种可靠性好、适用性广的物体表达方法，保证场景中任意形状的物体都能提取出有价值的语义信息，从而促进视觉同步定位与建图的精度和语义性。

本发明为视觉SLAM算法提供了一种通用的物体语义表达方法，服务于机器人在场景特征稀少的情况下利用物体语义约束满足导航精度，并构建与稀疏点云不同的语义性地图。本发明通过机器人搭载的双目相机获取关于场景的当前帧图像；从当前帧左右图像中检测场景里的物体；对得到的左右图像中的物体进行匹配，完成相同物体的配对；利用卡尔曼滤波与匈牙利匹配对双目帧间的物体进行跟踪，一旦检测到相同物体，利用两帧中的四张图像完成对物体的语义信息提取，包括物体的大小，位置和姿态。本发明提出了一种可靠性好、适用性广的物体表达方法，保证场景中任意形状的物体都能提取出有价值的语义信息，从而促进视觉同步定位与建图的精度和语义性。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (4)

1.一种基于双目视觉的物体表达方法，其特征在于，包括以下步骤：

步骤1：利用机器人搭载的双目相机，采集场景中的当前帧图像对；

步骤2：物体单帧识别与匹配：对左右图像分别进行物体识别，将得到的物体矩形框利用像素重叠度判别是否为同一物体，完成匹配并进入步骤3；

步骤3：物体帧间跟踪：通过步骤2得到物体集合后，对每个物体判断是否在历史帧中出现，对已出现的物体进行归类，对未出现的物体进行跟踪初始化，对跟踪失败的物体进行剔除，进入步骤4；

步骤4：物体语义信息提取：将步骤3已跟踪超过两帧的物体进行语义信息的提取，进入步骤5；

步骤5：输出场景物体的语义信息，结束。

2.根据权利要求1所述的一种基于双目视觉的物体表达方法，其特征在于：所述的步骤2具体为：

步骤2.1：物体信息提取；

定义图像中物体的信息为：

B＝(w,h,b,σ,c)

其中，w和h表示物体矩形框的像素长度和宽度；b＝(u,v)表示矩形框的中心像素位置；σ和c分别表示矩形框的置信度和物体类别；

对左右图像进行物体识别，得到图像中所有的物体：

O_L＝{B_Li＝(w_Li,h_Li,b_Li,σ_Li,c_Li)|c_Li∈C,σ_Li∈[0,1],i＝1,2,...,k}

O_R＝{B_Ri＝(w_Ri,h_Ri,b_Ri,σ_Ri,c_Ri)|c_Ri∈C,σ_Ri∈[0,1],i＝1,2,...,m}

其中，O_L和O_R表示当前帧左右图像中的所有物体集合；k和m表示当前帧左右图像中的所有物体数量；C表示先设置的物体类别集合；

步骤2.2：单帧物体匹配；

定义物体之间的像素重叠度为：

其中，S(·)＝w·h表示矩形框的面积；A,B表示图像上的两个物体；

将双目单帧左图上的物体依次计算与右图上物体的像素重叠度，选出重叠度最大的作为其候选匹配对，形成候选匹配对集合：M_can＝{(B_Lcan,B_Rcan)|B_Lcan∈O_L,B_Rcan∈O_R}；

步骤2.3：误匹配判别：对候选匹配对集合M_can中的任意一个匹配对(B_{Lcan_i},B_{Rcan_i})，首先利用双目特性约束判别矩形框中心b＝(u,v)的相对位置，若不满足u_Lcan＜u_Rcan和|v_Lcan-v_Rcan|＜10，则剔除该候选匹配对，进入步骤2.4；

步骤2.4：重复性检验；在完成所有候选匹配对的相对位置判别后，若集合M_can中存在两个或多个候选匹配对中有同一物体信息B，则将这些候选匹配对全部剔除，保证每个物体信息至多出现在一个候选匹配对中，进入步骤2.5；

步骤2.5：最终得到双目单帧左右图像的匹配对集合，即每个匹配对表示同一物体在左右图像上的成像信息：

M＝{(B_LM,B_RM)|B_LM∈O_L,B_RM∈O_R}。

3.根据权利要求1所述的一种基于双目视觉的物体表达方法，其特征在于：所述的步骤3具体为：

步骤3.1：判断当前关键帧是否为初始关键帧，即i＝0，若当前关键帧是初始关键帧，则执行步骤3.2；否则，执行步骤3.3；

步骤3.2：初始化跟踪系统，设置卡尔曼滤波的跟踪子变量为

和

表示矩形框中心位置像素坐标的速度大小；设置过程协方差矩阵P、测量协方差矩阵Q、测量矩阵R中变量的的噪声，设置转换矩阵H、卡尔曼增益矩阵K、状态转移矩阵F；根据初始关键帧配对集合M⁰中的左图结果

初始化跟踪变量

X⁰中跟踪的物体个数与

一致，所有的

和

均初始化为0，进入步骤3.3；

步骤3.3：物体运动预测：对上一关键帧的跟踪变量X^i-1进行预测，得到当前关键帧中物体的先验状态

以及先验的过程协方差矩阵

步骤3.4：物体匹配：计算物体先验信息

中每个矩形框与物体测量信息

中所有矩形框的像素重叠度，并将其数值取负，形成匹配的代价矩阵，进而得到匹配结果；

步骤3.5：更新物体运动状态：根据匹配结果从X^i-1中筛选出跟踪成功的物体集合

并更新其状态：

其中，I为单位阵；Kⁱ为卡尔曼增益；Zⁱ为测量值，根据匹配结果从

中得出；

步骤3.6：更新跟踪系统：对

中未匹配上的物体进行卡尔曼滤波初始化，初始化参数与步骤3.2中一致，并添加到跟踪成功的物体集合

中，最终形成当前关键帧的跟踪变量Xⁱ。

4.根据权利要求1所述的一种基于双目视觉的物体表达方法，其特征在于：所述的步骤4具体为：

步骤4.1：物体条件判别：从当前关键帧的跟踪物体Xⁱ中筛选出跟踪次数大于2且没有提取语义信息的物体，组成集合Oⁱ＝{o₁,o₂,...,o_l}，进入步骤4.2；

步骤4.2：物体语义信息恢复：

对集合Oⁱ中的任一物体oⁱ，从历史跟踪过程中得到双目两帧上的检测矩形框，即4个矩形框，根据矩形顶点坐标转换成16条直线集合

每条直线用三个参数表示，即l＝(a,b,c,1)；

空间平面与相机平面相交是一条直线：

π＝P^TL

其中，π＝(π₁,π₂,π₃,π₄)表示空间中的某一平面，由四个参数表示；P＝K[R|t]表示相机投影矩阵，由相机内参K，相机旋转矩阵R和位移向量t组成；

已知空间中椭球可以由一个4×4的对称阵Q表示，则面坐标形式的空间椭球方程为：

π^TQ^*π＝0

其中，Q^*是Q的伴随矩阵，也是一个对称阵，共有十个自由度，即：

则面坐标形式的空间椭球方程改写为：

将上式简写成aq＝0，这样16个与物体最小包络椭球相切的平面集合可组成方程组：

a_iq＝0,(i＝1,2,...,16)

利用奇异值分解可以求得一个最小二乘解：

Aq＝UDV^T

其中，A是由a_i组成的16×10的系数矩阵；U，D和V分别是奇异值奇异值分解得到的矩阵；取V的最后一列，即最小二乘解q，这样就得到最小包络椭球的伴随阵Q^*；

将矩阵Q^*对应的伴随阵Q进一步分解：

其中，s₁，s₂和s₃为椭球的三个半轴长，即大小；Q是点坐标形式的二次曲面矩阵，即

Q₃₃为矩阵Q左上角3×3的矩阵；λ₁，λ₂和λ₃为Q₃₃的特征值；椭球的旋转角θ＝(θ₁,θ₂,θ₃)则是Q₃₃的特征向量；椭球的位移对应矩阵

的最后一列的归一化坐标：t＝(q₄,q₇,q₉)/q₁₀＝(t₁,t₂,t₃)；

步骤4.3：从双目相机的两帧中，恢复出物体的语义表达

Images