使用OpenCV进行单目测距

使用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 fyfloat 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 <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() + ase(it);selectMax(window, gray, kp);}else{std::vector<KeyPoint>::iterator it = kp.begin() + ase(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(), d());    //复制可以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：140mmfloat real_dis;//蓝色物体像素框 // 一个矩形，用于框出蓝色物体之外的角点 // 单位（像素）float pix_x_0;        //图1：480      图2：520        图3：520float pix_x_1;        //图1：600      图2：640        图3：640float pix_y_0;        //图1：460      图2：580        图3：580float pix_y_1;        //图1：600      图2：650        图3：650float dis_x_draw;     //画框的像素差//xfloat 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;
}

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