CN105913410A

CN105913410A - 一种基于机器视觉的远距离运动物体高度测量的装置与方法

Info

Publication number: CN105913410A
Application number: CN201610119781.5A
Authority: CN
Inventors: 仝卫国; 刘士波; 汪淼依泉; 孙艺萌
Original assignee: North China Electric Power University
Current assignee: North China Electric Power University
Priority date: 2016-03-03
Filing date: 2016-03-03
Publication date: 2016-08-31

Abstract

本发明公开了一种基于机器视觉的远距离运动物体高度测量的装置与方法，装置主要由双目视觉同步采集系统、图像数据处理系统、无线数据传输单元、远程监控终端及部分组成，该装置的图像采集系统可实现双目摄像机同步采集图像，为后续处理提供较为准确的数据；图像处理系统采用FPGA加DSP结构，由FPGA对数据进行预处理，再由DSP芯片对数据进行后续处理，减轻了DSP运算数据量，提高了装置的实时性；在图像特征匹配阶段，采用Harris角点探测器实现子像素级的特征提取，提高了运算精度；在高度测量阶段，采用BP神经网络作为误差校正环节，提高了测量精度。该基于机器视觉的远距离运动物体高度测量装置和方法有效解决了对远距离的运动物体的高度测量问题。

Description

一种基于机器视觉的远距离运动物体高度测量的装置与方法

技术领域

本发明属于机器视觉测量领域，设计的一种基于机器视觉原理对远距离运动物体高度进行测量的装置与方法可应用于海上运输、陆上交通、防止输电线被通行物刮断等有限高要求的领域。

背景技术

机器视觉利用计算机模拟人的视觉或与人类视觉有关的智能行为，从所获取的图像中提取信息，对客观世界的三维景物和物体进行形态和运动识别，在现实生活中得到了广泛应用。双目立体视觉技术是机器视觉的一个重要分支，其与人类的立体感知过程十分类似，直接模拟人类视觉处理景物的方式，简便可靠。双目立体视觉技术应用前景极大。特别是进入新世纪以来，随着技术手段的不断发展，双目立体视觉技术被越来越广泛的应用在生活的各个领域中，如产品的检测与测量，医学影像的三维分析、航空照片与卫星照片的解释、三维地图绘制等许多方面，在生产生活中发挥了极大的作用。

对运动目标的检测分析是计算机视觉任务中的一项重要内容，在机器人视觉导航、公共场景监控、军事、航天、陆上交通、海上运输等方面有着广泛的应用。但是，在该技术的实际研究过程中，大多数工作是基于单目视觉展开的。单目视觉与双目视觉皆可完成运动物体的检测与跟踪任务。而且，单目视觉的信息量小，每次只处理一张图片，运算速度较快。但是，单目视觉会丢失掉场景的三维信息，得到的目标运动信息只是相对的，双目视觉系统可以提取立体图像对之间的视差信息，在一定程度上恢复场景的三维信息。当需要对目标进行实际的三维位置和深度测量时，双目立体视觉有着不可替代的优点。

在海上运输领域，船舶经常会通过一些桥梁，桥梁的高度是一定的，因此通过的船舶高度则不能过高，否则会对桥体造成损伤；而在陆上，立交桥，公路隧道，有输电线跨域的道段以及一些必要场所都有限高要求，如果通行物过高会对立交桥、隧道、输电线和一些其他设施造成损坏甚至是损毁，既造成了公私财物的损失，也影响了交通。虽然采取了一些措施如设立限高标志等，但总是无法避免碰撞的发生。以船舶过桥碰撞问题为例，据最新的资料显示，在最近的三十年里，世界各地的船撞桥事故多达3000余起。而最严重的即导致整个桥梁塌陷的就超过了100余起，其他领域的碰撞事故更是不胜枚举。在陆上交通方面，所采取的方式大多也只是设立限高标识或是设立限高杆，并没有有效的提前预测预防措施。而在海上运输方面，其解决方案可分为被动防撞和主动防撞两类。被动防撞的研究主要是为了增强桥体本身的防撞能力；而主动防撞的研究内容主要是通过各种措施来预防碰撞的发生。主动防撞措施可以在潜在碰撞发生前做出判断，避免碰撞的发生，防患于未然，将风险降到最低，对桥梁的安全运行更加有保障。然而目前的大部分研究主要是针对被动防撞开展的，主动防撞措施的相关研究还较少。因此，海上运输、陆上交通和输电线防刮断等领域亟需能有效检测通行物高度的测量装置对有潜在超高危险的通行物进行高度进行测量，以提前采取措施。

