背景知识

任务需求：将相机上的一个点投影到真实世界平面上去。

原则上单目相机是不可以的，因为只记录了二维信息，真实世界是三维的，双目相机可以通过视差，或者单目+IMU组合，但是由于特征点在地面上的先验知识，因此可以进行反透视变换。方法有很多种那个，这里采用计算相机的内参和外参的方法。基础知识理论在视觉slam14讲中有详细说明，但是其代码是c++这里采用python opencv实现。

相机内参外参标定：https://blog.csdn.net/qq_29931565/article/details/119395353

逆投影，roadmap:https://blog.csdn.net/qq_53086461/article/details/128028199

需要安装提前安装好numpy、glob、cv2

pip install numpy
pip install opencv-python
pip install glob

2、内参标定

2.1打印一张棋盘格

2.2拍摄50张照片

import cv2
camera = cv2.VideoCapture(0)
i = 1
while i < 50:
    _, frame = camera.read()
    cv2.imwrite("E:/images/"+str(i)+'.png', frame, [int(cv2.IMWRITE_PNG_COMPRESSION), 0]) 
    cv2.imshow('frame', frame)
    i += 1
    if cv2.waitKey(200) & 0xFF == 27: # 按ESC键退出
        break
cv2.destroyAllWindows()

2.3 计算内参、畸变

需要更改的地方

第4行：调整为实际的棋盘格数量，格子数-1（格子内部交点数量）

第5行：调整为实际的棋盘格数量

第6行：调整为实际的棋盘格大小

第9行：上一步的存储位置

import cv2
import numpy as np
import glob
objp = np.zeros((6 * 9, 3), np.float32)
objp[:, :2] = np.mgrid[0:9, 0:6].T.reshape(-1, 2)  # 将世界坐标系建在标定板上，所有点的Z坐标全部为0，所以只需要赋值x和y
objp = 2.6 * objp  # 打印棋盘格一格的边长为2.6cm
obj_points = []  # 存储3D点
img_points = []  # 存储2D点
images = glob.glob("images/*.png")  # 黑白棋盘的图片路径

for fname in images:
    img = cv2.imread(fname)
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    size = gray.shape[::-1]
    ret, corners = cv2.findChessboardCorners(gray, (9, 6), None)
    if ret:
        obj_points.append(objp)
        corners2 = cv2.cornerSubPix(gray, corners, (5, 5), (-1, -1),
                                    (cv2.TERM_CRITERIA_MAX_ITER | cv2.TERM_CRITERIA_EPS, 30, 0.001))
        if [corners2]:
            img_points.append(corners2)
        else:
            img_points.append(corners)
        cv2.drawChessboardCorners(img, (9, 6), corners, ret)  # 记住，OpenCV的绘制函数一般无返回值
        cv2.waitKey(1)
_, mtx, dist, _, _ = cv2.calibrateCamera(obj_points, img_points, size, None, None)

# 内参数矩阵
Camera_intrinsic = {"mtx": mtx, "dist": dist, }
print("内参")
print(mtx)
print("畸变")
print(dist)

2.4 计算外参

固定相机！！！（之后就不要动了）

因为笔记本自带了一个相机，所以这里是用第二个，根据自己情况修改数字

camera = cv2.VideoCapture(1)

查看参考资料1：原作者代码显示俯仰角距离等信息，打开line 65、66 ，注释掉 line 67、68即可，效果如下。

外参包含两部分，旋转矩阵rvec_matrix(公式中的 $\text{[math]}$ )和平移向量tvec(公式中的 $\text{[math]}$ )，我们通过参考资料2的方式反透视变化， $\text{[math]}$ 为相机的投影模型，简化为内参矩阵。

$\text{[math]}$ 的标定我们在图中选取特定的点，进行运算。

代码中为#####部分，uv是相机图像中的坐标值的点，xy1是结算出来对应真实世界的点。

以该图片举例：

红色是七个点在图像中的位置，坐标原点在左上角

绿色是七个点在真实世界中的位置，坐标原点在右下角0号点位置

输入：点在图片中的位置，输出点在真实坐标系的位置

