python+OpenCV 实时测量相机位姿（相机外参）

白马非马

胶片相机爱好者

最近需要调节摄像头与地面的夹角，即需要 实时测量摄像头的位姿 ，上网找相关的资料但没有发现相关的代码，便集百家之长，自己整合出来了一个，特此感谢我所搜索到的对我有帮助的代码的博主.

文末贴有完整代码！！！

步骤分为：

相机标定(matlab或者python)得到相机内参。
给棋盘格的角点赋予三维坐标。
利用findChessboardCorners与cornerSubPix获取图像上的角点坐标。
利用solvePnP解算相机外参（旋转矩阵与平移矩阵）。
结合摄像头实时测量位姿。

相机标定

分享一个在线生成标定图案并能下载包含标定图案pdf文件的网址。

网址： Camera Calibration Pattern Generator

要注意的是matlab与opencv中的内参矩阵元素顺序不一致。

畸变参数也不同，opencv中：k1,k2,p1,p2,k3

本文相机标定的图片集

链接： https:// pan.baidu.com/s/1I9U_GT H75GQVE0Wsudx14A?pwd=vdkk

提取码：vdkk

# 内参数矩阵
Camera_intrinsic_8mm = {
    "mtx": np.array([[1.33988831e+03, 0.00000000e+00, 6.51139957e+02],
                     [0.00000000e+00, 1.33659412e+03, 4.92876896e+02],
                     [0.00000000e+00, 0.00000000e+00, 1.00000000e+00]],
                    dtype=np.double),
    "dist": np.array([[-1.20912144e-01, 5.88024656e-02, -2.28958046e-04, -5.41222488e-04,
                       4.97592916e-01]], dtype=np.double),
}

获取棋盘格世界三维坐标

# w h分别是棋盘格模板长边和短边规格（角点个数）
w = 10
h = 7
# 世界坐标系中的棋盘格点,例如(0,0,0), (1,0,0), (2,0,0) ....,(8,5,0)，去掉Z坐标，记为二维矩阵，认为在棋盘格这个平面上Z=0
objp = np.zeros((w * h, 3), np.float32)  # 构造0矩阵，88行3列，用于存放角点的世界坐标
objp[:, :2] = np.mgrid[0:w, 0:h].T.reshape(-1, 2)  # 三维网格坐标划分
worldpoint = objp * 20  # 棋盘格的宽度为15mm (array(70,3))

获取图像上的角点坐标

类似于用opencv进行相机标定，检测棋盘格角点。

在这里贴一下python相机标定与畸变矫正的代码。

import cv2
import numpy as np
import glob
# 读取图像，解决imread不能读取中文路径的问题
def cv_imread(filePath):
    cv_img = cv2.imdecode(np.fromfile(filePath, dtype=np.uint8), -1)
    # imdecode读取的是rgb，如果后续需要opencv处理的话，需要转换成bgr，转换后图片颜色会变化
    # cv_img = cv2.cvtColor(cv_img, cv2.COLOR_RGB2BGR)
    return cv_img