本发明利用机器视觉原理测量对较远距离(如陆上100米以外，海上1000米以外)的船舶、汽车等通行物进行高度测量，对高度较高易发生碰撞的车辆、船舶等进行较早预测，避免通行物与桥体、输电线等的碰撞，提出了一种提前预测的解决方案。

发明内容

本发明提出了一种基于机器视觉的远距离运动物体高度测量的装置和方法，用于解决海上运输，陆上交通等的限高通行领域的通行物高度预报问题。

为此，本发明提出了一种基于机器视觉的远距离运动物体高度测量装置，包括：双目视觉同步采集系统，图像数据处理系统，无线数据传输单元，远程监控终端。

进一步地，所述双目视觉同步采集系统，由长焦距镜头，双目图像传感器(1)，FPGA同步控制电路(2)，数据存储器SDRAM(3)，数据输出接口(4)，供电电路组成。其特征在于，FPGA(2)分别和双目图像传感器(1)、数据存储器SDRAM(3)和数据输出接口(4)相连；双目图像传感器(1)再和数据存储器SDRAM(3)相连；数据存储器SDRAM(3)再和数据输出接口(4)相连；由FPGA产生场同步信号和行同步信号，保证两图像传感器(1)同步工作；同时由FPGA(2)产生对数据存储器SDRAM(3)和数据输出接口(4)的控制信号。由长焦距镜头和双目图像传感器所组成的双目摄像系统，其左摄像机系统和右摄像机系统的基线距应保持一定距离，以保证较远的测量距离；同时左右摄像系统的光轴保持水平。

进一步的，所述图像数据处理系统，由DSP芯片(TMS320C6416)(9)，FPGA(XC4VLX80)(8)，JTAG接口，复位控制电路，时钟电路，输入数据端口(7)，输出数据端口(10)，数据存储器SDRAM(11)，FPGA程序存储器EPRAM(5)，DSP程序存储器FLASH(6)和供电电路组成。其特征在于：FPGA(XC4VLX80)(8)从输入数据端口(7)中读入图像数据，并存入数据存储器SDRAM(11)，同时由FPGA(XC4VLX80)(8)读出数据存储器SDRAM(11)数据并对图像进行滤波等预处理，并将预处理结果存入数据存储器SDRAM(11)；FPGA(XC4VLX80)(8)负责对DSP芯片(TMS320C6416)(9)发出控制命令，使DSP芯片(TMS320C6416)(9)对数据存储器SDRAM(11)中的预处理结果进行特征匹配、三维重建等运算；输出数据端口(10)负责将DSP芯片(TMS320C6416)(9)运算结果输出到无线数据传输单元。

本发明还提供了一种基于机器视觉的远距离运动物体高度测量方法，包括以下步骤：

步骤一：将本发明所述装置安装完毕，对该装置进行标定；

步骤二：训练基于神经网络的误差校正模型；

步骤三：由双目视觉同步采集系统对场景图像进行采样，并通过数据输出接口(4)将数据传入图像数据处理系统等待处理；

步骤四：由图像数据处理系统的FPGA(XC4VLX80)(8)对数据进行预处理；

步骤五：由图像数据处理系统的DSP芯片(TMS320C6416)(9)对数据进行处理，计算目标的高度信息；

步骤六：将图像及运算结果通过无线数据传输单元传送给远程监控终端。

进一步地，所述步骤一的标定过程步骤如下：

步骤(1)：对权利要求1所述装置进行标定，获得两摄像机的内参数和外参数；

步骤(2)：对双目测量装置进行水平标定，具体方法如下：

用双目摄像机拍摄一个与铅垂线平行的已知长度的标定杆，记其上端点为M，下端点为N，中间点为E，且ME与EN的比已知；获得其在左右摄像机上的图像，其端点M在左摄像机的像为M_l，图像坐标为(x_ml,y_ml)，端点N在左摄像机的像为N_l,图像坐标为(x_nl,y_nl)，中间点E在左摄像机的像为E_l，图像坐标为(x_el,y_el)；同时端点M在右摄像机中的像为M_r，图像坐标为(x_mr,y_mr)，端点N在右摄像机的像为N_r，图像坐标为(x_nr,y_nr)，中间点E在左摄像机的像为E_r，图像坐标为(x_er,y_er)；

