【OpenCV实战】从相机标定到PnP测距：手把手实现单目视觉定位（C++代码详解）-编程阁

1. 相机标定基础与实战准备

单目视觉定位就像给机器人装上了一只"智慧之眼"，而相机标定就是教会这只眼睛如何正确理解世界。想象一下，如果你戴了一副度数不合适的眼镜，看到的物体位置和形状都会失真——相机标定要解决的就是类似的问题。

在实际操作前，我们需要准备以下硬件：

普通USB摄像头（笔记本内置摄像头也可）
打印好的棋盘格标定板（建议A4尺寸）
平整的硬纸板（用于固定标定板）

棋盘格标定板推荐使用7x7的网格，每个格子边长建议20-30mm。我常用瓦楞纸板做底板，既轻便又能保持平整。有个小技巧：把标定板贴在烤盘上，这样既平整又方便多角度拍摄。

2. 相机标定全流程详解

2.1 图像采集实战技巧

采集标定图像时，很多新手容易犯一个错误——只在同一角度拍摄。我建议采用"空间八字法"：

保持标定板静止，移动摄像头
从左上、右上、正前、左下、右下五个基本方位拍摄
每个方位再分别做±30°的倾斜
最后加几张近距离特写（距离20cm左右）

// 改进版的图像采集代码 VideoCapture cap(0); if(!cap.isOpened()) { cerr << "摄像头打开失败，请检查设备连接" << endl; return -1; } int count = 1; while(true) { Mat frame; cap >> frame; imshow("实时预览", frame); int key = waitKey(30); if(key == 's') { // 按s键保存 string filename = format("calib_%02d.jpg", count++); imwrite(filename, frame); cout << "已保存：" << filename << endl; } else if(key == 27) break; // ESC退出 }

2.2 标定核心代码逐行解析

张正友标定法的核心在于通过多组2D-3D点对应关系求解相机参数。下面这段代码我优化了错误处理机制：

// 改进的标定代码 vector<vector<Point2f>> imagePoints; Size boardSize(7,7); float squareSize = 25.0f; // 棋盘格实际尺寸(mm) // 世界坐标系中的角点坐标 vector<vector<Point3f>> objectPoints(1); for(int i=0; i<boardSize.height; ++i) for(int j=0; j<boardSize.width; ++j) objectPoints[0].emplace_back(j*squareSize, i*squareSize, 0); objectPoints.resize(imagePoints.size(), objectPoints[0]); // 执行标定 Mat cameraMatrix, distCoeffs; vector<Mat> rvecs, tvecs; double rms = calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, rvecs, tvecs, CALIB_FIX_K3 | CALIB_FIX_PRINCIPAL_POINT); cout << "重投影误差：" << rms << "像素" << endl;

关键参数说明：

CALIB_FIX_K3：固定k3畸变系数，防止过拟合
CALIB_FIX_PRINCIPAL_POINT：固定主点坐标，提升稳定性
理想的RMS误差应小于0.5像素

3. PnP测距原理与实现

3.1 solvePnP算法深度剖析

PnP（Perspective-n-Point）问题的本质是求解相机位姿。当已知：

物体3D坐标（世界坐标系）
对应2D图像坐标
相机内参

就能计算出相机相对于物体的旋转(R)和平移(T)。solvePnP提供了几种求解方法：

ITERATIVE（默认）：基于Levenberg-Marquardt优化，精度高但速度慢
EPNP：适合点数>4的情况，速度快
P3P：只需3个点，但对噪声敏感

// PnP求解代码优化版 vector<Point3f> objectPoints = { {-42.5, -42.5, 0}, // 左上 {42.5, -42.5, 0}, // 右上 {42.5, 42.5, 0}, // 右下 {-42.5, 42.5, 0} // 左下 }; vector<Point2f> imagePoints; // 通过特征提取或手动标注获取图像坐标 Mat rvec, tvec; bool success = solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec, false, SOLVEPNP_ITERATIVE); if(!success) { cerr << "PnP求解失败！" << endl; return -1; }

3.2 距离计算与精度提升

获取平移向量T后，实际距离计算需要特别注意坐标系转换。我总结了一个可靠的计算公式：

Mat rotMat; Rodrigues(rvec, rotMat); // 旋转向量转矩阵 // 计算相机在世界坐标系中的位置 Mat camPos = -rotMat.t() * tvec; double distance = norm(camPos); // 计算欧式距离 // 更精确的Z轴距离计算 Mat zAxis(3,1,CV_64F); zAxis.at<double>(0) = 0; zAxis.at<double>(1) = 0; zAxis.at<double>(2) = 1; Mat camZ = rotMat.t() * zAxis; double zDistance = tvec.dot(camZ);

实测中发现三个精度提升技巧：

使用至少4个特征点（推荐6-8个）
特征点应尽量分散在物体四周
对于平面物体，确保Z坐标设置正确

4. 工程实践与调试技巧

4.1 常见问题排查指南

在项目落地过程中，我踩过不少坑，这里分享几个典型问题的解决方案：

问题1：标定误差过大

检查棋盘格是否平整
确保拍摄角度多样（建议15-20张）
尝试调整findChessboardCorners的窗口大小

问题2：PnP结果不稳定

确认世界坐标系与图像坐标系对应关系正确
检查特征点坐标是否准确
尝试不同的PnP求解方法

问题3：距离计算偏差大

验证物体实际尺寸输入是否正确
检查相机内参是否准确
确保物体与相机光轴基本垂直

4.2 性能优化建议

对于实时性要求高的应用，可以采用以下优化策略：

缓存标定结果：将相机参数保存为YAML文件

// 保存相机参数 FileStorage fs("camera_params.yml", FileStorage::WRITE); fs << "camera_matrix" << cameraMatrix; fs << "dist_coeffs" << distCoeffs; fs.release(); // 读取相机参数 FileStorage fs("camera_params.yml", FileStorage::READ); fs["camera_matrix"] >> cameraMatrix; fs["dist_coeffs"] >> distCoeffs; fs.release();