主要利用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 = (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;
}
学习记录，理性参考，欢迎指正
文章出处登录后可见！
已经登录？立即刷新
