背景知识
任务需求：将相机上的一个点投影到真实世界平面上去。
原则上单目相机是不可以的，因为只记录了二维信息，真实世界是三维的，双目相机可以通过视差，或者单目+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 glob2、内参标定
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(公式中的
)和平移向量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键退出
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()
这里是结果，可以看到还是有一定误差，系数是根据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、留坑：后续可能和（八）做联动
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
