python+opencv+棋盘格实现相机标定及相对位姿估计
引言
相机标定的主要目的是为了求相机的内参及畸变系数等参数，通过相机的内参等可以将像素坐标转换为相机坐标，相机坐标又可通过变换得到世界坐标。
下面为了验证标定算法的准确性，采用了同一组棋盘格数据，通过与另外两个测试软件标定结果进行对比，其标定结果基本一致。
1，使用相机采集含棋盘格图像14张
2，进行相机标定
（1）测试软件1标定结果（内参及畸变系数）
https://blog.csdn.net/qq_42951560/article/details/126248810
【原创工具 | OpenCV-CamCalib】一个基于 OpenCV 的自动化相机数据采集和标定程序
R:
[0.0013, -0.0684, 1.5544]
[0.0313, -0.0572, 0.6501]
[0.0561, -0.0275, -0.6579]
[-0.0813, -0.1536, -1.4020]
[0.2893, -0.1042, -2.0699]
[-0.1846, -0.0407, -2.1670]
[0.2581, -0.0658, 3.0851]
[0.4249, 0.0644, -3.0719]
[-0.3689, 0.0267, 2.5249]
[0.1895, 0.0745, 1.8568]
[0.1489, -0.2197, 1.6593]
[0.1603, -0.1441, 1.0397]
[-0.2111, -0.0610, -0.0107]
[0.2734, -0.0441, 0.0333]
T:
[57.2825, -164.7185, 657.7062]
[-39.4535, -154.2506, 653.6776]
[-125.4226, -36.9211, 655.3406]
[-114.6057, 114.1176, 616.0881]
[-71.4379, 43.0685, 624.4835]
[56.3861, 127.9344, 618.4703]
[162.7121, 26.3284, 619.2786]
[25.0943, 24.0527, 654.0335]
[61.2546, 46.1862, 655.6716]
[165.0184, -153.1011, 611.0288]
[64.0926, -122.1281, 613.9142]
[7.0449, -136.1629, 608.7691]
[-115.8937, 13.3168, 642.5956]
[-97.2399, -115.5981, 610.6633]
（2）测试软件2标定结果（内参及畸变系数）
https://blog.csdn.net/Big_Huang/article/details/106166254
基于opencv 和 pyqt5 的相机标定助手的设计
相机内部矩阵:
[[1.09803736e+03 0.00000000e+00 6.63737765e+02]
[0.00000000e+00 1.09791044e+03 4.38657425e+02]
[0.00000000e+00 0.00000000e+00 1.00000000e+00]]
畸变参数:
[[ 0.00218144 -0.00925042 0.00024789 -0.0002825 0.20841267]]
旋转矩阵:
[array([[ 1.33903055e-03],
[-6.83598890e-02],
[ 1.55438056e+00]]), array([[ 0.03125883],
[-0.05715461],
[ 0.65011621]]), array([[ 0.05605446],
[-0.02753977],
[-0.65790633]]), array([[-0.08126108],
[-0.15363931],
[-1.40200001]]), array([[ 0.28931372],
[-0.10420944],
[-2.06993946]]), array([[-0.18457149],
[-0.0407243 ],
[-2.16699213]]), array([[ 0.25805958],
[-0.06580844],
[ 3.08507844]]), array([[ 0.42490026],
[ 0.06435579],
[-3.07186989]]), array([[-0.36890203],
[ 0.02674808],
[ 2.52486102]]), array([[0.18946187],
[0.07454542],
[1.85677179]]), array([[ 0.14889827],
[-0.21970662],
[ 1.65930809]]), array([[ 0.16029379],
[-0.14407197],
[ 1.03966592]]), array([[-0.21111973],
[-0.06096786],
[-0.01069444]]), array([[ 0.27336398],
[-0.04413062],
[ 0.03325271]])]
平移矩阵:
[array([[ 57.28312236],
[-164.71792893],
[ 657.70732026]]), array([[ -39.45288044],
[-154.25005049],
[ 653.67874071]]), array([[-125.42200989],
[ -36.92050016],
[ 655.34164677]]), array([[-114.60511114],
[ 114.11816637],
[ 616.08898813]]), array([[-71.43727126],
[ 43.06902124],
[624.48437836]]), array([[ 56.38668594],
[127.93492448],
[618.4712321 ]]), array([[162.7126308 ],
[ 26.32896621],
[619.27933957]]), array([[ 25.09489496],
[ 24.05330674],
[654.03439771]]), array([[ 61.25519273],
[ 46.18683531],
[655.67243968]]), array([[ 165.01894101],
[-153.10050991],
[ 611.02969995]]), array([[ 64.09318336],
[-122.12753319],
[ 613.9153159 ]]), array([[ 7.04548205],
[-136.16238216],
[ 608.7700887 ]]), array([[-115.89311233],
[ 13.31740073],
[ 642.59649204]]), array([[ -97.23935687],
[-115.59753028],
[ 610.66435603]])]
（3）Python算法标定及原点位姿获取
参考1：
https://blog.csdn.net/qq_29931565/article/details/119395353
【OpenCV】OpenCV-Python实现相机标定+利用棋盘格相对位姿估计
参考2:
（3-1）python代码如下（此代码经修改验证，以下方为准）
import numpy as np
import glob
import cv2
import math
#当前验证此算法的标定结果与其他标定基本一致
#1，相机标定获取内参及畸变系数
#角点个数
w = 11
h = 8
b_w = 20 #棋盘格边长20mm
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
objp = np.zeros((w * h, 3), np.float32)
objp[:, :2] = np.mgrid[0:w, 0:h].T.reshape(-1, 2) # 将世界坐标系建在标定板上，所有点的Z坐标全部为0，所以只需要赋值x和y
objp = b_w * objp # 打印棋盘格一格的边长为2.6cm
#print(objp)
obj_points = [] # 存储3D点
img_points = [] # 存储2D点
#images = glob.glob("E:/image/*.png") # 黑白棋盘的图片路径
def get_image_paths(folder_path):
# 使用通配符筛选出所有jpg/png图片
return glob.glob(f"{folder_path}/**/*.jpg", recursive=True)
# 如果需要包括其他格式的图片，可以在这里添加，例如：png
# return glob.glob(f"{folder_path}/**/*.jpg", recursive=True) + \
# glob.glob(f"{folder_path}/**/*.png", recursive=True)
# 使用示例
#folder_path = "G:/3dversion/weiziguji/8mm/" # 替换为你的文件夹路径
folder_path = "C:\\Users\\zhaocai\\Pictures\\test"
images = get_image_paths(folder_path)
size = None
for fname in images:
img = cv2.imread(fname)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
size = gray.shape[::-1]
ret, corners = cv2.findChessboardCorners(gray, (w, h), None)
if ret:
obj_points.append(objp) #世界坐标系中的三维点始终不变
#此处的winsize（会影响到畸变系数）
corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-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, (w, h), corners, ret) # 记住，OpenCV的绘制函数一般无返回值
#cv2.imshow("demo",img)
#cv2.waitKey(0)
# print(obj_points)
# print(img_points)
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(obj_points, img_points, size, None, None)
result = "摄像机矩阵:\n {}\n 畸变参数:\n {}\n 旋转矩阵:\n {}\n 平移矩阵:\n {}".format(mtx, dist, rvecs, tvecs)
print(result)
# 内参数矩阵、畸变系数
Camera_intrinsic = {"mtx": mtx, "dist": dist, }
#2，获取当前位姿（原点位姿）
obj_points = objp # 存储3D点
img_points = [] # 存储2D点
for fname in images:
#_, frame = camera.read()
frame = cv2.imread(fname)
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
size = gray.shape[::-1]
ret, corners = cv2.findChessboardCorners(gray, (w, h), None)
if ret: # 画面中有棋盘格
img_points = np.array(corners)
cv2.drawChessboardCorners(frame, (w, h), corners, ret)
# rvec: 旋转向量 tvec: 平移向量
_, rvec, tvec = cv2.solvePnP(obj_points, img_points, Camera_intrinsic["mtx"], Camera_intrinsic["dist"]) # 解算位姿
# print(rvec)
#print(tvec)
distance = math.sqrt(tvec[0][0] ** 2 + tvec[1][0] ** 2 + tvec[2][0] ** 2) # 计算距离，距离相机的距离
#将旋转向量转换成欧拉角（绕x轴转动pitch，绕y轴转动yaw，绕z轴转动roll）
rvec_matrix = cv2.Rodrigues(rvec)[0] # 旋转向量->旋转矩阵
proj_matrix = np.hstack((rvec_matrix, tvec)) # hstack: 水平合并
eulerAngles = cv2.decomposeProjectionMatrix(proj_matrix)[6] # 欧拉角
#print(eulerAngles)
pitch, yaw, roll = eulerAngles[0][0], eulerAngles[1][0], eulerAngles[2][0]
cv2.putText(frame, "dist: %.2fmm, 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.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
#3，画坐标轴和立方体
len = b_w
def draw(img, corners, imgpts, imgpts2):
#line必须转为int型才能绘制
corners = np.int32(corners)
imgpts2 = np.int32(imgpts2)
corner = tuple(corners[0].ravel()) # ravel()方法将数组维度拉成一维数组
# img要画的图像，corner起点，tuple终点，颜色，粗细
img = cv2.line(img, corner, tuple(imgpts2[0].ravel()), (255, 0, 0), 8)
img = cv2.line(img, corner, tuple(imgpts2[1].ravel()), (0, 255, 0), 8)
img = cv2.line(img, corner, tuple(imgpts2[2].ravel()), (0, 0, 255), 8)
font = cv2.FONT_HERSHEY_SIMPLEX
cv2.putText(img, 'X', tuple(imgpts2[0].ravel() + 2), font, 1, (255, 0, 0), 2, cv2.LINE_AA)
cv2.putText(img, 'Y', tuple(imgpts2[1].ravel() + 2), font, 1, (0, 255, 0), 2, cv2.LINE_AA)
cv2.putText(img, 'Z', tuple(imgpts2[2].ravel() + 2), font, 1, (0, 0, 255), 2, cv2.LINE_AA)
imgpts = np.int32(imgpts).reshape(-1, 2) # draw ground floor in green
for i, j in zip(range(4), range(4, 8)): # 正方体顶点逐个连接
img = cv2.line(img, tuple(imgpts[i]), tuple(imgpts[j]), (255, 215, 0), 3) # draw top layer in red color
# imgpts[4:]是八个顶点中上面四个顶点
# imgpts[:4]是八个顶点中下面四个顶点
# 用函数drawContours画出上下两个盖子，它的第一个参数是原始图像，第二个参数是轮廓，一个python列表，第三个参数是轮廓的索引（当设置为-1时绘制所有轮廓）
img = cv2.drawContours(img, [imgpts[4:]], -1, (255, 215, 0), 3)
img = cv2.drawContours(img, [imgpts[:4]], -1, (255, 215, 0), 3)
return img
objp = np.zeros((w * h, 3), np.float32)
objp[:, :2] = np.mgrid[0:w * len:len, 0:h * len:len].T.reshape(-1, 2)
axis = np.float32([[0, 0, 0], [0, 2 * len, 0], [2 * len, 2 * len, 0], [2 * len, 0, 0],
[0, 0, -2 * len], [0, 2 * len, -2 * len], [2 * len, 2 * len, -2 * len], [2 * len, 0, -2 * len]])
axis2 = np.float32([[3 * len, 0, 0], [0, 3 * len, 0], [0, 0, -3 * len]]).reshape(-1, 3)
# images = glob.glob('*.jpg')
i = 1
for fname in images:
img = cv2.imread(fname)
# cv2.imshow('世界坐标系与小盒子', img)
# cv2.waitKey(0)
#img = cv2.resize(img, None, fx=0.4, fy=0.4, interpolation=cv2.INTER_CUBIC)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
# 找到棋盘格角点
# 寻找角点，存入corners，ret是找到角点的flag
ret, corners = cv2.findChessboardCorners(gray, (w, h), None)
if ret is True:
corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
#print(corners2)
# 求解物体位姿的需要
_, rvecs, tvecs, inliers = cv2.solvePnPRansac(objp, corners2, mtx, dist)
# projectPoints()根据所给的3D坐标和已知的几何变换来求解投影后的2D坐标。
# imgpts是整体的8个顶点
imgpts, _ = cv2.projectPoints(axis, rvecs, tvecs, mtx, dist)
# imgpts2是三个坐标轴的x,y,z划线终点
imgpts2, _ = cv2.projectPoints(axis2, rvecs, tvecs, mtx, dist)
#绘制方格
img = draw(img, corners2, imgpts, imgpts2)
#绘制x、y、z
distance = math.sqrt(tvec[0][0] ** 2 + tvec[1][0] ** 2 + tvec[2][0] ** 2) # 计算距离
# print(distance)
rvec_matrix = cv2.Rodrigues(rvec)[0] # 旋转向量->旋转矩阵
proj_matrix = np.hstack((rvec_matrix, tvec)) # hstack: 水平合并
eulerAngles = cv2.decomposeProjectionMatrix(proj_matrix)[6] # 欧拉角
# print(eulerAngles)
pitch, yaw, roll = eulerAngles[0][0], eulerAngles[1][0], eulerAngles[2][0]
p0 = tuple(corners[0].ravel())
cv2.putText(img, "x: %.2f, y: %.2f, dist: %.2fmm, yaw: %.2f, pitch: %.2f, roll: %.2f" % (p0[0],p0[1],distance, yaw, pitch, roll),
(10, frame.shape[0] - 20), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
cv2.imshow('世界坐标系与小盒子', img)
#cv2.imwrite(str(i) + '.png', img)
cv2.waitKey(0)
i += 1
cv2.destroyAllWindows()
print("完毕")
（3-2）标定及位姿结果如下
12920
输入时w=11，h=8，b-w=20
G:\3dversion\weiziguji\.venv\Scripts\python.exe G:\3dversion\weiziguji\XiangJiBiaoDing.py
摄像机矩阵:
[[1.09803736e+03 0.00000000e+00 6.63737765e+02]
[0.00000000e+00 1.09791044e+03 4.38657425e+02]
[0.00000000e+00 0.00000000e+00 1.00000000e+00]]
畸变参数:
[[ 0.00218144 -0.00925043 0.00024789 -0.0002825 0.20841267]]
旋转矩阵:
(array([[ 1.33903054e-03],
[-6.83598890e-02],
[ 1.55438056e+00]]), array([[ 0.03125883],
[-0.05715461],
[ 0.65011621]]), array([[ 0.05605446],
[-0.02753977],
[-0.65790633]]), array([[-0.08126108],
[-0.15363931],
[-1.40200001]]), array([[ 0.28931372],
[-0.10420944],
[-2.06993946]]), array([[-0.18457149],
[-0.0407243 ],
[-2.16699213]]), array([[ 0.25805958],
[-0.06580844],
[ 3.08507844]]), array([[ 0.42490026],
[ 0.06435579],
[-3.07186989]]), array([[-0.36890203],
[ 0.02674808],
[ 2.52486102]]), array([[0.18946187],
[0.07454542],
[1.85677179]]), array([[ 0.14889827],
[-0.21970662],
[ 1.65930809]]), array([[ 0.16029379],
[-0.14407197],
[ 1.03966592]]), array([[-0.21111973],
[-0.06096786],
[-0.01069444]]), array([[ 0.27336398],
[-0.04413062],
[ 0.03325271]]))
平移矩阵:
(array([[ 57.28312237],
[-164.71792893],
[ 657.70732026]]), array([[ -39.45288044],
[-154.25005049],
[ 653.67874071]]), array([[-125.42200989],
[ -36.92050016],
[ 655.34164677]]), array([[-114.60511114],
[ 114.11816638],
[ 616.08898813]]), array([[-71.43727126],
[ 43.06902124],
[624.48437836]]), array([[ 56.38668594],
[127.93492448],
[618.47123209]]), array([[162.7126308 ],
[ 26.32896621],
[619.27933956]]), array([[ 25.09489497],
[ 24.05330674],
[654.03439771]]), array([[ 61.25519273],
[ 46.18683531],
[655.67243967]]), array([[ 165.01894101],
[-153.1005099 ],
[ 611.02969995]]), array([[ 64.09318337],
[-122.12753319],
[ 613.9153159 ]]), array([[ 7.04548206],
[-136.16238216],
[ 608.7700887 ]]), array([[-115.89311233],
[ 13.31740073],
[ 642.59649204]]), array([[ -97.23935687],
[-115.59753028],
[ 610.66435603]]))
此时可以求得x、y、z，rx（pitch）、ry（yaw）、rz（roll）的位姿
有了此位姿即可进行下一步