OpenCV solvePnP位姿估计

目录

一、概述

二、实现代码

2.1solvePnP函数

2.1.1输入参数

2.1.2输出参数

2.2完整代码

三、实现效果

3.1标定板位姿

3.2标定板到相机的变换矩阵

---

一、概述

        完成相机标定后，可以通过检测标定板在图像中的位置来计算标定板在相机坐标系下的位姿（外参）。通过cv2.solvePnP求出标定板在相机坐标系下的姿态矩阵后，保存下来可用于后续的手眼标定

具体步骤如下：

1. 相机标定：获取相机的内参矩阵和畸变系数。
2. 检测标定板：在图像中检测棋盘格角点。
3. 计算外参：使用 cv2.solvePnP 计算标定板在相机坐标系下的位姿。

二、实现代码

2.1solvePnP函数

        cv2.solvePnP 是 OpenCV 中用于估计对象姿态的函数。它计算从 3D 点到 2D 图像点的变换，包括旋转和位移。

retval, rvec, tvec = cv2.solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs[, rvec[, tvec[, useExtrinsicGuess[, flags]]]])

2.1.1输入参数

2.1.2输出参数

2.2完整代码

import cv2
import numpy as np
import glob

# 加载相机标定参数
with np.load('camera_calibration.npz') as data:
    mtx = data['mtx']
    dist = data['dist']

# 设置棋盘格参数
chessboard_size = (11, 8)  # 棋盘格的内角点个数
square_size = 1.0  # 棋盘格每个方格的实际大小，单位可以是毫米、厘米或米

# 准备棋盘格的世界坐标系坐标（假设z=0）
objp = np.zeros((chessboard_size[0] * chessboard_size[1], 3), np.float32)
objp[:, :2] = np.mgrid[0:chessboard_size[0], 0:chessboard_size[1]].T.reshape(-1, 2)
objp *= square_size

# 用于存储每张图像的位姿矩阵
pose_matrices = []

# 获取所有棋盘格图像的路径
images = glob.glob('board_image/*.png')

for image_path in images:
    # 读取图像
    img = cv2.imread(image_path)
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)

    # 检测棋盘格角点
    ret, corners = cv2.findChessboardCorners(gray, chessboard_size, None)

    if ret:
        # 优化角点位置到亚像素级精度
        criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
        corners = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)

        # 计算标定板在相机坐标系下的位姿(标定板在相机坐标系下的位姿）
        ret, rvec, tvec = cv2.solvePnP(objp, corners, mtx, dist)

        # 将旋转向量转换为旋转矩阵
        rmat, _ = cv2.Rodrigues(rvec)

        # 组合旋转矩阵和平移向量为位姿矩阵
        pose_matrix = np.hstack((rmat, tvec))
        pose_matrices.append(pose_matrix)

        # 在图像上绘制坐标轴用于可视化
        axis = np.float32([[3, 0, 0], [0, 3, 0], [0, 0, -3]]).reshape(-1, 3)
        imgpts, _ = cv2.projectPoints(axis, rvec, tvec, mtx, dist)

        img = cv2.drawChessboardCorners(img, chessboard_size, corners, ret)
        corner = tuple(corners[0].ravel().astype(int))

        # 确保坐标点是以正确的格式传递给 cv2.line 函数
        img = cv2.line(img, corner, tuple(imgpts[0].ravel().astype(int)), (255, 0, 0), 5)
        img = cv2.line(img, corner, tuple(imgpts[1].ravel().astype(int)), (0, 255, 0), 5)
        img = cv2.line(img, corner, tuple(imgpts[2].ravel().astype(int)), (0, 0, 255), 5)

        cv2.imshow('Pose Estimation', img)
        cv2.waitKey(0)

cv2.destroyAllWindows()

# # 保存所有位姿矩阵到文件
# np.save('pose_matrices.npy', np.array(pose_matrices))
#
# 输出所有位姿矩阵
for i, pose_matrix in enumerate(pose_matrices):
    print(f"Pose matrix for image {i+1}:\n", pose_matrix)

三、实现效果

3.1标定板位姿

3.2标定板到相机的变换矩阵

Pose matrix for image 1:
 [[ 9.54690321e-01  1.63896269e-01  2.48403711e-01 -9.17541763e+00]
 [-1.45600442e-01  9.85200423e-01 -9.04468778e-02 -1.04041170e+01]
 [-2.59551347e-01  5.01810686e-02  9.64424677e-01  8.94162448e+01]]
Pose matrix for image 2:
 [[ 9.68117431e-01 -2.30857543e-02  2.49430728e-01 -4.40319806e+00]
 [ 5.02887074e-02  9.93383921e-01 -1.03244525e-01 -8.17764301e+00]
 [-2.45396997e-01  1.12496373e-01  9.62873242e-01  8.84715217e+01]]
Pose matrix for image 3:
 [[  0.77862222  -0.56844737   0.26573487  -4.22080186]
 [  0.61191435   0.78162231  -0.12094374 -12.23688825]
 [ -0.13895415   0.25677646   0.95642961  87.62645824]]
Pose matrix for image 4:
 [[ 3.65280489e-01 -8.84489869e-01  2.90254777e-01 -4.59486253e-01]
 [ 9.30890782e-01  3.45882851e-01 -1.17504915e-01 -1.47317632e+01]
 [ 3.53775679e-03  3.13117749e-01  9.49707723e-01  8.60033729e+01]]
Pose matrix for image 5:
 [[ 2.10536679e-02 -9.53945915e-01  2.99238925e-01  2.39861926e+00]
 [ 9.94637523e-01 -1.03281967e-02 -1.02905426e-01 -1.39552702e+01]
 [ 1.01256809e-01  2.99800800e-01  9.48612955e-01  8.52141515e+01]]
Pose matrix for image 6:
 [[-8.08420711e-01 -5.24826800e-01  2.66482240e-01  5.53844533e+00]
 [ 5.25127881e-01 -8.47600539e-01 -7.62498222e-02 -8.15940333e+00]
 [ 2.65888440e-01  7.82953186e-02  9.60819015e-01  8.55355115e+01]]
Pose matrix for image 7:
 [[-0.90930726  0.32912313  0.25463362  0.90404698]
 [-0.3662639  -0.92345944 -0.11433903 -3.69221782]
 [ 0.1975122  -0.19723241  0.96025429 87.56252021]]