103 lines
3.5 KiB
C
103 lines
3.5 KiB
C
|
#pragma once
|
||
|
|
|
||
|
|
#include <opencv2/opencv.hpp>
|
||
|
|
#include <opencv2/calib3d.hpp>
|
||
|
|
|
||
|
|
#include <cstdlib>
|
||
|
|
#include <vector>
|
||
|
|
#include "sort_test/detection.h"
|
||
|
|
#include <unistd.h>
|
||
|
|
|
||
|
|
|
||
|
|
using namespace cv;
|
||
|
|
|
||
|
|
class Ranging
|
||
|
|
{
|
||
|
|
private:
|
||
|
|
// rknn_handle hdx;
|
||
|
|
cv::VideoCapture vcapture;
|
||
|
|
Detection yolov5s;
|
||
|
|
/* 2-stereo
|
||
|
|
Mat cam_matrix_left = (Mat_<double>(3, 3) <<
|
||
|
|
4.791153893499601e+02, 0, 0,
|
||
|
|
0,4.798785696902847e+02, 0,
|
||
|
|
3.195422157433843e+02, 2.355081129251360e+02, 1);
|
||
|
|
Mat cam_matrix_right = (Mat_<double>(3, 3) <<
|
||
|
|
4.851946576927952e+02, 0, 0,
|
||
|
|
0,4.848412081674973e+02, 0,
|
||
|
|
3.139237321746737e+02, 2.316610217516937e+02, 1);
|
||
|
|
Mat distortion_l = (Mat_<double>(1, 5) <<0.091215339806518,-0.088601421082219, 0,
|
||
|
|
0, 0);
|
||
|
|
|
||
|
|
Mat distortion_r = (Mat_<double>(1, 5) <<0.086266675232625,-0.070869167707634, 0,
|
||
|
|
0, 0);
|
||
|
|
Mat rotate = (Mat_<double>(3, 3) <<
|
||
|
|
0.999999240684794, -5.243648990073547e-05, -0.001231210888060,
|
||
|
|
5.272610271745578e-05, 0.999999970951700, -0.002653487630467,
|
||
|
|
0.001231198519510, -2.352594617066257e-04,0.999999214401287);
|
||
|
|
Mat trans = (Mat_<double>(3, 1) <<
|
||
|
|
-61.636981845981540, -1.107000282904877, -1.084419989523733);
|
||
|
|
*/
|
||
|
|
/*
|
||
|
|
Mat cam_matrix_left = (Mat_<double>(3, 3) <<
|
||
|
|
4.869084743604942e+02, 0, 0,
|
||
|
|
0,4.859921620798602e+02, 0,
|
||
|
|
2.813183643903652e+02, 2.267657091677993e+02, 1);
|
||
|
|
Mat cam_matrix_right = (Mat_<double>(3, 3) <<
|
||
|
|
4.859133331805883e+02, 0, 0,
|
||
|
|
0,4.850356748771951e+02, 0,
|
||
|
|
2.970046483040089e+02, 2.324763397214774e+02, 1);
|
||
|
|
Mat distortion_l = (Mat_<double>(1, 5) <<0.121235284781974,-0.161097849662596, 0,
|
||
|
|
0, 0);
|
||
|
|
|
||
|
|
Mat distortion_r = (Mat_<double>(1, 5) <<0.105479235005840,-0.120347246815955, 0,
|
||
|
|
0, 0);
|
||
|
|
Mat rotate = (Mat_<double>(3, 3) <<
|
||
|
|
0.999921984818601, 3.753847738839353e-04, -0.012485325894835,
|
||
|
|
-4.085449515452996e-04, 0.999996396040715, -0.002653487630467,
|
||
|
|
0.012484284819374, 0.002658381435011,0.999918534502034);
|
||
|
|
Mat trans = (Mat_<double>(3, 1) <<
|
||
|
|
-60.319997608188590, -0.019664800882533, -0.638993708428792);
|
||
|
|
*/
|
||
|
|
// 3_stereo:
|
||
|
|
Mat cam_matrix_left = (Mat_<double>(3, 3) <<
|
||
|
|
725.8008655, 0, 606.83544339,
|
||
|
|
0, 725.71074058, 355.60009442,
|
||
|
|
0, 0, 1);
|
||
|
|
Mat cam_matrix_right = (Mat_<double>(3, 3) <<
|
||
|
|
720.47514477, 0, 611.91679085,
|
||
|
|
0, 720.93965216, 328.55158146,
|
||
|
|
0, 0, 1);
|
||
|
|
Mat distortion_l = (Mat_<double>(1, 5) <<
|
||
|
|
0.0794014, -0.07158048, -0.00233537, -0.00538731, -0.07631366);
|
||
|
|
|
||
|
|
Mat distortion_r = (Mat_<double>(1, 5) <<
|
||
|
|
0.10602397, -0.12121708, -0.0039814, -0.00316623, -0.06940564);
|
||
|
|
Mat rotate = (Mat_<double>(3, 3) <<
|
||
|
|
9.99952249e-01, -3.99415019e-04, -9.76426160e-03,
|
||
|
|
4.05975519e-04, 9.99999693e-01, 6.69916194e-04,
|
||
|
|
9.76399103e-03, -6.73848256e-04, 9.99952104e-01);
|
||
|
|
Mat trans = (Mat_<double>(3, 1) <<
|
||
|
|
-45.23755445, -0.59995078, -3.05666663);
|
||
|
|
cv::Mat mapX1, mapX2, mapY1, mapY2, q, Z;
|
||
|
|
|
||
|
|
int imgw, imgh;
|
||
|
|
detect_result_group_t detect_result_group;
|
||
|
|
cv::Mat info;
|
||
|
|
cv::Mat empty;
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
public:
|
||
|
|
Ranging(){};
|
||
|
|
Ranging(int imgw, int imgh);
|
||
|
|
void rectifyImage(cv::Mat &oriImgL, cv::Mat &oriImgR);
|
||
|
|
void getInfo(cv::Mat &imgl, cv::Mat &imgr, cv::Mat &detBoxes, cv::Mat &info);
|
||
|
|
std::vector<float> pic2cam(int u, int v);
|
||
|
|
std::vector<int> muban(cv::Mat &left_image, cv::Mat &right_image, const int *coordinates);
|
||
|
|
std::vector<cv::Mat> get_range(int frame_num, int detect_interval);
|
||
|
|
void horizon_estimate(cv::Mat& img, cv::Mat& bboxs,int k);
|
||
|
|
|
||
|
|
};
|
||
|
|
|