【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点对应关系求解相机参数。下面这段代码我优化了错误处理机制// 改进的标定代码 vectorvectorPoint2f imagePoints; Size boardSize(7,7); float squareSize 25.0f; // 棋盘格实际尺寸(mm) // 世界坐标系中的角点坐标 vectorvectorPoint3f objectPoints(1); for(int i0; iboardSize.height; i) for(int j0; jboardSize.width; j) objectPoints[0].emplace_back(j*squareSize, i*squareSize, 0); objectPoints.resize(imagePoints.size(), objectPoints[0]); // 执行标定 Mat cameraMatrix, distCoeffs; vectorMat 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算法深度剖析PnPPerspective-n-Point问题的本质是求解相机位姿。当已知物体3D坐标世界坐标系对应2D图像坐标相机内参就能计算出相机相对于物体的旋转(R)和平移(T)。solvePnP提供了几种求解方法ITERATIVE默认基于Levenberg-Marquardt优化精度高但速度慢EPNP适合点数4的情况速度快P3P只需3个点但对噪声敏感// PnP求解代码优化版 vectorPoint3f objectPoints { {-42.5, -42.5, 0}, // 左上 {42.5, -42.5, 0}, // 右上 {42.5, 42.5, 0}, // 右下 {-42.5, 42.5, 0} // 左下 }; vectorPoint2f 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.atdouble(0) 0; zAxis.atdouble(1) 0; zAxis.atdouble(2) 1; Mat camZ rotMat.t() * zAxis; double zDistance tvec.dot(camZ);实测中发现三个精度提升技巧使用至少4个特征点推荐6-8个特征点应尽量分散在物体四周对于平面物体确保Z坐标设置正确4. 工程实践与调试技巧4.1 常见问题排查指南在项目落地过程中我踩过不少坑这里分享几个典型问题的解决方案问题1标定误差过大检查棋盘格是否平整确保拍摄角度多样建议15-20张尝试调整findChessboardCorners的窗口大小问题2PnP结果不稳定确认世界坐标系与图像坐标系对应关系正确检查特征点坐标是否准确尝试不同的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();多线程处理将图像采集和计算分离ROI优化只在感兴趣区域执行特征检测在最近的一个AGV导航项目中通过上述优化将处理速度从原来的200ms/帧提升到了50ms/帧满足了实时性要求。关键是要根据具体应用场景选择合适的精度和速度平衡点。