CN112184763A - 一种机器人视觉导航中视觉里程计构建方法 - Google Patents

Abstract

本发明提供了一种机器人视觉导航中视觉里程计构建方法，包括：提取图像特征点；获取物体边界框的位置；判断提取到的特征点是否在检测得到的物体边界框内，若不是，则剔除特征点；若在物体边界框内，则通过光流跟踪物体边界框内的特征点，并通过光流极线法进行动态点的判断，若物体边界框内动态点数目大于阈值，则认为该物体边界框内物体是运动的，将其中所有特征点剔除。若未超过阈值，则认为该物体边界框内物体是静止的，物体边界框内的特征点是有效特征点。根据有效特征点估算相机运动轨迹，恢复出相机旋转和平移运动，估计出相机位姿，完成视觉里程计的构建。本发明解决了动态环境下位姿估计的不准确问题,提高了定位精度。

Description

一种机器人视觉导航中视觉里程计构建方法

技术领域

本发明涉及视觉导航及图像处理的技术领域，具体涉及一种机器人视觉导航中视觉里程计构建方法。

背景技术

机器人进入未知环境后实现定位和导航功能离不开视觉里程计，视觉里程计指通过机器视觉技术，处理分析连续视频序列帧图像以完成对移动相机的位置及姿态估计，实现导航及定位功能。传统的视觉里程计在动态环境下鲁棒性与精度较差，场景中的运动物体会造成位姿估计中特征点的误匹配，进而影响定位精度与建图的准确性，受外部环境中移动物体特征点的干扰较大。

发明内容

发明目的：为解决背景技术中存在的技术问题，本发明提出一种机器人视觉导航中视觉里程计构建方法，包括如下步骤：

步骤1，读取原始图像，通过双阈值法提取特征点并获得特征点的位置；

步骤2，通过YOLOv3算法【可参考“Redmon J,Farhadi A.YOLOv3:an incrementalimprovement[OL].[2018-04-08].”】对原始图像进行目标检测，获取物体边界框的位置；

步骤3，根据特征点的位置及物体边界框的位置，判断特征点是否在物体边界框内，剔除不在物体边界框内的特征点，获得在物体边界框内的特征点；

步骤4，对物体边界框内的特征点进行光流跟踪，获取在物体边界框内特征点的运动位置；

步骤5，根据步骤4得到的运动位置，通过光流极线法进行动态特征点的判定，判断在物体边界框内的特征点是静态特征点还是动态特征点；

步骤6，根据所述的物体边界框内的动态特征点来判断物体运动情况，当物体边界框内动态特征点的数目达到一定阈值，则判定该物体边界框内物体是运动的，剔除物体边界框内所有的特征点，获取静态物体特征点，即为有效特征点；

步骤7，根据有效特征点估算相机运动轨迹，恢复出相机旋转和平移运动，估计出相机位姿，完成视觉里程计的构建；

本发明中，步骤1包括如下步骤：

步骤1-1：在原始图像中任取二十个像素p₁,p₂......p₂₀，p₂₀表示选取的第20个像素，分别检测出它们的亮度Ip₁,Ip₂......Ip₂₀，设定阈值Tp₁：

Ip_i为所取的第i个像素点的亮度，i取值为1～20；

步骤1-2：在原始图像上任意选取像素p，检测出它的亮度Ip，设定阈值Tp₂，Tp₂为Ip的20％；

步骤1-3：以像素p为中心选取半径为3的圆上16个像素点；

步骤1-4：如果选取出的圆上，有连续十二个像素点的亮度大于

或者圆上有连续十二个像素点的亮度小于

则像素p这个点标记为特征点，获取这个特征点的坐标(x_r,y_r)并保存。其中，x_r表示特征点的行坐标，y_r表示特征点的列坐标；

步骤1-5：循环步骤1-2～步骤1-4，对原始图像上的每一个像素点执行相同操作，得到特征点以及特征点位置。

步骤3包括：

步骤3-1：对比步骤1获取的特征点位置与步骤2获取的物体边界框的位置，判断特征点是否在物体边界框内；

步骤3-2：剔除不在物体边界框内的特征点，保留在物体边界框内的特征点；

步骤3-3：重复步骤3-1～步骤3-2，直至步骤1获取的特征点与步骤2获取的物体边界框都比较完毕。

步骤4包括：