import cv2
import numpy as np
import glob
import math
objp = np.zeros((6 * 9, 3), np.float32)
objp[:, :2] = np.mgrid[0:9, 0:6].T.reshape(-1, 2)  # 将世界坐标系建在标定板上，所有点的Z坐标全部为0，所以只需要赋值x和y
objp = 2.5 * objp  # 打印棋盘格一格的边长为2.6cm
obj_points = []  # 存储3D点
img_points = []  # 存储2D点
images = glob.glob("images/*.png")  # 黑白棋盘的图片路径


mtx=[[444.26980017,0.0,330.00914635], [  0.0,442.49395109,239.56771487], [  0.0,0.0,1.0]]
mtx=np.array(mtx)

dist = [[ 1.47649702e-01,-2.72487592e-01 ,7.01492649e-05 ,2.74167922e-03,   1.84670647e-01]]
dist=np.array(dist)
# 内参数矩阵
Camera_intrinsic = {"mtx": mtx, "dist": dist, }
print("内参")
print(mtx)
print("畸变")
print(dist)



obj_points = objp  # 存储3D点
img_points = []  # 存储2D点

# 从摄像头获取视频图像
camera = cv2.VideoCapture(1)

while True:
    _, frame = camera.read()
    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
    size = gray.shape[::-1]
    ret, corners = cv2.findChessboardCorners(gray, (9, 6), None)
    if ret:  # 画面中有棋盘格
        img_points = np.array(corners)
        cv2.drawChessboardCorners(frame, (9, 6), corners, ret)
        # rvec: 旋转向量 tvec: 平移向量
        _, rvec, tvec = cv2.solvePnP(obj_points, img_points, Camera_intrinsic["mtx"], Camera_intrinsic["dist"])  # 解算位姿
        distance = math.sqrt(tvec[0] ** 2 + tvec[1] ** 2 + tvec[2] ** 2)  # 计算距离
        rvec_matrix = cv2.Rodrigues(rvec)[0]  # 旋转向量->旋转矩阵
        print("旋转矩阵")
        print(rvec_matrix)
        print("tvec")
        print(tvec)
        #####################
        rt =np.array([[rvec_matrix[0][0],rvec_matrix[0][1],tvec[0]],
                    [rvec_matrix[1][0],rvec_matrix[1][1],tvec[1]],
                    [rvec_matrix[2][0],rvec_matrix[2][1],tvec[2]]], dtype=np.float)
        rt_i=np.linalg.inv(rt)
        pi_i = np.linalg.inv(Camera_intrinsic["mtx"])
        uv = np.array([[434,396,362,331,338,376,480],
                       [349,331,315,300,347,368,371],
                       [1,1,1,1,1,1,1]])
        xy1 = 0.340909*rt_i.dot(pi_i.dot(uv))
        print("xy1")
        print(xy1)
        #####################
        proj_matrix = np.hstack((rvec_matrix, tvec))  # hstack: 水平合并
        eulerAngles = cv2.decomposeProjectionMatrix(proj_matrix)[6]  # 欧拉角
        pitch, yaw, roll = eulerAngles[0], eulerAngles[1], eulerAngles[2]
        #cv2.putText(frame, "dist: %.2fcm, yaw: %.2f, pitch: %.2f, roll: %.2f" % (distance, yaw, pitch, roll),
                    #(10, frame.shape[0] - 20), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
        cv2.putText(frame, "dist: %.2fcm,x : %.2f,y : %.2f, z: %.2f" % (distance, tvec[0], tvec[1], tvec[2]),
        (10, frame.shape[0] - 20), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)

        cv2.imshow('frame', frame)
        if cv2.waitKey(1) & 0xFF == 27:  # 按ESC键退出
            break
    else:  # 画面中没有棋盘格
        cv2.putText(frame, "Unable to Detect Chessboard", (20, frame.shape[0] - 20), cv2.FONT_HERSHEY_SIMPLEX, 1.3,
                    (0, 0, 255), 3)
        cv2.imshow('frame', frame)
        if cv2.waitKey(1) & 0xFF == 27:  # 按ESC键退出
            break
