CN106482711B - 一种基于灰度特征提取和稠密光流法的室内定位方法 - Google Patents

Abstract

一种基于灰度特征提取和稠密光流法的室内定位方法，本发明涉及室内定位方法。本发明为了解决现有室内定位技术成本高、所需时间较长、工序复杂以及传统光流法精度低的问题。本发明步骤为：一：使用相机传感器进行视频采集并标识出录像的起点；二：每隔时间t进行一帧图像的截取；三：得到两幅图像运动过程的水平和垂直速度；四：计算图像的灰度平均值，选取灰度值大于平均灰度值的像素点，记录每个选取的像素的位置；五、得到去噪后的平均水平速度和平均垂直速度六、计算两幅相邻图像运动过程中的水平位移和垂直位移；七、重复执行步骤一至步骤六，得到行进路线以及每个时间点的具***置。本发明应用于室内场景定位技术领域。

Description

一种基于灰度特征提取和稠密光流法的室内定位方法

技术领域

本发明涉及基于灰度特征提取和稠密光流法的室内定位方法。

背景技术

近年来，由于相应设备和技术广泛的进步以及实际应用场景中对基于位置服务的无缝解决方案的必要性，室内定位***获得了广泛的关注和研究，这些***开辟了一种全自动目标定位检测的新技术领域。在室外环境下，传统全球卫星导航***(GlobalNavigation Satellite System，GNSS)的定位结果是对用户位置进行精确估计的最可靠来源之一。然而在室内或封闭的环境中，由于卫星信号会受到严重的衰减导致定位精度的严重损失，GNSS是不可行的。

目前，室内定位***技术的研究热点主要包括WiFi定位技术和蓝牙定位技术。其中，WiFi定位技术具有无线接入点已部署范围广和可扩散繁殖的特点，利用用户智能手机检测到的各无线接入点的信号强度完成对用户位置的估计。然而，该技术严重依赖无线接入点的个数，并且在商场等实际环境的进口和出口等周边区域的定位精度急剧下降。蓝牙定位技术通过利用用户接收的蓝牙信号的强度完成对用户位置的估计，并且可实现1米的定位精度，然而该技术在探寻蓝牙信号阶段存在的高延迟现象限制了它实际中的应用。综合已经以上室内定位技术的优缺点，需要一种可扩展性强，部署成本低，所需时延小，定位精度高且稳定的定位***。由于图像中包含了丰富的信息，带有图像传感器的智能手机的高普及性和可扩展移植性，以及计算机处理技术大大增强等优势，使得基于视觉的室内定位技术成为新的研究热点。

视觉室内定位技术无需额外的部署，只需通过用户终端拍摄的室内场景图片，配合已建立的室内场景数据库，即可完成对用户位置更为准确且鲁棒性更强的估计结果。并且由于图像包含丰富的场景信息，视觉室内定位还可以为用户提供更好的可视化服务，这是其他定位方式无法比拟的。目前已经提出的视觉室内定位技术主要采用位置指纹算法，通过将终端拍摄的场景图像与用户位置共同建立数据库，然后采用快速检索算法得出数据库中最匹配的场景图像及对应位置，完成定位服务。

然而主流的室内视觉定位算法研究，是提前建立图像数据库，在数据库中加入位置距离等信息。然后在线阶段，通过终端获取图片，通过特征匹配等方法与数据库中的图片进行匹配，然后进行相应的计算得到用户当前的位置。建立数据库的过程属于离线过程，数据获取会耗费大量的时间，而得到的数据又需要占一定的存储空间，于此同时，获取的数据也必然存在着一定的误差。因此，本文采用基于光流法的室内视觉定位算法进行室内定位，这种算法不需要离线建立数据库，因此节省了大量的时间，大幅度降低了了对存储设备的需求，而且避免了离线建立数据库产生的误差对定位的影响。

在进行光流法进行室内视觉定位的时候，由于受光照、障碍、图片中的特征是否明显等因素的影响，光流法的定位会产生一定的误差，因此通过对图片进行特征提取、特征选择与特征匹配的方法，对图片进行处理，从而从整幅图片的光流中选取置信区间。虽然算法复杂度上升了，但是定位精度提高了。虽然算法复杂度上升了，但是位所花费的总时间相对主流定位方法仍然是大幅度减小了。

发明内容