步骤4-1：任取一个在步骤3获取的在物体边界框内的特征点(x,y)，记录当前时刻为t，记该特征点在t+dt时刻，运动到(x+dx,y+dy)处；

其中，x表示所取的特征点在t时刻的行坐标，y表示所取的特征点在t时刻的列坐标，(x,y)表示所取的特征点在t时刻的坐标，t+dt表示摄像头读取到下一帧图像的时刻，x+dx表示所取的特征点在t+dt时刻的行坐标，y+dy表示所取的特征点在t+dt时刻的列坐标，(x+dx,y+dy)表示所取的特征点在t+dt时刻的坐标。

步骤4-2：设定同一个空间点的像素灰度值固定不变，以I(x,y,t)描述所取的特征点在t时刻的灰度值，I(x+dx,y+dy,t+dt)表示所取的特征点在t+dt时刻的灰度值，则I(x+dx,y+dy,t+dt)＝I(x,y,t)；通过泰勒级数对方程进行展开，保留一阶项，得到：

由于灰度值不变，则有

其中，

表示当前帧图像在所取的特征点处x方向的偏导，

Ix表示当前帧图像在所取的特征点处x方向的梯度，

表示当前帧图像在所取的特征点处y方向的偏导，

Iy表示当前帧图像在所取的特征点处y方向的梯度，

表示当前帧图像灰度对时间的变化量的偏导，

It表示当前帧图像灰度对时间的变化量，

u,v分别表示所取的特征点在x轴上的运动速度、y轴上的运动速度。

步骤4-3：设定一个窗口内的像素具有相同的运动，则在一个大小为3×3的窗口中，利用最小二乘法，则有

其中，

It₁至It₉为所述的大小为3×3的窗口中的九个像素点的灰度值，Ix₁至Ix₉为所述的大小为3×3的窗口中的九个像素点x方向的梯度，Iy₁至Iy₉为所述的大小为3×3的窗口中的九个像素点y方向的梯度。A^T表示矩阵A的转置，(A^TA)^-1表示矩阵A^T与矩阵A乘积的逆矩阵。

步骤4-4：根据步骤4-3得到的所取的特征点在x轴，y轴上的运动速度

得到所取的特征点在t+dt时刻的(x+dx,y+dy)＝(x+udt,y+vdt)；

步骤4-5：通过对连续两帧图像进行光流追踪，获得步骤3所述在物体边界框内特征点在下一帧图像对应的匹配点；

步骤5包括：

步骤5-1：任取步骤4-5所获得的八对匹配点，计算基础矩阵F，计算公式如下：

其中，q_k＝(u_k,v_k,1),q′_k＝(u′_k,v′_k,1)分别表示所得到的八对匹配点中第k对匹配点归一化后的坐标，k的取值为1～8；u_k,u_k'分别表示所得到的八对匹配点中第k对匹配点归一化后的行坐标，v_k,v_k'分别表示所得到的八对匹配点中第k对匹配点归一化后的列坐标。

步骤5-2：利用步骤5-1得到的基础矩阵F，以步骤4所述的在t时刻坐标为(x,y)的特征点为例，将该特征点投影到t+dt时刻的图像，得到在t+dt时刻该特征点对应的极线l2；

l₂＝Fq

其中，q表示步骤4-1所取的特征点在t时刻的坐标(x,y)归一化后的坐标(x₁,y₁,1)，

步骤5-3：计算步骤5-2所取特征点在t+dt时刻的匹配点q'到极线l2的距离D，距离公式如下：

其中q'表示步骤5-2所取特征点在t+dt时刻对应的匹配点归一化坐标(x₁',y₁',1)，

表示q₁′的转置，q₀表示步骤5-2所取特征点在t时刻的坐标(x,y)，X,Y分别表示极线l2一般式中自变量与因变量的系数。

步骤5-4：判断距离D与阈值m关系，如果距离D大于阈值m，则判定所取的特征点是一个动态点；其中，m值为6。

步骤5-5：重复步骤5-2～步骤5-4，直至在物体边界框内特征点都判断完毕。

步骤6包括：

步骤6-1：将步骤5所获得的动态特征点位置保存，对比步骤5得到的动态特征点的位置与步骤2获得的物体边界框的位置，获得动态特征点落在物体区域的数目；

步骤6-2：判断步骤6-1获取到的动态特征点落在物体区域的数目与阈值n的大小，如果数目大于阈值n，则判定步骤2检测到的物体边界框内物体是运动的，剔除物体边界框内所有的特征点；其中，阈值n为15；