如果摄像机的光轴水平，则下式成立：

进一步地，所述步骤二的基于神经网络的误差校正模型训练方法如下：

步骤(1)：训练数据获取

将已知高度的标准杆放在装置前一定距离内，并使标定杆在横向范围内移动，并拍下不同位置下标定杆的图像；然后将标定杆往后移动一段距离(如五十米)，然后再在横向范围内移动，并拍下不同位置下标定杆的图像；不断向后移动一段距离(如五十米)，并重复上述操作，直至抵近该双目测量装置的测量范围；并记录下标准杆在不同位置时的顶点及杆上其他测量点的真实坐标系。

步骤(2)：训练校正模型网络

对步骤(1)所获得图像数据进行计算，计算出不同位置下的标定杆的顶点和其他测量点的计算坐标，将计算坐标作为神经网络的输入，相应的标定杆顶点及其他相应测量点的真实坐标作为期望输出，然后训练网络，最终得到基于神经网络的误差校正模型；

步骤(3)：将步骤(2)中获得的基于神经网络的误差校正模型写入数据处理系统的DSP程序存储器FLASH(6)。

进一步地，所述步骤五的数据处理过程包括如下步骤：

步骤(1)：由图像数据处理系统的DSP芯片(TMS320C6416)(9)对经过预处理步骤的图像进行目标识别：

首先从左摄像机图像中提取一个目标，然后在右摄像机图像中寻找相对应的目标；

步骤(2)：对目标所在区域进行子像素级的角点特征提取，并配对：

这里采用Harris角点探测器对角点进行子像素级别提取，采用那个的拟合曲面是高斯曲面，其函数为：

其中，拟合出的(x₀,y₀)即为角点的内插值；然后运用双目视觉约束条件和基于特征的匹配方式建立两幅图像间特征角点的对应关系。

步骤(3)：利用配对点的图像坐标计算相应点的世界坐标；

步骤(4)：运用基于神经网络的误差校正模型对步骤(3)中计算出的坐标进行校正，获得精确的世界坐标系坐标，建立稀疏的深度图；

步骤(5)：利用内插方法对步骤(4)获得的稀疏的深度图进行插值，获得整个目标的深度图；

步骤(6)：计算出目标的最高点A，其坐标为(x_ac,y_ac,z_ac)，再从目标中得到其最低点D，其坐标为(x_dc,y_dc,z_dc)，则目标高度的计算通过下式取得：

H＝y_ac-y_dc (3)

所求出的H即为所求。

进一步地，所述的世界坐标系的计算方法如下：

以左摄像机坐标系为准，右摄像机坐标系与左摄像机坐标系的转换矩阵为M＝[R T]，设现实中目标上一点P，设其在左摄像机坐标的坐标(x_c,y_c,z_c)，在右摄像机坐标系的坐标为(x_r,y_r,z_r)，在左右摄像机中所成的像点的图像坐标分别为P_l(x_pl,y_pl)和P_r(x_pr,y_pr)，基线距为B，摄像机焦距为f，则根据摄像机透视变换有：

两摄像机坐标系之间的空间位置关系可通过空间转换矩阵M表示为：

当R为单位矩阵时，摄像机相面点之间的对应关系可表示为：

由上式可以计算出特征点P在左摄像机坐标系坐标：

由此获得各个主要特征点的坐标。

本发明具有的有益效果：本发明可以对较远距离运动物体的高度进行测量，并通过增加摄像系统基线距来增加测量距离，使装置可对较远距离(如陆上100米以外，海上1000米以外)的物体高度进行测量；引入子像素级特征提取和基于神经网络的误差校正模型增加测量精度，实现了对超高物体进行提前预报，防止碰撞事件的发生，拥有良好的应用前景。

附图说明

图1基于机器视觉的远距离运动物体高度测量方法流程图

图2双目摄像机标定水平示意图

图3基于神经网络的误差校正模型示意图

图4双目视觉同步采集系统示意图

图5图像数据处理系统处理过程示意图

图6双目视觉计算高度原理示意图

图7双目视觉三维重建原理示意图

具体实施方式

下面将结合附图，详细阐述本发明所述的一种基于机器视觉的远距离运动物体高度测量的装置与方法的实施方式。

