普通摄像头进行单目测距的尝试-五八三

我正在参加「掘金·启航计划」

引言

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

先修知识

目前研究单目测距的领域主要还是集中在智能交通领域，由于交通上有几个先天的优势，使得研究更加容易：

一是车道线是平行线，这一点很容易保证；

二是车辆识别非常成熟，可以快速定位；

三是摄像头相对固定。

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

这个变换过程的讲解可以参考这篇博文，讲的非常详细： 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()
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()
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()