def calibration():
    # 找棋盘格角点
    criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
    # print(cv2.TERM_CRITERIA_EPS,'',cv2.TERM_CRITERIA_MAX_ITER)
    # w h分别是棋盘格模板长边和短边规格（角点个数）
    w = 10
    h = 7
    # 世界坐标系中的棋盘格点,例如(0,0,0), (1,0,0), (2,0,0) ....,(8,5,0)，去掉Z坐标，记为二维矩阵，认为在棋盘格这个平面上Z=0
    objp = np.zeros((w * h, 3), np.float32)  # 构造0矩阵，88行3列，用于存放角点的世界坐标
    objp[:, :2] = np.mgrid[0:w, 0:h].T.reshape(-1, 2)  # 三维网格坐标划分
    # 储存棋盘格角点的世界坐标和图像坐标对
    objpoints = []  # 在世界坐标系中的三维点
    imgpoints = []  # 在图像平面的二维点
    images = glob.glob('D:/OneDrive/OneDrive - mail.scut.edu.cn/volume/calibration/8mm/*.jpg')
    for fname in images:
        img = cv_imread(fname)
        gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
        # 粗略找到棋盘格角点 这里找到的是这张图片中角点的亚像素点位置，共11×8 = 88个点，gray必须是8位灰度或者彩色图，（w,h）为角点规模
        ret, corners = cv2.findChessboardCorners(gray, (w, h))
        # 如果找到足够点对，将其存储起来
        if ret == True:
            # 精确找到角点坐标
            corners = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
            # 将正确的objp点放入objpoints中
            objpoints.append(objp)
            imgpoints.append(corners)
            # 将角点在图像上显示
            cv2.drawChessboardCorners(img, (w, h), corners, ret)
            cv2.imshow('findCorners', img)
            cv2.waitKey()
    cv2.destroyAllWindows()
    ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)
    # 去畸变
    img2 = cv2.imread('D:/OneDrive/OneDrive - mail.scut.edu.cn/volume/calibration/5mm/2023-03-13_09_17_14_069.jpg')
    h, w = img2.shape[:2]
    newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (w, h), 0, (w, h))  # 自由比例参数
    dst = cv2.undistort(img2, mtx, dist, None, newcameramtx)
    # 根据前面ROI区域裁剪图片
    # x,y,w,h = roi
    # dst = dst[y:y+h, x:x+w]
    # dst = cv2.resize(dst,(400,400))
    cv2.imshow('fin', dst)
    # end = time.time() - s
    cv2.waitKey()
    cv2.destroyAllWindows()
    # 反投影误差
    total_error = 0
    for i in range(len(objpoints)):
        imgpoints2, _ = cv2.projectPoints(objpoints[i], rvecs[i], tvecs[i], mtx, dist)
        error = cv2.norm(imgpoints[i], imgpoints2, cv2.NORM_L2) / len(imgpoints2)
        total_error += error
    print("total error: ", total_error / len(objpoints))
    # 打印我们要求的两个矩阵参数
    print("newcameramtx内参:\n", newcameramtx)
    print("dist畸变值:\n", dist)
    print("newcameramtx旋转（向量）外参:\n", rvecs)
    print("dist平移（向量）外参:\n", tvecs)
    print("roi:", roi)
def Correct(img):
    mtx = np.array([[1.34121216e+03, 0.00000000e+00, 6.49632172e+02],
                    [0.00000000e+00, 1.33802307e+03, 4.93509463e+02],
                    [0.00000000e+00, 0.00000000e+00, 1.00000000e+00]])  # 内参
    dist = np.array([-1.16174749e-01, 1.18791150e-02, -1.99795339e-04, -7.44955520e-04,
                     6.34923566e-01])  # 畸变参数--顺序很重要！！！：k1,k2,p1,p2,k3
    img_distort = cv2.undistort(img, np.array(mtx), np.array(dist))
    return img_distort
# 进行相机标定
calibration()

而在测位姿时，只需要检测角点即可。

gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
# 粗略找到棋盘格角点 这里找到的是这张图片中角点的亚像素点位置，共11×8 = 88个点，gray必须是8位灰度或者彩色图，（w,h）为角点规模
ret, corners = cv2.findChessboardCorners(gray, (w, h))
# 如果找到足够点对，将其存储起来
if ret:
   # 精确找到角点坐标
   corners = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
   imagepoint = np.squeeze(corners)  # 将corners降为二维

solvePnP解算相机位姿（旋转矩阵与平移矩阵）

看其他求解位姿文章中，都是用四个角点来解算，但是opencv中的solvepnp支持4个以上的角点检测，就可以利用相机标定的角点检测函数，直接解算，比较方便。

(success, rvec, tvec) = cv2.solvePnP(worldpoint, imagepoint, Camera_intrinsic_5mm["mtx"], Camera_intrinsic_5mm["dist"])
distance = math.sqrt(tvec[0] ** 2 + tvec[1] ** 2 + tvec[2] ** 2) / 10  # 测算距离
rvec_matrix = cv2.Rodrigues(rvec)[0]
proj_matrix = np.hstack((rvec_matrix, rvec))
eulerAngles = -cv2.decomposeProjectionMatrix(proj_matrix)[6]  # 欧拉角
pitch, yaw, roll = eulerAngles[0], eulerAngles[1], eulerAngles[2]

摄像头实时测量位姿

在这里贴上实时测量位姿的完整代码，由于用的工业相机不同（本文用的大恒），调用相机的函数也不一样，但本质就是输入一张图片而已，只需要更改摄像头的调用函数就可以直接使用。

