棋盘格测距-单目相机（OpenCV/C++）

一、文章内容简述：

1’ 通过cv::findChessboardCorners寻找棋盘格角点

2‘ 用cv::solvePnP计算旋转向量rvec和平移向量tvec

3’ 通过公式计算相机到棋盘格的距离

float distance = sqrt(tvec.at<double>(0,0) * tvec.at<double>(0,0) + tvec.at<double>(1,0) * tvec.at<double>(1,0) + tvec.at<double>(2,0) * tvec.at<double>(2,0)) / 10;

 

二、实现过程

已完成单目相机标定的情况下：

（可以参考http://t.csdnimg.cn/v72VN  虽然是我很久之前写的python的，但实现是没啥问题）

需要以下内容：

1、已知相机的内参矩阵cameraMatrix和畸变参数distCoeffs（相机标定）

2、需要拍摄一张棋盘格图像

3、需要知道棋盘格的w方向和h方向角点数量

实现代码如下：

#include <opencv2/opencv.hpp>

int main() {
    // 读取棋盘格图像
    cv::Mat image = cv::imread("chessboard.jpg");
    
    // 定义棋盘格的尺寸和角点列表
    cv::Size patternSize(11, 8);
    std::vector<cv::Point2f> corners;
    
    // 寻找棋盘格角点
    bool found = cv::findChessboardCorners(image, patternSize, corners);
    
    if (found) {
        // 优化角点坐标精度
        cv::cornerSubPix(image, corners, cv::Size(11, 11), cv::Size(-1, -1),
                         cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::COUNT, 30, 0.1));
        
        // 定义棋盘格三维坐标
        std::vector<cv::Point3f> objectPoints;
        float squareSize = 1.0;
        for (int i = 0; i < patternSize.height; ++i) {
            for (int j = 0; j < patternSize.width; ++j) {
                objectPoints.push_back(cv::Point3f(j * squareSize, i * squareSize, 0));
            }
        }
        
        // 定义相机参数
        cv::Mat cameraMatrix = cv::Mat::eye(3, 3, CV_64F);
        cv::Mat distCoeffs = cv::Mat::zeros(4, 1, CV_64F);
        
        // 计算 rvec 和 tvec
        cv::Mat rvec, tvec;
        cv::solvePnP(objectPoints, corners, cameraMatrix, distCoeffs, rvec, tvec);
        
        // 输出结果
        std::cout << "rvec: " << rvec << std::endl;
        std::cout << "tvec: " << tvec << std::endl;

        //计算相机距离被测物的实际距离
        float distance = sqrt(tvec.at<double>(0,0) * tvec.at<double>(0,0) + tvec.at<double>(1,0) * tvec.at<double>(1,0) + tvec.at<double>(2,0) * tvec.at<double>(2,0)) / 10; 
        std::cout << "distance = "<< distance << std::endl;

    } else {
        std::cout << "未找到棋盘格角点！" << std::endl;
    }
    
    return 0;
}

三、补充

使用cv::solvePnP测距的方法，任意被测物都可以实现。只需要有世界坐标系的角点（手动测量）和对应点的像素坐标即可。

参考文章：http://t.csdnimg.cn/KixCO

本文中使用棋盘格做测距，通过cv::findChessboardCorners寻找所有棋盘格角点后再计算可以大大提高测距精度。