1. 为什么需要单目视觉定位在机器人抓取、AR标记定位等场景中我们经常需要知道物体与相机之间的精确距离。传统测距方法如超声波或激光雷达成本较高而基于单目相机的视觉定位方案只需一个普通摄像头配合OpenCV就能实现毫米级精度的距离测量。我去年给工厂做的零件分拣系统就用了这套方案成本降低了70%以上。单目测距的核心原理是透视n点问题(PnP)。简单来说只要知道物体上至少4个特征点的3D世界坐标、对应的2D像素坐标以及相机内参就能计算出物体相对于相机的位置和姿态。这就好比人类用一只眼睛判断距离——虽然不如双目立体视觉精确但足够应付大多数工业场景。2. 相机标定获取视觉定位的尺子2.1 标定原理与棋盘格准备相机标定就像给相机打造一把精确的尺子。由于镜头存在桶形畸变、枕形畸变等问题原始图像会发生扭曲。我用手机拍过一扇窗户结果直线变成了曲线——这就是典型的镜头畸变。推荐使用OpenCV自带的棋盘格图案路径通常在opencv/samples/data/chessboard.png。这个黑白相间的网格就像我们用的方格纸角点黑白格交界处坐标非常容易计算。实际标定时建议使用A4纸打印棋盘格并贴在平整硬板上棋盘格边长建议10-20mm后续代码中square_size参数要与此一致至少准备15张不同角度的照片覆盖图像各个区域2.2 标定实操代码详解先来看图像采集代码。这段程序会打开摄像头按K键保存当前帧#include opencv2/opencv.hpp using namespace cv; int main() { VideoCapture cap(0); if(!cap.isOpened()) return -1; Mat frame; int count 1; while(1) { cap frame; imshow(Calibration, frame); char key waitKey(1); if(key q) break; if(key k) { imwrite(format(%d.jpg, count), frame); putText(frame, Captured!, Point(50,50), FONT_HERSHEY_SIMPLEX, 1, Scalar(0,255,0), 2); imshow(Calibration, frame); waitKey(300); } } return 0; }标定核心代码使用calibrateCamera()函数。关键参数说明object_points: 每张图片中角点的3D坐标Z0image_points_seq: 检测到的角点像素坐标cameraMatrix: 输出内参矩阵 [fx, 0, cx; 0, fy, cy; 0,0,1]distCoeffs: 输出畸变系数 [k1,k2,p1,p2,k3]// 标定过程核心代码片段 vectorvectorPoint3f object_points; vectorvectorPoint2f image_points_seq; Size board_size(7,7); // 棋盘格内角点数量 Size square_size(10,10); // 每个格子实际尺寸(mm) // 为每张图片初始化3D坐标 for(int t0; timage_count; t) { vectorPoint3f temp; for(int i0; iboard_size.height; i) for(int j0; jboard_size.width; j) temp.push_back(Point3f(i*square_size.width, j*square_size.height, 0)); object_points.push_back(temp); } // 执行标定 calibrateCamera(object_points, image_points_seq, image_size, cameraMatrix, distCoeffs, rvecs, tvecs);标定完成后建议检查重投影误差通常应0.1像素。我遇到过误差过大的情况发现是棋盘格没有完全进入画面导致的。3. PnP测距从原理到实现3.1 solvePnP算法选择OpenCV提供多种PnP求解方法方法所需点数特点适用场景ITERATIVE≥4最精确但较慢通用场景EPNP≥4速度快实时系统P3P4仅需4个点特征点少时DLS≥4适用于共面点平面物体实测发现对于边长8.5cm的方形物体三种方法测距结果如下ITERATIVE: 140.98mm EPNP: 141.07mm P3P: 100.82mm (误差较大)3.2 完整测距代码实现#include opencv2/opencv.hpp using namespace cv; using namespace std; int main() { // 加载测试图像 Mat img imread(target.jpg); // 2D像素坐标 (通过PS或鼠标事件获取) vectorPoint2d img_points { Point2d(152, 92), // 左上 Point2d(426, 94), // 右上 Point2d(428, 394), // 右下 Point2d(126, 380) // 左下 }; // 3D世界坐标 (单位mm) vectorPoint3d obj_points { Point3d(-42.5, -42.5, 0), // 左上 Point3d(42.5, -42.5, 0), // 右上 Point3d(42.5, 42.5, 0), // 右下 Point3d(-42.5, 42.5, 0) // 左下 }; // 标定获取的内参和畸变系数 Mat camera_matrix (Mat_double(3,3) 505.17, 0, 310.17, 0, 488.61, 249.95, 0, 0, 1); Mat dist_coeffs (Mat_double(5,1) 0.131, -0.965, 0.0011, -0.0011, 2.286); // 求解PnP Mat rvec, tvec; solvePnP(obj_points, img_points, camera_matrix, dist_coeffs, rvec, tvec, false, SOLVEPNP_ITERATIVE); // 计算相机位置 Mat R; Rodrigues(rvec, R); // 旋转向量转矩阵 Mat cam_pos -R.t() * tvec; // 相机在世界坐标系中的位置 cout Distance: cam_pos.atdouble(2) mm endl; // 可视化特征点 for(auto pt : img_points) { circle(img, pt, 5, Scalar(0,0,255), -1); } imshow(Result, img); waitKey(0); return 0; }4. 工程实践中的优化技巧4.1 特征点提取自动化手动标注特征点效率低下。对于规则物体可以用以下方法自动检测// 自动检测矩形物体轮廓 vectorvectorPoint contours; findContours(binary_img, contours, RETR_EXTERNAL, CHAIN_APPROX_SIMPLE); // 提取最大轮廓并拟合多边形 vectorPoint approx; approxPolyDP(contours[max_idx], approx, 10, true); // 排序四个顶点 (左上开始顺时针) sort(approx.begin(), approx.end(), [](Point a, Point b) { return a.x a.y b.x b.y; });4.2 精度提升方法在实际项目中我通过以下方法将误差控制在1%以内温度补偿相机CMOS会随温度漂移每小时重新标定一次多帧平均采集10帧数据取中值光照控制使用850nm红外光源滤光片消除环境光干扰非线性优化使用Levenberg-Marquardt算法优化PnP结果4.3 常见问题排查标定误差大检查棋盘格是否平整拍摄角度是否多样PnP结果异常确认世界坐标系与像素坐标系点顺序一致深度值为负检查坐标系定义通常Z轴指向相机前方OpenCV版本差异solvePnP的参数顺序在3.x和4.x版本有变化记得第一次调试时我得到的结果总是差30cm后来发现是把世界坐标单位从米错当成毫米了。这种低级错误在工程中反而最常见。
【OpenCV实战】从相机标定到PnP测距：手把手实现单目视觉定位（附完整C++源码）
1. 为什么需要单目视觉定位在机器人抓取、AR标记定位等场景中我们经常需要知道物体与相机之间的精确距离。传统测距方法如超声波或激光雷达成本较高而基于单目相机的视觉定位方案只需一个普通摄像头配合OpenCV就能实现毫米级精度的距离测量。我去年给工厂做的零件分拣系统就用了这套方案成本降低了70%以上。单目测距的核心原理是透视n点问题(PnP)。简单来说只要知道物体上至少4个特征点的3D世界坐标、对应的2D像素坐标以及相机内参就能计算出物体相对于相机的位置和姿态。这就好比人类用一只眼睛判断距离——虽然不如双目立体视觉精确但足够应付大多数工业场景。2. 相机标定获取视觉定位的尺子2.1 标定原理与棋盘格准备相机标定就像给相机打造一把精确的尺子。由于镜头存在桶形畸变、枕形畸变等问题原始图像会发生扭曲。我用手机拍过一扇窗户结果直线变成了曲线——这就是典型的镜头畸变。推荐使用OpenCV自带的棋盘格图案路径通常在opencv/samples/data/chessboard.png。这个黑白相间的网格就像我们用的方格纸角点黑白格交界处坐标非常容易计算。实际标定时建议使用A4纸打印棋盘格并贴在平整硬板上棋盘格边长建议10-20mm后续代码中square_size参数要与此一致至少准备15张不同角度的照片覆盖图像各个区域2.2 标定实操代码详解先来看图像采集代码。这段程序会打开摄像头按K键保存当前帧#include opencv2/opencv.hpp using namespace cv; int main() { VideoCapture cap(0); if(!cap.isOpened()) return -1; Mat frame; int count 1; while(1) { cap frame; imshow(Calibration, frame); char key waitKey(1); if(key q) break; if(key k) { imwrite(format(%d.jpg, count), frame); putText(frame, Captured!, Point(50,50), FONT_HERSHEY_SIMPLEX, 1, Scalar(0,255,0), 2); imshow(Calibration, frame); waitKey(300); } } return 0; }标定核心代码使用calibrateCamera()函数。关键参数说明object_points: 每张图片中角点的3D坐标Z0image_points_seq: 检测到的角点像素坐标cameraMatrix: 输出内参矩阵 [fx, 0, cx; 0, fy, cy; 0,0,1]distCoeffs: 输出畸变系数 [k1,k2,p1,p2,k3]// 标定过程核心代码片段 vectorvectorPoint3f object_points; vectorvectorPoint2f image_points_seq; Size board_size(7,7); // 棋盘格内角点数量 Size square_size(10,10); // 每个格子实际尺寸(mm) // 为每张图片初始化3D坐标 for(int t0; timage_count; t) { vectorPoint3f temp; for(int i0; iboard_size.height; i) for(int j0; jboard_size.width; j) temp.push_back(Point3f(i*square_size.width, j*square_size.height, 0)); object_points.push_back(temp); } // 执行标定 calibrateCamera(object_points, image_points_seq, image_size, cameraMatrix, distCoeffs, rvecs, tvecs);标定完成后建议检查重投影误差通常应0.1像素。我遇到过误差过大的情况发现是棋盘格没有完全进入画面导致的。3. PnP测距从原理到实现3.1 solvePnP算法选择OpenCV提供多种PnP求解方法方法所需点数特点适用场景ITERATIVE≥4最精确但较慢通用场景EPNP≥4速度快实时系统P3P4仅需4个点特征点少时DLS≥4适用于共面点平面物体实测发现对于边长8.5cm的方形物体三种方法测距结果如下ITERATIVE: 140.98mm EPNP: 141.07mm P3P: 100.82mm (误差较大)3.2 完整测距代码实现#include opencv2/opencv.hpp using namespace cv; using namespace std; int main() { // 加载测试图像 Mat img imread(target.jpg); // 2D像素坐标 (通过PS或鼠标事件获取) vectorPoint2d img_points { Point2d(152, 92), // 左上 Point2d(426, 94), // 右上 Point2d(428, 394), // 右下 Point2d(126, 380) // 左下 }; // 3D世界坐标 (单位mm) vectorPoint3d obj_points { Point3d(-42.5, -42.5, 0), // 左上 Point3d(42.5, -42.5, 0), // 右上 Point3d(42.5, 42.5, 0), // 右下 Point3d(-42.5, 42.5, 0) // 左下 }; // 标定获取的内参和畸变系数 Mat camera_matrix (Mat_double(3,3) 505.17, 0, 310.17, 0, 488.61, 249.95, 0, 0, 1); Mat dist_coeffs (Mat_double(5,1) 0.131, -0.965, 0.0011, -0.0011, 2.286); // 求解PnP Mat rvec, tvec; solvePnP(obj_points, img_points, camera_matrix, dist_coeffs, rvec, tvec, false, SOLVEPNP_ITERATIVE); // 计算相机位置 Mat R; Rodrigues(rvec, R); // 旋转向量转矩阵 Mat cam_pos -R.t() * tvec; // 相机在世界坐标系中的位置 cout Distance: cam_pos.atdouble(2) mm endl; // 可视化特征点 for(auto pt : img_points) { circle(img, pt, 5, Scalar(0,0,255), -1); } imshow(Result, img); waitKey(0); return 0; }4. 工程实践中的优化技巧4.1 特征点提取自动化手动标注特征点效率低下。对于规则物体可以用以下方法自动检测// 自动检测矩形物体轮廓 vectorvectorPoint contours; findContours(binary_img, contours, RETR_EXTERNAL, CHAIN_APPROX_SIMPLE); // 提取最大轮廓并拟合多边形 vectorPoint approx; approxPolyDP(contours[max_idx], approx, 10, true); // 排序四个顶点 (左上开始顺时针) sort(approx.begin(), approx.end(), [](Point a, Point b) { return a.x a.y b.x b.y; });4.2 精度提升方法在实际项目中我通过以下方法将误差控制在1%以内温度补偿相机CMOS会随温度漂移每小时重新标定一次多帧平均采集10帧数据取中值光照控制使用850nm红外光源滤光片消除环境光干扰非线性优化使用Levenberg-Marquardt算法优化PnP结果4.3 常见问题排查标定误差大检查棋盘格是否平整拍摄角度是否多样PnP结果异常确认世界坐标系与像素坐标系点顺序一致深度值为负检查坐标系定义通常Z轴指向相机前方OpenCV版本差异solvePnP的参数顺序在3.x和4.x版本有变化记得第一次调试时我得到的结果总是差30cm后来发现是把世界坐标单位从米错当成毫米了。这种低级错误在工程中反而最常见。