步骤6-3：重复步骤6-1～步骤6-2，直至步骤5得到的动态特征点与物体边界框都比较完毕，获得静态特征点，即为有效特征点。

步骤7包括：

步骤7-1：根据步骤6获得的有效特征点计算位姿估计本质矩阵【可参考“王君,徐晓凤,董明利,孙鹏,陈敏.单目移动机器人相对位姿估计方法[J].应用光学,2019,40(04):535-541.”】；

步骤7-2：根据本质矩阵恢复相机的运动信息，得到相机旋转矩阵与平移矩阵，从而估算出相机位姿【可参考“王鑫.基于单目视觉的目标物定位算法研究[D].哈尔滨:哈尔滨工业大学,2016.”】。

有益效果：本发明公开了一种机器人视觉导航中视觉里程计构建方法，普通视觉里程计在动态环境下鲁棒性较差，运动物体会造成位姿估计中的误匹配，进而影响建图的准确性。该方法有效解决了运动物体对普通视觉里程计的干扰，提高了定位导航精度。

附图说明

下面结合附图和具体实施方式对本发明做更进一步的具体说明，本发明的上述或其他方面的优点将会变得更加清楚。

图1为本发明技术流程框图。

图2为本发明提取的特征点结果。

图3为本发明进行目标检测的结果。

图4为本发明剔除不在物体边界框内的特征点后的结果。

图5所示为对极约束平面示意图。

图6为本发明剔除动态特征点后的结果。

具体实施方式

如图1所示，本发明公开了一种机器人视觉导航中视觉里程计构建方法，包括如下步骤：

步骤1，读取原始图像，通过双阈值法提取特征点并获得特征点的位置；

步骤2，通过YOLOv3算法对原始图像进行目标检测，获取物体边界框的位置；图3所示为目标检测结果图，分别框出了三轮车，行人，轿车的边界框。

步骤3，根据所述的特征点的位置及物体边界框的位置，判断特征点是否在物体边界框内，剔除不在物体边界框内的特征点，获得在物体边界框内的特征点；

步骤4，对所述的物体边界框内的特征点进行光流跟踪，获取在物体边界框内特征点的运动位置；

步骤5，根据所述的在物体边界框内的特征点的运动位置，通过光流极线法进行动态特征点的判定，判断在物体边界框内的特征点是静态特征点还是动态特征点；

步骤6，根据所述的物体边界框内的动态特征点来判断物体运动情况，当物体边界框内动态特征点的数目达到一定阈值，则认为该物体边界框内物体是运动的，剔除物体边界框内所有的特征点，获取静态物体特征点，即为有效特征点；

步骤7，根据所述的有效特征点估算相机运动轨迹，恢复出相机旋转和平移运动，估计出相机位姿，完成视觉里程计的构建；

本发明中，步骤1包括如下步骤：

步骤1-1：在原始图像中任取二十个像素p₁,p₂......p₂₀，分别检测出它们的亮度Ip₁,Ip₂......Ip₂₀，设定阈值1Tp₁；

其中，

Ip_i为所取的第i个像素点的亮度，i取值为1～20。

步骤1-2：在原始图像上任意选取像素p，检测出它的亮度Ip，设定阈值2Tp₂，Tp₂为Ip的20％；

步骤1-3：以像素p为中心选取半径为3的圆上16个像素点；

步骤1-4：若选取出的圆上，有连续十二个像素点的亮度大于

或者圆上有连续十二个像素点的亮度小于

则像素p这个点标记为特征点，获取特征点的坐标(x_r,y_r)并保存。其中，x_r表示特征点的行坐标，y_r表示特征点的列坐标，

步骤1-5：循环步骤1-2到1-4，对图像上的每一个像素点执行相同操作，得到特征点以及特征点位置。图2所示为一例特征点提取效果图。

步骤3包括：

步骤3-1：对比步骤1获取的特征点位置与步骤2获取的物体边界框的位置，判断特征点是否在物体边界框内；

步骤3-2：剔除不在物体边界框内的特征点，保留在物体边界框内的特征点；

步骤3-3：重复步骤3-1，步骤3-2，直至步骤1获取的特征点与步骤2获取的物体边界框都比较完毕。

图4为剔除不在物体边界框内的特征点后的结果。

步骤4包括：