本发明是为了解决现有室内定位技术成本高、所需时间较长、工序复杂以及传统光流法精度低的问题，而提出的一种基于灰度特征提取和稠密光流法的室内定位方法。

一种基于灰度特征提取和稠密光流法的室内定位方法按以下步骤实现：

步骤一：人在室内行走或车在行进过程中，使用相机传感器进行固定方向和视角的视频采集，并标识出录像的起点；

步骤二：对步骤一得到的视频每隔时间T进行一帧图像的截取；

步骤三：对每两幅相邻图像利用稠密型光流法进行计算，得到这两幅图像运动过程的水平和垂直速度(u,v)；

步骤四：对图像进行灰度变换，得到灰度图像后计算图像的灰度平均值，选取灰度值大于平均灰度值的像素点，记录每个选取的像素的位置用(l_l(m),l_v(m))表示，其中m为第m个像素，l_l为像素的横坐标集合，l_v为像素的纵坐标集合；

步骤五：根据(l_l(m),l_v(m))确定的位置，进行光流的选取得到水平速度u_n((l_l(m),l_v(m)))和垂直速度v_n((l_l(m),l_v(m)))，根据得到的所有点的水平速度与垂直速度，计算平均水平速度和平均垂直速度，并进行噪点的去除，得到去噪后的平均水平速度u'和平均垂直速度；

步骤六：根据相机标定原理将步骤五得到的平均水平速度和平均垂直速度，转换为三维空间的平均水平速度和平均垂直速度，并根据时间间隔T，计算两幅相邻图像运动过程中的水平位移和垂直位移；

步骤七：重复执行步骤一至步骤六，直至全部位移计算完毕，并根据起点，得到行进路线以及每个时间点的具***置。

发明效果：

本发明考虑了主流室内视觉定位算法花费较大前期时间以及人力的投入(主流室内视觉定位算法需要提前建立数据库，在走廊中根据需要每隔一段距离便需要采集一副图片并且进行相应的距离测量如到墙的距离等，而且必须保证该位置附近有明显的特征不同，且不同地方特征尽可能不同)，数据库的建立要求对场景密集踩点，每一个点固定位置照片并且准确测量距离，因此每一个点花费的时间至少3-5秒甚至更多，这在一个如机场大小的室内，工作量是不可想象的。在建立好数据库后，用户进行照相并将照片和数据库匹配，如果数据库中场景相似的图片较多很容易产生误匹配。因此我提出的一种基于灰度特征提取和稠密光流法的室内定位方法，不需要提前建立数据库，大大节省了时间，而且由于定位过程中不需要进行和数据库的匹配，因此完全不存在误匹配的问题。相对于传统的室内光流法定位算法，由于我考虑到了室内光线明暗变化的问题，提取灰度较高的点(灰度较大的点收光影变化较小)进行最终速度的计算，从而提高了传统光流法的精度，在光线较好的白天误差由10cm降低到1cm左右。

附图说明

图1为本发明实施例的稠密型光流法的结合灰度特征提取的室内定位算法的流程图；

图2为本发明实施例的30个单步80cm的定位误差累计分布图；

图3为本发明实施例的30个单步80cm的定位结果图。

具体实施方式

具体实施方式一：如图1所示，一种基于灰度特征提取和稠密光流法的室内定位方法包括以下步骤：

步骤一：人在室内行走或车在行进过程中，使用相机传感器进行固定方向和视角的视频采集，并标识出录像的起点；

步骤二：对步骤一得到的视频每隔时间T进行一帧图像的截取；

步骤三：对每两幅相邻图像利用稠密型光流法进行计算，得到这两幅图像运动过程的水平和垂直速度(u,v)；

步骤四：对图像进行灰度变换，得到灰度图像后计算图像的灰度平均值，选取灰度值大于平均灰度值的像素点，记录每个选取的像素的位置用(l_l(m),l_v(m))表示，其中m为第m个像素，l_l为像素的横坐标集合，l_v为像素的纵坐标集合；

步骤五：根据(l_l(m),l_v(m))确定的位置，进行光流的选取得到水平速度u_n((l_l(m),l_v(m)))和垂直速度v_n((l_l(m),l_v(m)))，根据得到的所有点的水平速度与垂直速度，计算平均水平速度和平均垂直速度，并进行噪点的去除，得到去噪后的平均水平速度和平均垂直速度；