如图1所示展示了基于机器视觉的远距离运动物体高度测量方法流程，包括摄像机标定、基于神经网络的误差校正模型训练、获取双目图像、图像预处理、数据处理并获得目标高度和数据传输至监控终端六步。值得注意的是，前两步是在摄像机安装完毕后进行的，为测量装置进行后续处理所必须的步骤，实际测量过程是从第三步开始的。另外，为保证装置有着较远的测量距离(如陆上100米以上，海上1000米以上)，由长焦距镜头和双目图像传感器所组成的双目摄像系统，其左摄像机系统和右摄像机系统的基线距应保持一定距离。

第一步是对双目摄像机进行标定。双目摄像机标定可分为两个步骤。

步骤(1)：对双目摄像机系统的标定，即获得双目摄像机的内外参数。

步骤(2)：对双目测量装置进行水平校正，如图2所示，展示了双目摄像机标定水平，具体方法如下：

如果摄像机的光轴水平，则下式成立：

第二步：基于神经网络的误差校正模型的训练。

由于各种原因，双目摄像机所获得的图像会有深度损失，这会导致测量的不准确。因此，要提高测量精度，需要对数据进行补偿。本发明采用神经网络对误差进行校正。神经网络模型如图3所示，网络的输入为双目视觉测量装置计算出的坐标，输出为该点校正后的坐标。基于神经网络的误差校正模型训练方法如下：

步骤(1)：训练数据获取

步骤(2)：训练校正模型网络

步骤(3)：将步骤(2)中获得的基于神经网络的误差校正模型写入数据处理系统的DSP程序存储器FLASH(6)，以方便测量装置在实际测量中能对计算结果进行校正。

第三步：获取双目图像。由于测量的是运动物体，因此左右摄像机需要同步工作，否则获得的数据将获得较大误差。如图4所示，展示了双目视觉同步采集系统。由FPGA同步控制电路(2)设置双目传感器(1)工作在被动模式，该模式下的图像传感器(1)的行同步和场同步由FPGA同步控制电路(2)发出；图像传感器(1)采集到的图像信号首先暂存在数据存储器SDRAM(3)，然后再通过数据输出接口(4)输入到图像数据处理系统。FPGA同步控制电路(2)负责控制协调图像传感器(1)、数据存储器SDRAM(3)和数据输出接口(4)。

第四步：图像预处理。如图5所示，展示了数据处理系统的处理过程。其中图像预处理过程将在FPGA(XC4VLX80)(8)中进行。具体流程是FPGA(XC4VLX80)(8)从输入数据端口(7)读入图像数据，然后存入数据存储器SDRAM(11)中，等待FPGA(XC4VLX80)(8)的处理；FPGA(XC4VLX80)(8)从数据存储器SDRAM(11)中读入数据，并进行预处理，并将处理结果再次存入数据存储器SDRAM(11)中，等待DSP芯片(TMS320C6416)(9)读入数据进一步处理。

第五步是数据处理，获得目标高度信息。如图4所示，该步骤的处理将在DSP芯片(TMS320C6416)(9)中进行。该步骤分为六个步骤：

首先从左摄像机图像中提取一个目标，然后在右摄像机图像中进行寻找，以减少特征匹配步骤中的数据量；

步骤(3)：利用配对点的图像坐标计算相应点的世界坐标；

步骤(6)：计算出目标的最高点A，其坐标为(x_ac,y_ac,z_ac)，再从目标中得到其最低点D，其坐标为(x_dc,y_dc,z_dc)，如图6所示，展示了高度测量原理，由于左右摄像机光轴水平，因此目标高度的计算可通过下式取得：

H＝y_ac-y_dc (3)

经过以上步骤，将获得目标点的深度信息。

需要指出的是，数据处理是采用的FPGA加DSP的结构，这样可以减轻DSP的运算负担，加快运算速度，提高实时性的要求。

第六步：将图像信息和高度信息通过无线数据传输单元传回远程监控终端，供监视人员参考。

近一步地，图7给出了双目视觉三维重建原理示意图，即DSP芯片进行三维重建的方法原理。以左摄像机坐标系为基准，右摄像机坐标系与左摄像机坐标系的转换矩阵为M＝[R T]，设现实中目标上一点P，设其在左摄像机坐标的坐标(x_c,y_c,z_c)，在右摄像机坐标系的坐标为(x_r,y_r,z_r)，在左右摄像机中所成的像点的图像坐标分别为P_l(x_pl,y_pl)和P_r(x_pr,y_pr)，基线距为B，摄像机焦距为f，则根据摄像机透视变换有：

当R为单位矩阵时，摄像机相面点之间的对应关系可表示为：

