使用OpenCV进行单目测距

主要利用opencv测量图片中的物体深度

课程作业记录

已知图片中要测的物体的实际高度、宽度、相机内参，根据单张图片求图片中指定物体深度

---

已知参数

内参矩阵：

fx	0	    cx			1433.965 		0			 	639.5
0	   fy	cy 		= 		   0	    1433.965	    511.5
0	    0	     1				      0                0            	 1

畸变参数：

[k1,k2,k3,p1,p2] = [-0.09    0.4416    0    0    -0.162]

物体实际宽高：
宽：117.3mm
高：165.2mm

---

计算焦距、计算X’、计算深度

利用相机模型公式解
主要公式：Z/f = X/X’= Y/Y，Z是要求的f可以算，X’可以算
f = fx * dx（dx为一个像素的宽度）
X’我原来使用u = alpha*X‘+cx 和alpha*f = fx 两个公式求，后来发现不太对（但公式好像又没错）
最后X’求取公式参考这里 ，有一点没懂，先用着吧

X’= h=sqrt ((横坐标之差*Dx)^2+(纵坐标之差Dy) ^2), Dx为每个像素的宽度，Dy为每个像素的高度

   //公式：Z/f = X/X‘ = Y/Y’ ； u = alpha*X‘+cx 或 v = belt*Y‘+cy； alpha*f = fx 或 belt*f = fy ；
    //已知：fx fy
    float dx = width_pic_img/(pix_dis);   
    float f = fx * dx;                  //焦距

    //求X' //像在传感器上的实际高度
    float X_ = sqrt((pix_dis)*(pix_dis)*dx*dx);   
    if(X_ <= 0)     
    {
        X_ = -X_;
    }
    
    //计算深度
    float dis = ((width * f)/X_) * 0.001;  //求深度并从mm转换成m //width已知，f是根据内参矩阵求的，X‘是根据单个像素距离求的

---

完整代码：

例如：

//gjx - 2022-5-21 - 单目测距
#include "opencv2/opencv.hpp"
#include 
#include 
#include 
using namespace std;
using namespace cv;

//局部极大值抑制，这里利用fast特征点的响应值做比较
void selectMax(int window, cv::Mat gray, std::vector<KeyPoint> & kp){
    //window是局部极大值抑制的窗口大小，r为半径
    int r = window / 2;
    if (window != 0){
        //对kp中的点进行局部极大值筛选
        for (int i = 0; i < kp.size(); i++){
            for (int j = i + 1; j < kp.size(); j++){
                //如果两个点的距离小于半径r，则删除其中响应值较小的点
                if (abs(kp[i].pt.x - kp[j].pt.x) + abs(kp[i].pt.y - kp[j].pt.y) <= 2 * r){
                    if (kp[i].response < kp[j].response){
                        std::vector<KeyPoint>::iterator it = kp.begin() + i;
                        kp.erase(it);
                        selectMax(window, gray, kp);
                    }
                    else{
                        std::vector<KeyPoint>::iterator it = kp.begin() + j;
                        kp.erase(it);
                        selectMax(window, gray, kp);
                    }
                }
            }
        }
    }
}
void fastpoint(cv::Mat gray, int threshold, int window, int pointNum, std::vector<KeyPoint> & kp){
    std::vector<KeyPoint> keypoint;
    cv::Ptr<cv::FastFeatureDetector> fast_ = cv::FastFeatureDetector::create(threshold);//threshold 为阈值，越大，特征点越少
    fast_->detect(gray, keypoint);  //fast特征检测
    if (keypoint.size() > pointNum){
        threshold = threshold + 5;
        fastpoint(gray, threshold, window, pointNum, keypoint);
    }
    selectMax(window, gray, keypoint);
    kp.assign(keypoint.begin(), keypoint.end());    //复制可以point到kp
}