步骤六：根据相机标定原理将步骤五得到的平均水平速度和平均垂直速度，转换为三维空间的平均水平速度和平均垂直速度，并根据时间间隔T，计算两幅相邻图像运动过程中的水平位移和垂直位移；

步骤七：重复执行步骤一至步骤六，直至全部位移计算完毕，并根据起点，得到行进路线以及每个时间点的具***置。

本发明适用于室内定位，例如用户在没有数据库的室内场景。本发明的在进行定位之前只需要已知起点，且摄像头相对于人或车的移动必须是硬链接，不需要提取建立数据库，在人行走或者车行进的过程中，以不变的角度和方向录制视频。

具体实施方式二：本实施方式与具体实施方式一不同的是：所述步骤三中对每两幅相邻图像利用稠密型光流法进行计算的具体过程为：

步骤三一：对两幅图像进行灰度变换，得到两幅灰度图像；

步骤三二：计算两幅图像的时间梯度矩阵f_t、第一幅图像的水平方向梯度f_u以及第一幅图片的垂直方向梯度f_v；

其中所述x_i为一幅图像中水平方向上第i个像素的横坐标，y_j为一幅图像中垂直方向上第j个像素的纵坐标，t_k为第k幅图像的时间；

步骤三三：计算光流平均分量与

其中为由第k幅图像的横坐标为i，纵坐标为j的像素及周围像素计算得到的水平方向平均速度；为由第k幅图像的横坐标为i，纵坐标为j的像素及周围像素计算得到的垂直方向平均速度；

步骤三四：根据图像的拉普拉斯算子，将和带入公式(6)；

其中为根据图像的拉普拉斯算子定义计算u的拉普拉斯变换，为根据图像的拉普拉斯算子定义计算v的拉普拉斯变换，u_xx为图像的水平方向的二阶梯度矩阵，u_yy为图像的垂直方向的二阶梯度矩阵；根据拉普拉斯的估计计算 (因为u和v是两个矩阵，按照图像的拉普拉斯算子算法来计算这个矩阵，本来拉普拉斯算子是对图像的灰度矩阵进行处理，这里是对速度矩阵进行处理，公式6是光流基本方程得来，为了能够将其中u_xx与u_yy的计算替换成 的估计，从而得到三五里面的迭代公式)；

步骤三五：设置光流初始值以及迭代次数，将u⁰和v⁰记为光流估计的初始值；u⁰为迭代开始前设定的水平方向速度初值，v⁰为迭代开始前设定的垂直方向速度初值；

步骤三六：进行光流的迭代计算，得到水平速度和垂直速度；

其中为迭代n次后得到的水平方向速度，为迭代n次后得到的垂直方向速度，a为平滑因子。

其它步骤及参数与具体实施方式一相同。

具体实施方式三：本实施方式与具体实施方式一或二不同的是：所述步骤四中对图像进行灰度变换，得到灰度图像后计算图像的灰度平均值，选取灰度值大于平均灰度值的像素点的具体过程为：

步骤四一、对图像进行灰度变换，变换为灰度图像，并计算整幅图像的平均灰度，记为

步骤四二、设置灰度阈值为

步骤四三、建立像素坐标系，相邻两个像素的距离为1，原点在图像的左上角，水平方向坐标记为l_l，垂直方向坐标记为l_v；从第一行第一个像素开始，每个像素的灰度与阈值相比，若大于等于阈值，则保存坐标；若小于阈值，则不保存。像素坐标系为二维坐标系，即只有横纵坐标，横坐标X轴为水平方向，纵坐标Y轴为垂直方向。

其它步骤及参数与具体实施方式一或二相同。

具体实施方式四：本实施方式与具体实施方式一至三之一不同的是：所述步骤五中得到去噪后的平均水平速度和平均垂直速度的具体过程为：

步骤五一：根据步骤四三保存的水平坐标l_l与垂直坐标l_v，得到新的水平速度集u_n和新的垂直水平速度集v_n；

步骤五二：用新的水平速度集u_n和新的垂直速度集v_n计算平均水平速度和平均垂直速度

步骤五三：设定水平速度阈值为和垂直速度阈值为和将u_n中所有速度值与和比较，若数值在和之间则保留，否则则剔除，得到新的水平速度集将v_n中所有速度值与和比较，若数值在和之间则保留，否则则剔除，得到新的垂直速度集计算水平平均速度和垂直平均速度

其它步骤及参数与具体实施方式一至三之一相同。