由上式可以计算出特征点P在左摄像机坐标系坐标：

由此获得各个主要特征点的坐标。

Claims

1.一种基于机器视觉的远距离运动物体高度测量的装置，其特征在于：包括双目视觉同步采集系统，图像数据处理系统，无线数据传输单元，远程监控终端，所述双目视觉同步采集系统，由长焦距镜头，双目图像传感器(1)，FPGA同步控制电路(2)，数据存储器SDRAM(3)，数据输出接口(4)，供电电路组成。

2.根据权利要求1所述的基于机器视觉的远距离运动物体高度测量的装置，其特征在于：所述双目视觉同步采集系统中的FPGA(2)分别和双目图像传感器(1)、数据存储器SDRAM(3)和数据输出接口(4)相连；双目图像传感器(1)再和数据存储器SDRAM(3)相连；数据存储器SDRAM(3)再和数据输出接口(4)相连；由FPGA产生场同步信号和行同步信号，保证两图像传感器(1)同步工作；同时由FPGA(2)产生对数据存储器SDRAM(3)和数据输出接口(4)的控制信号。

3.根据权利要求2所述的基于机器视觉的远距离运动物体高度测量的装置，其特征在于，由长焦距镜头和双目图像传感器(1)所组成的双目摄像系统，其左摄像机系统和右摄像机系统的基线距应保持一定距离，同时左右摄像系统的光轴保持水平。

4.根据权利要求1所述的基于机器视觉的远距离运动物体高度测量的装置，其特征在于，所述图像数据处理系统，由DSP芯片(9)，FPGA(8)，JTAG接口，复位控制电路，时钟电路，输入数据端口(7)，输出数据端口(10)，数据存储器SDRAM(11)，FPGA程序存储器EPRAM(5)，DSP程序存储器FLASH(6)和供电电路组成，FPGA(8)从输入数据端口(7)中读入图像数据，并存入数据存储器SDRAM(11)，同时由FPGA(8)读出数据存储器SDRAM(11)数据并对图像进行滤波等预处理，并将预处理结果存入数据存储器SDRAM(11)；FPGA(XC4VLX80)(8)负责对DSP芯片(9)发出控制命令，使DSP芯片(9)对数据存储器SDRAM(11)中的预处理结果进行特征匹配、三维重建等运算；输出数据端口(10)负责将DSP芯片(9)运算结果输出到无线数据传输单元。

5.一种如权利要求1所述的基于机器视觉的远距离运动物体高度测量的装置的测量方法，其特征在于包括以下步骤：

步骤一：将所述双目视觉同步采集系统，图像数据处理系统，无线数据传输单元，远程监控终端安装完毕，对该装置进行标定；

步骤二：训练基于神经网络的误差校正模型；

6.根据权利要求5所述的基于机器视觉的远距离运动物体高度测量的装置的测量方法，其特征在于，所述步骤一的标定过程步骤如下：

步骤一：对权利要求1所述装置进行标定，获得两摄像机的内参数和外参数；

步骤二：对双目测量装置进行水平标定，具体方法如下：

如果摄像机的光轴水平，则下式成立：

\frac{y_{m l} - y_{e l}}{y_{e l} - y_{n l}} = \frac{y_{m r} - y_{e r}}{y_{e r} - y_{n r}} = \frac{M E}{E N} .

7.根据权利要求5所述的基于机器视觉的远距离运动物体高度测量的装置的测量方法，其特征在于，所述步骤二的基于神经网络的误差校正模型训练方法如下：

步骤一：训练数据获取

步骤二：训练校正模型网络

对步骤一所获得图像数据进行计算，计算出不同位置下的标定杆的顶点和其他测量点的计算坐标，将计算坐标作为神经网络的输入，相应的标定杆顶点及其他相应测量点的真实坐标作为期望输出，然后训练网络，最终得到基于神经网络的误差校正模型；

步骤三：将步骤二中获得的基于神经网络的误差校正模型写入数据处理系统的DSP程序存储器FLASH。

8.根据权利要求5所述的基于机器视觉的远距离运动物体高度测量的装置的测量方法，其特征在于，所述步骤五的数据处理过程包括如下步骤：

步骤一：由图像数据处理系统的DSP芯片(9)对经过预处理步骤的图像进行目标识别：

步骤二：对目标所在区域进行子像素级的角点特征提取，并配对：

f (x, y) = {ce}^{- \frac{{(x - x_{0})}^{2} + {(y - y_{0})}^{2}}{2 δ^{2}}}

