使用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‘+cxalpha*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 = (pix_dis)/width_pic_img;   
    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 <iostream>
#include <vector>
#include <math.h>
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 = (pix_dis)/width_pic_img;   
    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;
}

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

文章出处登录后可见!

已经登录?立即刷新

共计人评分,平均

到目前为止还没有投票!成为第一位评论此文章。

(0)
上一篇 2022年5月22日 上午11:08
下一篇 2022年5月22日 上午11:12

相关推荐

本站注重文章个人版权,不会主动收集付费或者带有商业版权的文章,如果出现侵权情况只可能是作者后期更改了版权声明,如果出现这种情况请主动联系我们,我们看到会在第一时间删除!本站专注于人工智能高质量优质文章收集,方便各位学者快速找到学习资源,本站收集的文章都会附上文章出处,如果不愿意分享到本平台,我们会第一时间删除!