具体实施方式五：本实施方式与具体实施方式一至四之一不同的是：所述步骤六中计算两幅相邻图像运动过程中的水平位移和垂直位移的具体过程为：

步骤六一：成像平面到摄像机之间的坐标转换，像平面坐标系(像平面坐标系与像素坐标系的区别在于像平面坐标系中的距离为实际测量距离)的原点O在像素坐标系下的坐标变为(u₀,v₀)，将单位像素在X轴和Y轴方向上的物理长度记成dx和dy，得到两个坐标系之间有如下数学关系：

其中所述s_x是摄像机成像的倾斜因子，成像平面两个坐标轴不会是正好成90°此参数表示成像平面一定的畸变。但是在现代制造工艺下，其值可以近似为零。

u₀为像平面坐标系的原点O在像素坐标系下的横坐标，v₀为像平面坐标系的原点O在像素坐标系下的纵坐标，dx为单位像素在X轴方向上的长度，dy为单位像素在Y轴方向上的长度，Xu为像素点在像素平面坐标系下的横坐标，Yv为像素点在像素平面坐标系下的纵坐标，x为像素点在像平面坐标系下的横坐标，y为像素点在像平面坐标系下的纵坐标；

步骤六二：成像平面到摄像机之间的坐标转换，得到像平面坐标系和摄像机坐标系之间的如下数学关系：

其中所述x_c为该点在摄像机坐标系下的X轴坐标，y_c为该点在相机三维坐标系下的Y轴坐标，z_c为该点在相机三维坐标系下的Z轴坐标，f_c为相机的焦距；

步骤六三：摄像机坐标系到世界坐标系的坐标转换，通过单位正交旋转矩阵R和平移向量t获得世界坐标系与摄像机坐标系的映射关系：

其中所述x_w为该点在世界三维坐标系下的X轴坐标，y_w为该点在世界三维坐标系下的Y轴坐标，z_w该点在世界三维坐标系下的Z轴坐标，R为单位正交旋转矩阵，t为平移向量；

摄像机坐标系中，摄像机在原点，x轴向右，z轴向前(朝向屏幕内或摄像机方向)，y轴向上(不是世界的上方而是摄像机本身的上方)。

步骤六四：由得到的三维世界坐标系的速度，与截取图像的时间相乘，得到两幅相邻图片运动过程中的水平位移d_u和垂直位移d_v。

本发明的变量与物理意义列表如下：

其它步骤及参数与具体实施方式一至四之一相同。

实施例一：

步骤一：人在室内行走或车在行进过程中，使用相机传感器进行固定方向和视角的视频采集，并标识出录像的起点；摄像头相对于人或车的移动必须是硬连接；所处的室内必须有丰富的特征(正常室内即可)，但是不能是整个屋子只有一个颜色且毫无其他特征，比如整个走廊除了白色只有白色，因此选取哈工大科技园2A栋12楼走廊为实验环境，在光线充足的白天，并且将走廊中的等全部打开以尽量减少光线变化，实验人员手持苹果手机，以大约每秒0.8米的速度行走24米的距离，并录制一段视频。

步骤二：根据需要以及行进的快慢，对步骤一得到的视频每隔时间t进行一帧图像的截取，t＝0.5s；

步骤三：对每两幅相邻图像利用稠密型光流法进行计算，得到这两幅图像运动过程的水平和垂直速度(u,v)，其中u和v为和图像尺寸相同大小的矩阵；

步骤三一：对两幅图像进行灰度变换，得到两幅灰度图像；

步骤三二：计算两幅图像的时间梯度矩阵f_t、第一幅图像的水平方向梯度f_u以及第一幅图片的垂直方向梯度f_v；

其中所述x_i为一幅图像中水平方向上第i个像素的横坐标，y_j为一幅图像中垂直方向上第j个像素的纵坐标，t_k为第k幅图像的时间；

步骤三三：计算光流平均分量与

其中为由第k幅图像的横坐标为i，纵坐标为j的像素及周围像素计算得到的水平方向平均速度；为由第k幅图像的横坐标为i，纵坐标为j的像素及周围像素计算得到的垂直方向平均速度；

步骤三四：根据图像的拉普拉斯算子，计算以及将计算得到的数据带入公式(6)；

