普通摄像头进行单目测距的尝试-六虎

我正在参与「启航计划」

导言

单目摄像头测距，这个课题既充满应战，又极具有意义，因为没有贵重的距离传感器而大幅度降低成本，潜力巨大。试想一下，普通的一个摄像头，只需进行简略的标定（只在画面上点几下），就能测算出画面中恣意两个点之间的距离，是不是很便利呢？

先修常识

现在研讨单目测距的范畴首要仍是会集在智能交通范畴，因为交通上有几个先天的优势，使得研讨愈加简略：

一是车道线是平行线，这一点很简略确保；

二是车辆识别十分老练，能够快速定位；

三是摄像头相对固定。

可是咱们摄像头捕获的画面是一个平面，与实在世界坐标系之间需要做一次平移和一次旋转变换，下面这张图比较直观的展示了这个变换。

这个变换过程的解说能够参阅这篇博文，讲的十分详细： Step1：模型 16个相机参数（内参、外参、畸变参数）_相机模型内涵参数_笔仍是要动的的博客-CSDN博客。其间，最重要的定论便是变换方程：

其间，K是相机内参矩阵，只跟相机本身出厂参数有关，后面是外参矩阵，由R（旋转矩阵）和T（平移矩阵）组合而来，Zc是份额系数

正文：

有了上面的定论，咱们的问题转化成了已知u,v如何求解出世界坐标系下的x,y,w，很明显，这其实是一个数学问题，需要线性代数来帮助。

先不要着急求解，咱们的首要任务应该是先弄清楚K、R、T的值。

K——内参矩阵

这儿咱们为了快速估算，作以下假定，假定路途监控摄像头的畸变忽略不计，这样咱们能够得到fx = fy，u0=1/2W，v0=1/2H，其间W是画面宽度，H是画面高度。这样的话，K矩阵只剩fx这一个未知数。

R——旋转矩阵

如果是用欧拉角来表示的话，还能够写成这样：

其间，是俯仰角，是偏航角，是翻滚角，公式推导请参阅：欧拉角（Eular Angle）详解 – Deep Studio (p-chao.com) 和依据相机旋转矩阵求解三个轴的旋转角/欧拉角/姿势角或旋转矩阵与欧拉角（Euler Angles）之间的相互转化，以及python和C++代码完成_相机外参数旋转矩阵求旋转视点_点亮～黑夜的博客-CSDN博客

单是看到这个矩阵，可能头有点大，可是考虑到实践状况，咱们能够大大简化，路途监控摄像头因为装置的联系，翻滚角基本近似为0，那么cos = 1，sin = 0。此刻矩阵R简化为

T——平移矩阵

平移矩阵指的是从世界坐标系原点到相机坐标系原点的变换矩阵。放到这个监控图中，便是从路面到相机所在位置，平移的距离便是相机的装置高度。

这儿装置高度咱们假定是已知的。下面咱们就来解这个方程。

如果直接去标定这几个角也是能够的，便是相对麻烦一点，有没有更简略的办法呢？答案是有的，便是使用图画消失点来核算。仍是看这张图，因为车道线平行的联系，咱们很简略就找到了其间一个消失点P：

详细的推导见：如何通过图画消失点核算相机的位姿？_Being_young的博客-CSDN博客，这儿给出定论：

这样的话，只需内参矩阵K和消失点P已知，就能解出俯仰角和偏航角，继而得到旋转矩阵R，结合已知的平移矩阵T，中间参数全部得到了，只差一个份额系数Zc，可是，内参矩阵这儿很难确认，因为没有标定的条件。再来看一下这个方程：

如果没有其它条件，仍然无法求解出Zc，在实践可能的状况中，咱们能够使用先验常识，比方某种车型的车辆长度或许标识牌等，确认画面中两点之间的距离。比方左边停在路旁边的小车，确认图画坐标P0到P1，对应的是实践的车长L，这样便得到7个方程8个未知数，仍然无法解。

这儿作者想到的一个办法是，使用核算机的功能优势，设置一个初始虚拟焦距F=0.01，再以步进方法递增到50，每次迭代一个值，就核算一遍K、R，求解出P0到P1的理论距离L1，使用车长L已知的先验条件，记载L1与L的差错，然后选取出最小差错所对应的虚拟焦距F0作为镜头的实践焦距，便得到了镜头所对应的近似内参矩阵K及旋转矩阵R。

依据上述思维，写了对应的python脚本，输入上图中参数，预设了摄像头高度是6米，路旁边黑车车长4.5m，得到成果如下图：

from   utils.k import Kmtrix
import numpy as np
import math
import matplotlib.pyplot as plt
# 获取f与距离联系,寻找最优解
def distance_list():
    h = 6000 #单位mm
    u0 = 365
    v0 = 188
    p0x = 497
    p0y = 31
    p1 = np.array([372,352,1])
    p2 = np.array([270,416,1])
    a_list = []
    b_list = []
    f_list = []
    dis_list = []
    for f in range(1,500):
        f= f / 10
        # 核算内参矩阵
        K = Kmtrix(f = f, u0= u0 , v0 = v0  )
        # 核算旋转矩阵
        a, b,Rmatrix = rotate_Matrix(p0x = p0x, p0y = p0y, K = K)
        # 核算外参矩阵
        RTmatrix = RT_Matrix(R = Rmatrix, h = h)
        #核算p1实在世界坐标
        rp1 = comp_real_position(Mrt = RTmatrix,K = K, h = h, pos = p1)
        #核算p2实在世界坐标
        rp2 = comp_real_position(Mrt = RTmatrix,K = K, h = h, pos = p2)
        # 核算两点距离
        AB = np.linalg.norm(rp2-rp1)/1000
        a_list.append(a)
        b_list.append(b)
        f_list.append(f)
        dis_list.append(AB)
    return a_list,b_list,f_list,dis_list