import gxipy as gx
import cv2
import numpy as np
import math
# 内参数矩阵
Camera_intrinsic_8mm = {
    "mtx": np.array([[1.33988831e+03, 0.00000000e+00, 6.51139957e+02],
                     [0.00000000e+00, 1.33659412e+03, 4.92876896e+02],
                     [0.00000000e+00, 0.00000000e+00, 1.00000000e+00]],
                    dtype=np.double),
    "dist": np.array([[-1.20912144e-01, 5.88024656e-02, -2.28958046e-04, -5.41222488e-04,
                       4.97592916e-01]], dtype=np.double),
Width_set = 1280  # 设置分辨率宽
Height_set = 960  # 设置分辨率高
framerate_set = 50  # 设置帧率
num = 100000  # 采集帧率次数
id = 0
device_manager = gx.DeviceManager()  # 创建设备对象
dev_num, dev_info_list = device_manager.update_device_list()  # 枚举设备，即枚举所有可用的设备
if dev_num == 0:
    print("Number of enumerated devices is 0")
else:
    print("")
    print("**********************************************************")
    print("创建设备成功，设备号为:%d" % dev_num)
cam = device_manager.open_device_by_sn(dev_info_list[id].get("sn"))
camera_id = dev_info_list[id].get("sn")
print("")
print("**********************************************************")
print("打开彩色摄像机成功，SN号为：%s" % camera_id)
cam.Width.set(Width_set)
cam.Height.set(Height_set)
# 设置连续采集
cam.TriggerMode.set(gx.GxSwitchEntry.OFF)  # 设置触发模式
# cam.AcquisitionFrameRateMode.set(gx.GxSwitchEntry.ON)
# 设置帧率
cam.AcquisitionFrameRate.set(framerate_set)
print("")
print("**********************************************************")
print("用户设置的帧率为:%d fps" % framerate_set)
framerate_get = cam.CurrentAcquisitionFrameRate.get()  # 获取当前采集的帧率
print("当前采集的帧率为:%d fps" % framerate_get)
cam.stream_on()
#这里是一下估测的核心代码
while True:
    # 采集图像
    raw_image = cam.data_stream[0].get_image()  # 打开第0通道数据流
    if raw_image is None:
        print("获取彩色原始图像失败.")
    img = raw_image.convert("RGB").get_numpy_array()  # 从彩色原始图像获取RGB图像
    # 找棋盘格角点
    criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
    # print(cv2.TERM_CRITERIA_EPS,'',cv2.TERM_CRITERIA_MAX_ITER)
    # w h分别是棋盘格模板长边和短边规格（角点个数）
    w = 10
    h = 7
    # 世界坐标系中的棋盘格点,例如(0,0,0), (1,0,0), (2,0,0) ....,(8,5,0)，去掉Z坐标，记为二维矩阵，认为在棋盘格这个平面上Z=0
    objp = np.zeros((w * h, 3), np.float32)  # 构造0矩阵，88行3列，用于存放角点的世界坐标
    objp[:, :2] = np.mgrid[0:w, 0:h].T.reshape(-1, 2)  # 三维网格坐标划分
    # 储存棋盘格角点的世界坐标和图像坐标对
    objpoints = []  # 在世界坐标系中的三维点
    imgpoints = []  # 在图像平面的二维点
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    # 粗略找到棋盘格角点 这里找到的是这张图片中角点的亚像素点位置，共11×8 = 88个点，gray必须是8位灰度或者彩色图，（w,h）为角点规模
    ret, corners = cv2.findChessboardCorners(gray, (w, h))
    # 如果找到足够点对，将其存储起来
    if ret:
        # 精确找到角点坐标
        corners = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
        worldpoint = objp * 20  # 棋盘格的宽度为20mm (array(70,3))
        imagepoint = np.squeeze(corners)  # 将corners降为二维
        (success, rvec, tvec) = cv2.solvePnP(worldpoint, imagepoint, Camera_intrinsic_8mm["mtx"], Camera_intrinsic_8mm["dist"])
        distance = math.sqrt(tvec[0] ** 2 + tvec[1] ** 2 + tvec[2] ** 2) / 10  # 测算距离
        rvec_matrix = cv2.Rodrigues(rvec)[0]
        proj_matrix = np.hstack((rvec_matrix, rvec))
        eulerAngles = -cv2.decomposeProjectionMatrix(proj_matrix)[6]  # 欧拉角
        pitch, yaw, roll = eulerAngles[0], eulerAngles[1], eulerAngles[2]
        # 显示参数文字
        cv2.putText(img, "%.2fcm,%.2f,%.2f,%.2f" % (distance, yaw, -pitch, roll),
                    (100, 50), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 255, 0), 3)