cv2.destroyAllWindows()

这里是结果，可以看到还是有一定误差，系数 $\text{[math]}$ 是根据4号点算的。

3、yolo中实现

这里我们更改了yolov5 detect.py中的Add bbox to image部分，让结果加在识别框上。思路是提取下边缘中点进行反透视投影。

                    if save_img or save_crop or view_img:  # Add bbox to image
                        c = int(cls)  # integer class
                        label = None if hide_labels else (names[c] if hide_conf else f' {names[c]}{conf:.2f}')
                        #########

                        mtx = [[444.26980017, 0.0, 330.00914635], [0.0, 442.49395109, 239.56771487], [0.0, 0.0, 1.0]]
                        mtx = np.array(mtx)
                        dist = [[1.47649702e-01, -2.72487592e-01, 7.01492649e-05, 2.74167922e-03, 1.84670647e-01]]
                        dist = np.array(dist)
                        Camera_intrinsic = {"mtx": mtx, "dist": dist, }
                        rvec_matrix= np.array([[-0.99611232,-0.05914762,-0.06528244],
                                                 [-0.04042639,-0.35150008,0.9353146 ],
                                                 [-0.07826841,0.93431753,0.34774244]])
                        tvec=np.array([[8.0553314],
                                         [8.50746768],
                                         [34.52738218]])

                        rt = np.array([[rvec_matrix[0][0], rvec_matrix[0][1], tvec[0]],
                                       [rvec_matrix[1][0], rvec_matrix[1][1], tvec[1]],
                                       [rvec_matrix[2][0], rvec_matrix[2][1], tvec[2]]], dtype=np.float)
                        rt_i = np.linalg.inv(rt)
                        pi_i = np.linalg.inv(Camera_intrinsic["mtx"])
                        x1, y1, x2, y2 = xyxy
                        xx = (x1 + x2) / 2
                        yy = max(y1, y2);
                        uv = np.array([[xx.cpu()],
                                       [yy.cpu()],
                                       [1]])
                        xy1 =rt_i.dot(pi_i.dot(uv))
                        xy1[0]=xy1[0]*0.43
                        xy1[1]=xy1[1]*0.43
                        xxx="%.3f" % xy1[0]
                        yyy="%.2f" % xy1[1]
                        label =xxx+" "+yyy
                        #########
                        annotator.box_label(xyxy, label, color=colors(c, True))

可以看到，误差似乎比单点更大了一点，怀疑是投影模型没有考虑畸变，更改如下：

before=np.array([[[xx.cpu(),yy.cpu()]]])
                        end = cv.undistortPoints(before,mtx, dist, P=mtx)
                        xx = end[0][0][0]
                        yy = end[0][0][1]
                        uv = np.array([[xx],
                                       [yy],
                                       [1]])

但是，后来发现差不多，测试了下去畸变函数，可以发现并没有什么区别。

import cv2 as cv
import numpy as np
mtx = [[444.26980017, 0.0, 330.00914635], [0.0, 442.49395109, 239.56771487], [0.0, 0.0, 1.0]]
mtx = np.array(mtx)
dist = [[1.47649702e-01, -2.72487592e-01, 7.01492649e-05, 2.74167922e-03, 1.84670647e-01]]
dist = np.array(dist)
img = cv.imread('imagess/1.png')
img_undistored = cv.undistort(img, mtx, dist)
cv.imwrite('imagess/11.png', img_undistored)

4、总结

原理上可行，根据棋盘格算距离也比较准确。但最终精度不高，可能是yolo本身下边缘也不准。

这种方法本来精度就有限，尤其上车后，还存在俯仰角度变化等，所以后续还要优化。

计算内参时候发现，选优不同数量的图片和角度时，结果有时候差距也比较大，可能是影响结果来源之一。另外一个打印的纸张后来发现不是严格的2.5cm，实际要大一点点，因此量格子的时候十个一量算平均数可能结果更加精确。

4、留坑：后续可能和（八）做联动

（九）相机内参、外参、反透视变换python opencv

背景知识