步骤4-1：任取一个在步骤3获取的在物体边界框内的特征点(x,y)，记录当前时刻为t,记该特征点在t+dt时刻，运动到(x+dx,y+dy)处；

其中，x表示所取的特征点在t时刻的行坐标，y表示所取的特征点在t时刻的列坐标，(x,y)表示所取的特征点在t时刻的坐标，t+dt表示摄像头读取到下一帧图像的时刻，x+dx表示所取的特征点在t+dt时刻的行坐标，y+dy表示所取的特征点在t+dt时刻的列坐标，(x+dx,y+dy)表示所取的特征点在t+dt时刻的坐标。

步骤4-2：假设同一个空间点的像素灰度值固定不变，以I(x,y,t)描述所取的特征点在t时刻的灰度值，I(x+dx,y+dy,t+dt)表示所取的特征点在t+dt时刻的灰度值，则I(x+dx,y+dy,t+dt)＝I(x,y,t)；通过泰勒级数对方程进行展开，保留一阶项，得到：

由于灰度值不变，则有

其中，

表示当前帧图像在所取的特征点处x方向的偏导，

Ix表示当前帧图像在所取的特征点处x方向的梯度，

表示当前帧图像在所取的特征点处y方向的偏导，

Iy表示当前帧图像在所取的特征点处y方向的梯度，

表示当前帧图像灰度对时间的变化量的偏导，

It表示当前帧图像灰度对时间的变化量，

u,v分别表示所取的特征点在x轴，y轴上的运动速度。

步骤4-3：假设一个窗口内的像素具有相同的运动，则在一个大小为3×3的窗口中，利用最小二乘法，则有

其中，

It₁至It₉为所述的大小为3×3的窗口中的九个像素点的灰度值，Ix₁至Ix₉为所述的大小为3×3的窗口中的九个像素点x方向的梯度，Iy₁至Iy₉为所述的大小为3×3的窗口中的九个像素点y方向的梯度。A^T表示矩阵A的转置，(A^TA)^-1表示矩阵A^T与矩阵A乘积的逆矩阵。

步骤4-4：根据步骤4-3得到的所取的特征点在x轴，y轴上的运动速度

得到所取的特征点在t+dt时刻的(x+dx,y+dy)＝(x+udt,y+vdt)；

步骤4-5：通过对连续两帧图像进行光流追踪，获得步骤3所述在物体边界框内特征点在下一帧图像对应的匹配点；

步骤5包括：

步骤5-1：任取步骤4-5所获得的八对匹配点，计算基础矩阵F，计算公式如下：

其中，q_k＝(u_k,v_k,1),q′_k＝(u′_k,v′_k,1)分别表示所得到的八对匹配点中第k对匹配点归一化后的坐标，k的取值为1～8；u_k,u'_k分别表示所得到的八对匹配点中第k对匹配点归一化后的行坐标，v_k,v'_k分别表示所得到的八对匹配点中第k对匹配点归一化后的列坐标。

步骤5-2：利用步骤5-1得到的基础矩阵F，以步骤4所述的在t时刻坐标为(x,y)的特征点为例，将该特征点投影到t+dt时刻的图像，得到在t+dt时刻该特征点对应的极线l2；

l₂＝Fq

其中，q表示步骤4-1所取的特征点在t时刻的坐标(x,y)归一化后的坐标(x₁,y₁,1)，

步骤5-3：计算步骤5-2所取特征点在t+dt时刻的匹配点q'到极线l2的距离，距离公式如下：

其中q'表示步骤5-2所取特征点在t+dt时刻对应的匹配点归一化坐标(x₁',y₁',1)，

表示q'₁的转置，q₀表示步骤5-2所取特征点在t时刻的坐标(x,y)，X,Y分别表示极线l2一般式中自变量与因变量的系数。

步骤5-4：判断D与阈值m关系，如果距离大于阈值,则认为所取的特征点是一个动态并保存其位置坐标点。其中，m值为6。

步骤5-5：重复步骤5-2至步骤5-4，直至步骤3所述在物体边界框内特征点都判断完毕。

下图5所示为对极约束平面示意图。

步骤6包括：

步骤6-1：将步骤5所获得的动态特征点位置保存，对比步骤5得到的动态特征点的位置与步骤2获得的物体边界框的位置，获得动态特征点落在物体区域的数目；

