1. 背景知识

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

原则上单目相机是不可以的，因为只记录了二维信息，真实世界是三维的，双目相机可以通过视差，或者单目+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 += 1if 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(公式中的)和平移向量tvec(公式中的)，我们通过参考资料2的方式反透视变化，为相机的投影模型，简化为内参矩阵。

的标定我们在图中选取特定的点，进行运算。

代码中为#####部分，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键退出breakelse:  # 画面中没有棋盘格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()

这里是结果，可以看到还是有一定误差，系数是根据4号点算的。

3、yolo中实现

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

                    if save_img or save_crop or view_img:  # Add bbox to imagec = int(cls)  # integer classlabel = 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 = xyxyxx = (x1 + x2) / 2yy = max(y1, y2);uv = np.array([[xx.cpu()],[yy.cpu()],[1]])xy1 =rt_i.dot(pi_i.dot(uv))xy1[0]=xy1[0]*0.43xy1[1]=xy1[1]*0.43xxx="%.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、总结

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

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

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

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