其中为根据图像的拉普拉斯算子定义计算u的拉普拉斯变换，为根据图像的拉普拉斯算子定义计算v的拉普拉斯变换，u_xx图像的水平方向的二阶梯度矩阵，u_yy为图像的垂直方向的二阶梯度矩阵；

步骤三五：设置光流初始值以及迭代次数，将u⁰和v⁰记为光流估计的初始值；u⁰为迭代开始前设定的水平方向速度初值，v⁰为迭代开始前设定的垂直方向速度初值；初始值为0，迭代次数根据需要设置，这里设置为100；

步骤三六：进行光流的迭代计算，得到水平速度和垂直速度；

其中为迭代n次后得到的水平方向速度，为迭代n次后得到的垂直方向速度，a为平滑因子。

步骤四：对图像进行灰度变换，得到灰度图像后计算图像的灰度平均值，选取灰度值大于平均灰度值的像素点(也可以根据场景需要提高或降低灰度选取的阈值选择)，记录每个选取的像素的位置用(l_l(m),l_v(m))表示，其中m为第m个像素，l_l为像素的横坐标集合，l_v为像素的纵坐标集合；

步骤五：根据(l_l(m),l_v(m))确定的位置，进行光流的选取得到水平速度u_n((l_l(m),l_v(m)))和垂直速度v_n((l_l(m),l_v(m)))，根据得到的所有点的水平速度与垂直速度，计算平均水平速度和平均垂直速度并进行噪点的去除，得到去噪后的平均水平速度和平均垂直速度

步骤六：根据相机标定原理将步骤五得到的平均水平速度和平均垂直速度，转换为三维空间的平均水平速度和平均垂直速度，并根据时间间隔t，计算两幅相邻图像运动过程中的水平位移和垂直位移；

步骤七：重复执行步骤一至步骤六，直至全部位移计算完毕，并根据起点，得到行进路线以及每个时间点的具***置。

30个单步80cm的定位误差累计分布图，如图2所示；30个单步80cm的定位结果图，如图3所示。

Claims (4)

1.一种基于灰度特征提取和稠密光流法的室内定位方法，其特征在于，所述基于灰度特征提取和稠密光流法的室内定位方法包括以下步骤：

步骤一：人在室内行走或车在行进过程中，使用相机传感器进行固定方向和视角的视频采集，并标识出录像的起点；

步骤二：对步骤一得到的视频每隔时间T进行一帧图像的截取；

步骤三：对每两幅相邻图像利用稠密型光流法进行计算，得到这两幅图像运动过程的水平和垂直速度(u,v)；

步骤四：对图像进行灰度变换，得到灰度图像后计算图像的灰度平均值，选取灰度值大于平均灰度值的像素点，记录每个选取的像素的位置用(l_l(m),l_v(m))表示，其中m为第m个像素，l_l为像素的横坐标集合，l_v为像素的纵坐标集合；

步骤五：根据(l_l(m),l_v(m))确定的位置，进行光流的选取得到水平速度u_n((l_l(m),l_v(m)))和垂直速度v_n((l_l(m),l_v(m)))，根据得到的所有点的水平速度与垂直速度，计算平均水平速度和平均垂直速度并进行噪点的去除，得到去噪后的平均水平速度和平均垂直速度

步骤六：根据相机标定原理将步骤五得到的平均水平速度和平均垂直速度，转换为三维空间的平均水平速度和平均垂直速度，并根据时间间隔T，计算两幅相邻图像运动过程中的水平位移和垂直位移；

步骤七：重复执行步骤一至步骤六，直至全部位移计算完毕，并根据起点，得到行进路线以及每个时间点的具***置；

所述步骤三中对每两幅相邻图像利用稠密型光流法进行计算的具体过程为：

步骤三一：对两幅图像进行灰度变换，得到两幅灰度图像；

步骤三二：计算两幅图像的时间梯度矩阵f_t、第一幅图像的水平方向梯度f_u以及第一幅图片的垂直方向梯度f_v；

其中所述x_i为一幅图像中水平方向上第i个像素的横坐标，y_j为一幅图像中垂直方向上第j个像素的纵坐标，t_k为第k幅图像的时间；

步骤三三：计算光流平均分量与

其中为由第k幅图像的横坐标为i，纵坐标为j的像素及周围像素计算得到的水平方向平均速度；为由第k幅图像的横坐标为i，纵坐标为j的像素及周围像素计算得到的垂直方向平均速度；

