使用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

利用相机模型公式解
主要公式： 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为每个像素的高度

    float dx = width_pic_img/(pix_dis);
    float f = fx * dx;

    float X_ = sqrt((pix_dis)*(pix_dis)*dx*dx);
    if(X_  0)
    {
        X_ = -X_;
    }

    float dis = ((width * f)/X_) * 0.001;

例如：

#include "opencv2/opencv.hpp"
#include
#include
#include
using namespace std;
using namespace cv;

void selectMax(int window, cv::Mat gray, std::vector<KeyPoint> & kp){

    int r = window / 2;
    if (window != 0){

        for (int i = 0; i < kp.size(); i++){
            for (int j = i + 1; j < kp.size(); j++){

                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);
    fast_->detect(gray, keypoint);
    if (keypoint.size() > pointNum){
        threshold = threshold + 5;
        fastpoint(gray, threshold, window, pointNum, keypoint);
    }
    selectMax(window, gray, keypoint);
    kp.assign(keypoint.begin(), keypoint.end());
}

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;
    float cx = 639.5;
    float width = 117.3;
    float width_pic_img = 170;
    float real_dis;

    float pix_x_0;
    float pix_x_1;
    float pix_y_0;
    float pix_y_1;
    float dis_x_draw;
    float dis_y_draw;

    cv::Mat distort_coeffs = cv::Mat::zeros(1, 5, CV_32FC1);
    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;
    int window1 = 7;
    int pointMaxNum1 = 200;

    int choice_img = 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;
    }

    float dx = width_pic_img/(pix_dis);
    float f = fx * dx;

    float X_ = sqrt((pix_dis)*(pix_dis)*dx*dx);
    if(X_  0)
    {
        X_ = -X_;
    }
    float dis = ((width * f)/X_) * 0.001;
    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;
}

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

Original: https://blog.csdn.net/weixin_46372127/article/details/124899160
Author: MoonSheep_G
Title: 使用OpenCV进行单目测距