步骤三：利用配对点的图像坐标计算相应点的世界坐标；

步骤四：运用基于神经网络的误差校正模型对步骤(3)中计算出的坐标进行校正，获得精确的世界坐标系坐标，建立稀疏的深度图；

步骤五：利用内插方法对步骤(4)获得的稀疏的深度图进行插值，获得整个目标的深度图；

步骤六：计算出目标的最高点A，其坐标为(x_ac,y_ac,z_ac)，再从目标中得到其最低点D，其坐标为(x_dc,y_dc,z_dc)，则目标高度的计算通过下式取得：

H＝y_ac-y_dc

所求出的H即为所求。

9.根据权利要求5所述的基于机器视觉的远距离运动物体高度测量的装置的测量方法，其特征在于：所述的世界坐标系的计算方法如下：

以左摄像机坐标系为基准，右摄像机坐标系与左摄像机坐标系的转换矩阵为M＝[R T]，设现实中目标上一点P，设其在左摄像机坐标的坐标(x_c,y_c,z_c)，在右摄像机坐标系的坐标为(x_r,y_r,z_r)，在左右摄像机中所成的像点的图像坐标分别为P_l(x_pl,y_pl)和P_r(x_pr,y_pr)，基线距为B，摄像机焦距为f，则根据摄像机透视变换有：

s_{l} [\begin{matrix} x_{p l} \\ y_{p l} \\ 1 \end{matrix}] = [\begin{matrix} f_{l} & 0 & 0 \\ 0 & f_{l} & 0 \\ 0 & 0 & 1 \end{matrix}] \cdot [\begin{matrix} x_{c} \\ y_{c} \\ z_{c} \end{matrix}]

s_{r} [\begin{matrix} x_{p r} \\ y_{p r} \\ 1 \end{matrix}] = [\begin{matrix} f_{r} & 0 & 0 \\ 0 & f_{r} & 0 \\ 0 & 0 & 1 \end{matrix}] \cdot [\begin{matrix} x_{r} \\ y_{r} \\ z_{r} \end{matrix}]

[\begin{matrix} x_{r} \\ y_{r} \\ z_{r} \end{matrix}] = M [\begin{matrix} x_{c} \\ y_{c} \\ z_{c} \\ 1 \end{matrix}] = [\begin{matrix} R & T \end{matrix}] [\begin{matrix} x_{c} \\ y_{c} \\ z_{c} \\ 1 \end{matrix}] = [\begin{matrix} r_{1} & r_{2} & r_{3} & t_{x} \\ r_{4} & r_{5} & r_{6} & t_{y} \\ r_{7} & r_{8} & r_{9} & t_{z} \end{matrix}] [\begin{matrix} x_{c} \\ y_{c} \\ z_{c} \\ 1 \end{matrix}]

当R为单位矩阵时，摄像机相面点之间的对应关系可表示为：

ρ_{r} [\begin{matrix} x_{p r} \\ y_{p r} \\ 1 \end{matrix}] = [\begin{matrix} f_{r} r_{1} & f_{r} r_{2} & f_{r} r_{3} & f_{r} t_{x} \\ f_{r} r_{4} & f_{r} r_{5} & f_{r} r_{6} & f_{r} t_{y} \\ r_{7} & r_{8} & r_{9} & t_{z} \end{matrix}] \cdot [\begin{matrix} z \cdot x_{l} / f_{l} \\ z \cdot y_{l} / f_{l} \\ z \\ 1 \end{matrix}]

由上式可以计算出特征点P在左摄像机坐标系坐标：

\{\begin{matrix} z_{c} = \frac{f_{l} (f_{r} t_{x} - x_{p r} t_{z})}{x_{p r} (r_{7} x_{p l} + r_{8} y_{p l} + f_{l} r_{9}) - f_{r} (r_{1} x_{p l} + r_{2} y_{p l} + f_{r} r_{3})} \\ = \frac{f_{l} (f_{r} t_{y} - y_{p r} t_{z})}{y_{p r} (r_{7} x_{p l} + r_{8} y_{p l} + f_{l} r_{9}) - f_{r} (r_{4} x_{p l} + r_{5} y_{p l} + f_{l} r_{6})} \\ x_{c} = \frac{z_{c} x_{p l}}{f_{l}} \\ y_{c} = \frac{z_{c} y_{p l}}{f_{l}} \end{matrix}

由此获得各个主要特征点的坐标。