int main()
{
    //参数设置
    cv::Mat K = cv::Mat::eye(3, 3, CV_32FC1);  //内参矩阵
    K.at<float>(0,0) = 1433.965;
    K.at<float>(1,1) = 1433.965;
    K.at<float>(0,2) = 639.5;
    K.at<float>(1,2) = 511.5;
    K.at<float>(2,2) = 1.0;

    float fx = 1433.965;        //内参（x）
    float cx = 639.5;           //内参（x）
    float width = 117.3;        //现实中 物体宽度（mm）
    float width_pic_img = 170;  //照片中 物体宽度（mm）// 尺子测量 图1：220mm  图2：170mm  图3：140mm
    float real_dis;

    //蓝色物体像素框 // 一个矩形，用于框出蓝色物体之外的角点 // 单位（像素）
    float pix_x_0;        //图1：480      图2：520        图3：520
    float pix_x_1;        //图1：600      图2：640        图3：640
    float pix_y_0;        //图1：460      图2：580        图3：580
    float pix_y_1;        //图1：600      图2：650        图3：650
    float dis_x_draw;     //画框的像素差//x
    float dis_y_draw;     //..........//y

    //畸变参数
    cv::Mat distort_coeffs = cv::Mat::zeros(1, 5, CV_32FC1); //畸变系数矩阵 顺序是[k1, k2, p1, p2, k3]
    distort_coeffs.at<float>(0,0) = -0.09;
    distort_coeffs.at<float>(0,1) = 0.4416;
    distort_coeffs.at<float>(0,2) = 0;
    distort_coeffs.at<float>(0,3) = 0;

    //非极大值抑制的参数
    int threshold = 45;                                     //fast阈值
    int window1 = 7;                                        //局部非极大值抑制窗口
    int pointMaxNum1 = 200;                                 //特征点最大个数

    //参数选择
    int choice_img = 3;     //1、2、3代表第几张图片
    string file;
    if(choice_img == 1)
    {
        file = "/home/gjx/work/1.9M.bmp";
        pix_x_0 = 480;  
        pix_x_1 = 600;    
        pix_y_0 = 560;    
        pix_y_1 = 600;       
        width_pic_img = 220;
        dis_x_draw = pix_x_1 - pix_x_0;
        dis_y_draw = pix_y_1 - pix_y_0;
        real_dis = 1.9;
    }
    else if(choice_img == 2)
    {
        file = "/home/gjx/work/2.5M.bmp";
        pix_x_0 = 520;  
        pix_x_1 = 640;    
        pix_y_0 = 580;    
        pix_y_1 = 650;       
        width_pic_img = 170;
        dis_x_draw = pix_x_1 - pix_x_0;
        dis_y_draw = pix_y_1 - pix_y_0;
        real_dis = 2.5;
    }
    else if(choice_img == 3)
    {
        file = "/home/gjx/work/3.1M.bmp";
        pix_x_0 = 520;  
        pix_x_1 = 640;    
        pix_y_0 = 580;    
        pix_y_1 = 650;       
        width_pic_img = 130;
        dis_x_draw = pix_x_1 - pix_x_0;
        dis_y_draw = pix_y_1 - pix_y_0;
        real_dis = 3.1;
    }

    //读取图片==================================================================================
    cv::Mat img_orgin = cv::imread(file);
    cv::Mat gray;
    cv::Mat img;

    //图像去畸变
    cv::undistort(img_orgin,img,K,distort_coeffs);
    //提角点
    cv::cvtColor(img, gray, cv::COLOR_BGR2GRAY);
    std::vector<KeyPoint> kp;
    fastpoint(gray, threshold, window1, pointMaxNum1, kp);  //提角点（带非极大值抑制）
    cv::drawKeypoints(img, kp, img, Scalar(0, 255, 0));     //画角点
    cv::rectangle(img, Rect(pix_x_0,pix_y_0,dis_x_draw,dis_y_draw),1,1,0);//画框以便去除其他角点

    //提四个角的角点并计算像素距离
    vector<KeyPoint> temp;
    float sum_x = 0;
    for(vector<KeyPoint>::iterator it = kp.begin();it < kp.end(); it++)
    {
        if((it->pt.x < pix_x_1 && it->pt.x > pix_x_0) && (it->pt.y < pix_y_1 && it->pt.y > pix_y_0))
        {
            img(cv::Rect(it->pt.x,it->pt.y,5,5)).setTo(255);
            temp.push_back(*it);
        }
    }
    vector<KeyPoint>::iterator iterator = temp.begin();
    float temp_ = iterator->pt.x;
    iterator++;
    float pix_dis = temp_ - iterator->pt.x;
    if(pix_dis < 0)
    {
        pix_dis = -pix_dis;
    }
    
    //公式：Z/f = X/X‘ = Y/Y’ ； u = alpha*X‘+cx 或 v = belt*Y‘+cy； alpha*f = fx 或 belt*f = fy ；
    //已知：fx fy
    //计算焦距==================================================================//可以单用x算，也可以单用y的算，也可以都算
    float dx = width_pic_img/(pix_dis);   
    float f = fx * dx;                  //焦距
    
    //求X' //像在传感器上的实际高度
    float X_ = sqrt((pix_dis)*(pix_dis)*dx*dx);   
    if(X_ <= 0)       
    {
        X_ = -X_;
    }
    float dis = ((width * f)/X_) * 0.001;  //求深度并从mm转换成m //width已知，f是根据内参矩阵求的，X‘是根据单个像素距离求的
    putText(img, format("real dis = %f m", real_dis), Point(60, 120), FONT_HERSHEY_SIMPLEX, 1.2, Scalar(0, 0, 255), 5, LINE_8);//在图片上显示文本
    putText(img, format("distance = %f m", dis), Point(60, 160), FONT_HERSHEY_SIMPLEX, 1.2, Scalar(0, 0, 255), 5, LINE_8);
    putText(img, format("img = %d", choice_img), Point(60, 80), FONT_HERSHEY_SIMPLEX, 1.2, Scalar(0, 0, 255), 5, LINE_8);
    cv::namedWindow("img", cv::WINDOW_NORMAL);
    cv::imshow("img", img);
    cv::waitKey(0);

    return 0;
}

---

学习记录，理性参考，欢迎指正