步骤三四：根据图像的拉普拉斯算子，将▽²u＝u_xx+u_yy和▽²v＝v_xx+v_yy带入公式(6)；

其中▽²u为根据图像的拉普拉斯算子定义计算u的拉普拉斯变换，▽²v为根据图像的拉普拉斯算子定义计算v的拉普拉斯变换，u_xx为图像的水平方向的二阶梯度矩阵，u_yy为图像的垂直方向的二阶梯度矩阵；根据拉普拉斯的估计计算

步骤三五：设置光流初始值以及迭代次数，将u⁰和v⁰记为光流估计的初始值；u⁰为迭代开始前设定的水平方向速度初值，v⁰为迭代开始前设定的垂直方向速度初值；

步骤三六：进行光流的迭代计算，得到水平速度和垂直速度；

其中为迭代n次后得到的水平方向速度，为迭代n次后得到的垂直方向速度，a为平滑因子。

2.根据权利要求1所述的一种基于灰度特征提取和稠密光流法的室内定位方法，其特征在于，所述步骤四中对图像进行灰度变换，得到灰度图像后计算图像的灰度平均值，选取灰度值大于平均灰度值的像素点的具体过程为：

步骤四一、对图像进行灰度变换，变换为灰度图像，并计算整幅图像的平均灰度，记为

步骤四二、设置灰度阈值为

步骤四三、建立像素坐标系，相邻两个像素的距离为1，水平方向坐标记为l_l，垂直方向坐标记为l_v；从第一行第一个像素开始，每个像素的灰度与阈值相比，若大于等于阈值，则保存坐标；若小于阈值，则不保存。

3.根据权利要求2所述的一种基于灰度特征提取和稠密光流法的室内定位方法，其特征在于，所述步骤五中得到去噪后的平均水平速度和平均垂直速度的具体过程为：

步骤五一：根据步骤四三保存的水平坐标l_l与垂直坐标l_v，得到新的水平速度集u_n和新的垂直水平速度集v_n；

步骤五二：用新的水平速度集u_n和新的垂直速度集v_n计算平均水平速度和平均垂直速度

步骤五三：设定水平速度阈值为和垂直速度阈值为和将u_n中所有速度值与和比较，若数值在和之间则保留，否则则剔除，得到新的水平速度集将v_n中所有速度值与和比较，若数值在和之间则保留，否则则剔除，得到新的垂直速度集计算水平平均速度和垂直平均速度

4.根据权利要求3所述的一种基于灰度特征提取和稠密光流法的室内定位方法，其特征在于，所述步骤六中计算两幅相邻图像运动过程中的水平位移和垂直位移的具体过程为：

步骤六一：成像平面到摄像机之间的坐标转换，像平面坐标系的原点O在像素坐标系下的坐标变为(u₀,v₀)，将单位像素在X轴和Y轴方向上的长度记成dx和dy，得到两个坐标系之间有如下数学关系：

其中所述s_x是摄像机成像的倾斜因子，u₀为像平面坐标系的原点O在像素坐标系下的横坐标，v₀为像平面坐标系的原点O在像素坐标系下的纵坐标，dx为单位像素在X轴方向上的长度，dy为单位像素在Y轴方向上的长度，Xu为像素点在像素平面坐标系下的横坐标，Yv为像素点在像素平面坐标系下的纵坐标，x为像素点在像平面坐标系下的横坐标，y为像素点在像平面坐标系下的纵坐标；

步骤六二：成像平面到摄像机之间的坐标转换，得到像平面坐标系和摄像机坐标系之间的如下数学关系：

其中所述x_c为该点在摄像机坐标系下的X轴坐标，y_c为该点在相机三维坐标系下的Y轴坐标，z_c为该点在相机三维坐标系下的Z轴坐标，f_c为相机的焦距；

步骤六三：摄像机坐标系到世界坐标系的坐标转换，通过单位正交旋转矩阵R和平移向量t获得世界坐标系与摄像机坐标系的映射关系：

其中所述x_w为该点在世界三维坐标系下的X轴坐标，y_w为该点在世界三维坐标系下的Y轴坐标，z_w该点在世界三维坐标系下的Z轴坐标，R为单位正交旋转矩阵，t为平移向量；

步骤六四：由得到的三维世界坐标系的速度，与截取图像的时间相乘，得到两幅相邻图片运动过程中的水平位移d_u和垂直位移d_v。