(九)相机内参、外参、反透视变换python opencv

  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打印一张棋盘格

(九)相机内参、外参、反透视变换python opencv_第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()
(九)相机内参、外参、反透视变换python opencv_第2张图片

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)

(九)相机内参、外参、反透视变换python opencv_第3张图片

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

(九)相机内参、外参、反透视变换python opencv_第4张图片

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

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

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

(九)相机内参、外参、反透视变换python opencv_第5张图片
(九)相机内参、外参、反透视变换python opencv_第6张图片

以该图片举例:

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

绿色是七个点在真实世界中的位置,坐标原点在右下角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()
(九)相机内参、外参、反透视变换python opencv_第7张图片

这里是结果,可以看到还是有一定误差,系数是根据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))
(九)相机内参、外参、反透视变换python opencv_第8张图片

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

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)
(九)相机内参、外参、反透视变换python opencv_第9张图片

4、总结

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

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

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

4、留坑:后续可能和(八)做联动

你可能感兴趣的:(jetson,agx,Xavier,自动驾驶,opencv,计算机视觉)