solvepnp

### 关于 `solvePnP` 函数的使用方法 OpenCV 中的 `cv::solvePnP` 函数用于解决透视 n 点 (Perspective-n-Point, PnP) 问题，即给定一组三维空间中的点及其对应的图像平面上的投影点，估计相机的姿态（旋转和平移）。此函数广泛应用于增强现实、机器人导航等领域。 #### 函数签名 ```cpp bool cv::solvePnP( InputArray objectPoints, InputArray imagePoints, InputArray cameraMatrix, InputArray distCoeffs, OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess = false, int flags = SOLVEPNP_ITERATIVE ); ``` 参数说明如下： - **objectPoints**: 表示世界坐标系下的物体特征点位置，尺寸为 N×3 或者 1×N 的矩阵，其中每行是一个三维向量。 - **imagePoints**: 对应上述对象点在同一视角下成像后的像素坐标，同样可以是 N×2 或者 1×N 形式的二维数组。 - **cameraMatrix**: 内参矩阵 K，通常由校准过程获得，形式为 \(\begin{bmatrix}f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0&0&1\end{bmatrix}\)，\(c_x\) 和 \(c_y\) 是主点偏移；\(f_x,f_y\) 则代表焦距[^1]。 - **distCoeffs**: 镜头畸变系数，一般情况下可以直接传入零矢量表示忽略畸变影响。 - **rvec**, **tvec**: 输出变量，分别存储旋转向量和位移向量。这两个向量描述了从世界坐标到摄像机坐标的变换关系。 - **useExtrinsicGuess**: 如果设置为 true，则会利用输入的初始猜测值来加速求解速度，默认为 false。 - **flags**: 解算器标志位，决定了采用哪种算法来进行优化计算，比如迭代法(SOLVEPNP_ITERATIVE)。 下面给出一段简单的 Python 实现例子，展示如何调用该 API 来获取目标物相对于摄像头的位置姿态信息： ```python import numpy as np import cv2 # 假设已知一些棋盘格角点的世界坐标(单位毫米) objp = np.zeros((6*9, 3), np.float32) objp[:, :2] = np.mgrid[0:9, 0:6].T.reshape(-1, 2)*25 # 每个方块边长25mm axis = np.float32([[3, 0, 0], [0, 3, 0], [0, 0, -3]]).reshape(-1, 3) criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001) for fname in images: img = cv2.imread(fname) gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY) ret, corners = cv2.findChessboardCorners(gray, (9, 6)) if ret == True: subpix_corners = cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria) _, rvecs, tvecs, _ = cv2.solvePnPRansac(objp, subpix_corners, mtx, dist) # 绘制轴线辅助理解结果 imgpts, jac = cv2.projectPoints(axis, rvecs, tvecs, mtx, dist) origin = tuple(corners[0].ravel()) axis_endpoints = [(tuple(p.ravel())) for p in imgpts] draw_axis_lines(img, origin, *axis_endpoints) def draw_axis_lines(image, o, a, b, c): """Draw coordinate axes on an image.""" color_map = {'X': (0, 0, 255), 'Y': (0, 255, 0), 'Z': (255, 0, 0)} labels = ['X', 'Y', 'Z'] points = zip([o]*len(labels), [a,b,c]) for label, pair in zip(labels, points): start_point, end_point = map(tuple,map(int,pair)) cv2.line(image,start_point,end_point,color_map[label],5) cv2.putText(image,label,end_point,cv2.FONT_HERSHEY_SIMPLEX,1,color_map[label],2) ``` 这段代码通过检测并精确定位标准棋盘图案上的角点作为参照物，进而估算出其相对拍摄设备的空间方位，并可视化显示出来。注意这里还加入了 RANSAC 方法提高鲁棒性以应对可能存在的异常数据干扰。