步骤6-2：判断步骤6-1获取到的动态特征点落在物体区域的数目与阈值n的大小，若数目大于阈值n，则认为步骤2检测到的物体边界框内物体是运动的，剔除物体边界框内所有的特征点；其中，阈值n为15；

步骤6-3：重复步骤6-1，6-2，直至步骤5得到的动态特征点与物体边界框都比较完毕，获得静态特征点，即为有效特征点。

通过步骤6-2得到，路人是运动的，三轮车和轿车是静止的。下图6所示为剔除动态特征点的结果图。

步骤7包括：

步骤7-1：根据步骤6获得的有效特征点计算位姿估计本质矩阵；

步骤7-2：根据本质矩阵恢复相机的运动信息，得到相机旋转矩阵与平移矩阵，从而估算出相机位姿。

本发明提供了一种机器人视觉导航中视觉里程计构建方法，具体实现该技术方案的方法和途径很多，以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。本实施例中未明确的各组成部分均可用现有技术加以实现。

Claims (7)

1.一种机器人视觉导航中视觉里程计构建方法，其特征在于，包括以下步骤：

步骤1，读取原始图像，通过双阈值法提取特征点并获得特征点的位置；

步骤2，通过YOLOv3算法对原始图像进行目标检测，获取物体边界框的位置；

步骤3，根据特征点的位置及物体边界框的位置，判断特征点是否在物体边界框内，剔除不在物体边界框内的特征点，获得在物体边界框内的特征点；

步骤4，对物体边界框内的特征点进行光流跟踪，获取在物体边界框内特征点的运动位置；

步骤5，根据步骤4得到的运动位置，通过光流极线法进行动态特征点的判定，判断在物体边界框内的特征点是静态特征点还是动态特征点；

步骤6，根据物体边界框内的动态特征点来判断物体运动情况，当物体边界框内动态特征点的数目达到一定阈值，则判定物体边界框内物体是运动的，剔除物体边界框内所有的特征点，获取静态物体特征点，即为有效特征点；

步骤7，根据有效特征点估算相机运动轨迹，恢复出相机旋转和平移运动，估计出相机位姿，完成视觉里程计的构建。

2.根据权利要求1所述的方法，特征在于，步骤1包括：

步骤1-1：在原始图像中任取二十个像素p₁,p₂......p₂₀，p₂₀表示选取的第20个像素，分别检测出它们的亮度Ip₁,Ip₂......Ip₂₀，设定阈值Tp₁：

Ip_i为所取的第i个像素点的亮度，i取值为1～20；

步骤1-2：在原始图像上任意选取像素p，检测出它的亮度Ip，设定阈值Tp₂，Tp₂为Ip的20％；

步骤1-3：以像素p为中心选取半径为3的圆上16个像素点；

步骤1-4：如果选取出的圆上，有连续十二个像素点的亮度大于

或者圆上有连续十二个像素点的亮度小于

则像素p这个点标记为特征点，获取这个特征点的坐标(x_r,y_r)并保存；其中，x_r表示特征点的行坐标，y_r表示特征点的列坐标；

步骤1-5：循环步骤1-2～步骤1-4，对原始图像上的每一个像素点执行相同操作，得到特征点以及特征点位置。

3.根据权利要求2所述的方法，特征在于，步骤3包括如下步骤：

步骤3-1：对比步骤1获取的特征点位置与步骤2获取的物体边界框的位置，判断特征点是否在物体边界框内；

步骤3-2：剔除不在物体边界框内的特征点，保留在物体边界框内的特征点；

步骤3-3：重复步骤3-1～步骤3-2，直至步骤1获取的特征点与步骤2获取的物体边界框都比较完毕。

4.根据权利要求3所述的方法，特征在于，步骤4包括：

步骤4-1：任取一个在步骤3获取的在物体边界框内的特征点(x,y)，记录当前时刻为t，记该特征点在t+dt时刻，运动到(x+dx,y+dy)处；

其中，x表示所取的特征点在t时刻的行坐标，y表示所取的特征点在t时刻的列坐标，(x,y)表示所取的特征点在t时刻的坐标，t+dt表示摄像头读取到下一帧图像的时刻，x+dx表示所取的特征点在t+dt时刻的行坐标，y+dy表示所取的特征点在t+dt时刻的列坐标，(x+dx,y+dy)表示所取的特征点在t+dt时刻的坐标；