def rotate_Matrix(*, p0x, p0y ,K):
    K_1 = np.linalg.inv(K)
    # 消失点在图画中坐标
    P0 = np.array([p0x,p0y,1])
    r3 = np.dot(K_1, P0)/np.linalg.norm(np.dot(K_1, P0))
    # print(r3)
    a = math.asin(r3[1])
    # print(f'a='+str(a))
    b= -math.atan(r3[0]/r3[2])
    # print(f'b='+str(b))
    # 旋转矩阵
    R = np.array([
        [math.cos(b), math.sin(a)*math.sin(b), -math.cos(a)*math.sin(b)],
        [0,       math.cos(a),          math.sin(a)],
        [math.sin(b),  -math.cos(b)*math.sin(a),  math.cos(a)*math.cos(b)]
    ])
    return a,b,R
# 外参矩阵
def RT_Matrix(*, R, h):
    T = np.array([0,h,0])
    temp = np.concatenate((R , T.reshape(-1,1)),axis=1)
    Mrt = np.concatenate((temp,np.array([[0,0,0,1]])),axis=0)
    # print('Mrt')
    # print(Mrt)
    return Mrt
# 核算实在世界坐标
def comp_real_position(*,Mrt,K, h, pos):
    K_E =  np.concatenate((K , np.array([0,0,0]).reshape(-1,1)),axis=1)
    M2 = np.dot(K_E, Mrt)
    r1 = M2[:,0]
    r2 = M2[:,1]
    r3 = M2[:,2]
    r4 = M2[:,3]
    M3 = np.concatenate((r1.reshape(-1,1),pos.reshape(-1,1),r3.reshape(-1,1)),axis=1)
    r5= -1 * h * r2 - r4
    res = np.dot(np.linalg.inv(M3),r5)
    return res
# 依据两点坐标，核算两点距离
def comp_distance(*, p1, p2):
    p1 = comp_real_position(p1)
    p1 = np.delete(p1,1,axis=0)
    p2 = comp_real_position(p2)
    p2 = np.delete(p2,1,axis=0)
    # print('实在世界坐标：')
    print(p1,p2)
    AB = np.linalg.norm(p2-p1)
    # print('两点距离（米）：')
    print(AB)
real_dis = 4.5
condition1 = lambda value: abs(value - real_dis)
if __name__ == '__main__':
        a_list,b_list,f_list,dis_list = distance_list()
        index = 0
        x = 100
        for i in range(0,len(dis_list)-1):
            if(abs(dis_list[i] - real_dis) < x):
                x = abs(dis_list[i] - real_dis)
                index = i 
        print(dis_list[0])
        print('最优焦距',f_list[index])
        print('距离',dis_list[index])
        print('index',index)
        x = np.array(f_list)
        y1 = np.array(dis_list)
        fig=plt.figure(figsize=(5, 4), dpi=100)
        plt.title('焦距与距离联系对应_摄像头焦距最优解',fontname="SimHei")
        plt.xlabel('focus distance')
        plt.ylabel('Two Point distance')
        plt.plot(x,y1)
        plt.plot(f_list[index], dis_list[index], "kx") 
        plt.text(f_list[index]+10,dis_list[index],'最优焦距 :'+ str(f_list[index])+'mm', fontsize=15,fontname="SimHei")
        plt.show()

这样，得到此摄像头虚拟焦距为1.9mm，代入到方程，即可求解恣意坐标，写一个求解脚本，输入图画坐标，得到P3P4距离为55.18m。

这个成果有必定差错，经查询得知，规范高速的车行道分界面规范长度是6米，距离9米，所以图中如果是规范划线的话，P3P4距离大概在30米。这儿的差错作者认为首要来自于3个方面：

内参矩阵K忽略了畸变，以理想镜头替代；
摄像头装置高度为预估，实践能够简略测量一下；
参照物车辆的长度是以经验估量的，如果有愈加准确的参照物，成果也会愈加准确；

以上的剖析纯属一家之言，还有许多不完善的当地，请大佬不吝赐教。

参阅资料：

依据相机旋转矩阵求解三个轴的旋转角/欧拉角/姿势角或旋转矩阵与欧拉角（Euler Angles）之间的相互转化，以及python和C++代码完成_相机外参数旋转矩阵求旋转视点_点亮～黑夜的博客-CSDN博客

基于车道线消失点的车距测量办法_消失点测距公式_海清河宴的博客-CSDN博客

单目3D方针检测论文笔记] 3D Bounding Box Estimation – 知乎 (zhihu.com)

欧拉角与旋转矩阵之间的转化公式及原理_欧拉角转旋转矩阵_LoongTech的博客-CSDN博客

欧拉角（Eular Angle）详解 – Deep Studio (p-chao.com)

透视没有那么难，就三点：地平线.消失点.视平线 (360doc.com)

如何通过图画消失点核算相机的位姿？_Being_young的博客-CSDN博客

Step1：模型 16个相机参数（内参、外参、畸变参数）_相机模型内涵参数_笔仍是要动的的博客-CSDN博客

Opencv中solvePnP函数的小结 – nikoooo – 博客园 (cnblogs.com)

普通摄像头进行单目测距的尝试

相关文章

贝叶斯，朴素贝叶斯的基础原理及例子

== 和 equals 区别

Flutter是如何渲染Widget的

浏览器断点调试——从入门到精通

作者信息