步骤4-2：设定同一个空间点的像素灰度值固定不变，以I(x,y,t)描述所取的特征点在t时刻的灰度值，I(x+dx,y+dy,t+dt)表示所取的特征点在t+dt时刻的灰度值，则I(x+dx,y+dy,t+dt)＝I(x,y,t)；通过泰勒级数对方程进行展开，保留一阶项，得到：

由于灰度值不变，则有

其中，

表示当前帧图像在所取的特征点处x方向的偏导，

Ix表示当前帧图像在所取的特征点处x方向的梯度，

表示当前帧图像在所取的特征点处y方向的偏导，

Iy表示当前帧图像在所取的特征点处y方向的梯度，

表示当前帧图像灰度对时间的变化量的偏导，

It表示当前帧图像灰度对时间的变化量，

u,v分别表示所取的特征点在x轴上的运动速度、y轴上的运动速度；

步骤4-3：设定一个窗口内的像素具有相同的运动，则在一个大小为3×3的窗口中，利用最小二乘法，则有

其中，

It₁至It₉为所述的大小为3×3的窗口中的九个像素点的灰度值，Ix₁至Ix₉为所述的大小为3×3的窗口中的九个像素点x方向的梯度，Iy₁至Iy₉为所述的大小为3×3的窗口中的九个像素点y方向的梯度；A^T表示矩阵A的转置，(A^TA)^-1表示矩阵A^T与矩阵A乘积的逆矩阵；

步骤4-4：根据步骤4-3得到的所取的特征点在x轴，y轴上的运动速度

得到所取的特征点在t+dt时刻的(x+dx,y+dy)＝(x+udt,y+vdt)；

步骤4-5：通过对连续两帧图像进行光流追踪，获得步骤3在物体边界框内特征点在下一帧图像对应的匹配点。

5.根据权利要求4所述的方法，特征在于，步骤5包括：

步骤5-1：任取步骤4-5所获得的八对匹配点，计算基础矩阵F，计算公式如下：

其中，q_k＝(u_k,v_k,1),q′_k＝(u′_k,v′_k,1)分别表示所得到的八对匹配点中第k对匹配点归一化后的坐标，k的取值为1～8；u_k,u_k'分别表示所得到的八对匹配点中第k对匹配点归一化后的行坐标，v_k,v_k'分别表示所得到的八对匹配点中第k对匹配点归一化后的列坐标；

步骤5-2：利用步骤5-1得到的基础矩阵F，以步骤4在t时刻坐标为(x,y)的特征点为例，将所述特征点投影到t+dt时刻的图像，得到在t+dt时刻该特征点对应的极线l2：

l₂＝Fq，

其中，q表示步骤4-1所取的特征点在t时刻的坐标(x,y)归一化后的坐标(x₁,y₁,1)。

步骤5-3：计算步骤5-2所取特征点在t+dt时刻的匹配点q'到极线l2的距离D，距离公式如下：

其中q'表示步骤5-2所取特征点在t+dt时刻对应的匹配点归一化坐标(x₁',y₁',1)，

表示q₁′的转置，q₀表示步骤5-2所取特征点在t时刻的坐标(x,y)，X,Y分别表示极线l2一般式中自变量与因变量的系数；

步骤5-4：判断距离D与阈值m关系，如果距离D大于阈值m，则判定所取的特征点是一个动态点；

步骤5-5：重复步骤5-2～步骤5-4，直至在物体边界框内特征点都判断完毕。

6.根据权利要求5所述的方法，其特征在于，步骤6包括：

步骤6-1：将步骤5所获得的动态特征点位置保存，对比步骤5得到的动态特征点的位置与步骤2获得的物体边界框的位置，获得动态特征点落在物体区域的数目；

步骤6-2：判断步骤6-1获取到的动态特征点落在物体区域的数目与阈值n的大小，如果数目大于阈值n，则判定步骤2检测到的物体边界框内物体是运动的，剔除物体边界框内所有的特征点；

步骤6-3：重复步骤6-1～步骤6-2，直至步骤5得到的动态特征点与物体边界框都比较完毕，获得静态特征点，即为有效特征点。

7.根据权利要求6所述的方法，特征在于，步骤7包括如下步骤：

步骤7-1：根据步骤6获得的有效特征点计算位姿估计本质矩阵；

步骤7-2：根据本质矩阵恢复相机的运动信息，得到相机旋转矩阵与平移矩阵，从而估算出相机位姿。

Images