├── 1-90.xlsx ├── README.md ├── Trajectory Illustration ├── Lane 1.fig ├── Lane 2.fig ├── Lane 3.fig ├── Ramp.fig ├── plottraj.m └── traj data.mat ├── data_description.docx ├── data_description2023.docx ├── step1_image_matching └── Project1 │ ├── Angle_test.cpp │ ├── Calibration.cpp │ ├── D2lanenum.m │ ├── FRAMES.h │ ├── Featurecheck - Copy.cpp │ ├── Featurecheck.cpp │ ├── LaneNum.cpp │ ├── LaneNumFunc.cpp │ ├── Load_circle.cpp │ ├── Load_circle_parameters.cpp │ ├── Main.cpp │ ├── Manualcheck.cpp │ ├── Match_circle.cpp │ ├── NOTE.cpp │ ├── OperateOneFrame.cpp │ ├── POINTS.cpp │ ├── Plot.cpp │ ├── extractBackground.cpp │ ├── f2r.py │ ├── featuretrack.cpp │ ├── file_operation_and_inits.cpp │ ├── first.csv │ ├── first2.csv │ ├── get_motion_parameters.cpp │ ├── lanenum.py │ ├── lanenumber1.jpg │ ├── lanenumberedtra1.png │ ├── lanenumberedtraroundabouts.jpg │ ├── load_and_store_2.cpp │ ├── load_block.cpp │ └── openh264-1.7.0-win64.dll ├── step2_training_img_generation └── Project1 │ ├── Angle_test.cpp │ ├── Calibration.cpp │ ├── D2lanenum.m │ ├── FRAMES.h │ ├── Featurecheck - Copy.cpp │ ├── Featurecheck.cpp │ ├── Get_learning_imgs.cpp │ ├── Lanefunctions.cpp │ ├── Lanes.h │ ├── Load_circle.cpp │ ├── Load_circle_parameters.cpp │ ├── Load_cut_edge.cpp │ ├── Main.cpp │ ├── Manualcheck.cpp │ ├── Match_circle.cpp │ ├── NOTE.cpp │ ├── OperateOneFrame.cpp │ ├── POINTS.cpp │ ├── Plot.cpp │ ├── Poly.h │ ├── extractBackground.cpp │ ├── f2r.py │ ├── featuretrack.cpp │ ├── file_operation_and_inits.cpp │ ├── first.csv │ ├── first2.csv │ ├── frame_load.cpp │ ├── generate_learning_data.cpp │ ├── get_motion_parameters.cpp │ ├── lanenum.py │ ├── lanenumber1.jpg │ ├── lanenumberedtra1.png │ ├── lanenumberedtraroundabouts.jpg │ ├── load_and_store_2.cpp │ ├── load_block.cpp │ ├── manually_add_train_data.cpp │ └── openh264-1.7.0-win64.dll ├── step3_lanedetection └── Project1 │ ├── Angle_test.cpp │ ├── Calibration.cpp │ ├── D2lanenum.m │ ├── FRAMES.h │ ├── Featurecheck - Copy.cpp │ ├── Featurecheck.cpp │ ├── Get_learning_imgs.cpp │ ├── Lanefunctions.cpp │ ├── Lanes.h │ ├── Load_circle.cpp │ ├── Load_circle_parameters.cpp │ ├── Load_cut_edge.cpp │ ├── Main.cpp │ ├── Manualcheck.cpp │ ├── Match_circle.cpp │ ├── NOTE.cpp │ ├── OperateOneFrame.cpp │ ├── POINTS.cpp │ ├── Plot.cpp │ ├── Poly.h │ ├── extractBackground.cpp │ ├── f2r.py │ ├── featuretrack.cpp │ ├── file_operation_and_inits.cpp │ ├── first.csv │ ├── first2.csv │ ├── frame_load.cpp │ ├── generate_learning_data.cpp │ ├── get_mergedmarker.cpp │ ├── get_mergedmarker_loc_load.cpp │ ├── get_motion_parameters.cpp │ ├── lanenum.py │ ├── lanenumber1.jpg │ ├── lanenumberedtra1.png │ ├── lanenumberedtraroundabouts.jpg │ ├── load_and_store_2.cpp │ ├── load_block.cpp │ ├── load_cir_edge.cpp │ ├── manually_add_train_data.cpp │ ├── marker_detection.cpp │ ├── marker_functions.cpp │ └── openh264-1.7.0-win64.dll └── step4_connection └── Project1 ├── Angle_test.cpp ├── Calibration.cpp ├── D2lanenum.m ├── FRAMES.h ├── Featurecheck - Copy.cpp ├── Featurecheck.cpp ├── Get_learning_imgs.cpp ├── Lanefunctions.cpp ├── Lanes.h ├── Load_circle.cpp ├── Load_circle_parameters.cpp ├── Load_cut_edge.cpp ├── Main.cpp ├── Manualcheck.cpp ├── Match_circle.cpp ├── NOTE.cpp ├── OperateOneFrame.cpp ├── POINTS.cpp ├── Plot.cpp ├── Poly.h ├── extractBackground.cpp ├── f2r.py ├── featuretrack.cpp ├── file_operation_and_inits.cpp ├── first.csv ├── first2.csv ├── frame_load.cpp ├── generate_learning_data.cpp ├── get_mergedmarker.cpp ├── get_mergedmarker_loc_load.cpp ├── get_motion_parameters.cpp ├── lanenum.py ├── lanenumber1.jpg ├── lanenumberedtra1.png ├── lanenumberedtraroundabouts.jpg ├── load_and_store_2.cpp ├── load_block.cpp ├── load_cir_edge.cpp ├── manually_add_train_data.cpp ├── marker_detection.cpp ├── marker_functions.cpp └── openh264-1.7.0-win64.dll /1-90.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATS-Lab/Filed-Experiment-Data-HIGH_Sim/b5f9845bd61591fdcd3e9101a22e458ca3afae18/1-90.xlsx -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Filed Experiment Data-HIGH Sim dataset 2 | The datasets were extracted from aerial videos recorded at several successive spaces of Interstate-75, United States. 3 | 4 | This work is published under the [CC BY-SA 4.0](https://creativecommons.org/licenses/by-nc/4.0/legalcode) licesne. 5 | CC BY-SA 4.0 6 | 7 | Due to the limited upload size for each file of GitHub, we uploaded the first 90 trajectories for illustration purpose. The whole dataset is available at https://drive.google.com/drive/folders/1HfMokgtkqi44t_khkdAB7QjnBqh_J7mx?usp=sharing (about 1GB). 8 | 9 | Some MATLAB codes that may help you to process the dataset can be found in the trajectory illustration package. Also, several plots of the first 90 trajectories can be found there. If you feel the dataset helps your research, please cite our two research papers related to this dataset as follows. 10 | 11 | Data for reserach 12 | Shi, X., Zhao, D., and Li, X., 2021. A Car Following-Based Method for Vehicle Trajectory Dataset Post-Processing. (Under review). Preprint: https://doi.org/10.13140/RG.2.2.28609.28001/1 13 | 14 | Shi, X., Zhao, D., Yao, H. and Li, X., 2021. Video-based Trajectory Generation for High-Granularity Highway Simulation (HIGH-Sim). https://doi.org/10.1016/j.commtr.2021.100014 15 | 16 | Please contact xiaopeng.li@wisc.edu if you have any questions. 17 | -------------------------------------------------------------------------------- /Trajectory Illustration/Lane 1.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATS-Lab/Filed-Experiment-Data-HIGH_Sim/b5f9845bd61591fdcd3e9101a22e458ca3afae18/Trajectory Illustration/Lane 1.fig -------------------------------------------------------------------------------- /Trajectory Illustration/Lane 2.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATS-Lab/Filed-Experiment-Data-HIGH_Sim/b5f9845bd61591fdcd3e9101a22e458ca3afae18/Trajectory Illustration/Lane 2.fig -------------------------------------------------------------------------------- /Trajectory Illustration/Lane 3.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATS-Lab/Filed-Experiment-Data-HIGH_Sim/b5f9845bd61591fdcd3e9101a22e458ca3afae18/Trajectory Illustration/Lane 3.fig -------------------------------------------------------------------------------- /Trajectory Illustration/Ramp.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATS-Lab/Filed-Experiment-Data-HIGH_Sim/b5f9845bd61591fdcd3e9101a22e458ca3afae18/Trajectory Illustration/Ramp.fig -------------------------------------------------------------------------------- /Trajectory Illustration/plottraj.m: -------------------------------------------------------------------------------- 1 | %seperate traj 2 | 3 | % traj{1,max(data(:,1))}={}; 4 | % count=1; 5 | % for i=1:length(data)-1 6 | % if data(i,1)==count 7 | % traj{1,count}(end+1,:)=data(i,:); 8 | % end 9 | % if data(i+1,1)==count+1 10 | % count=count+1; 11 | % end 12 | % end 13 | 14 | %% 15 | count=1; 16 | record=[1]; 17 | for i=1:length(traj) 18 | inil=traj{1,i}(1,13); 19 | for k=1:length(traj{1,i}) 20 | curl=traj{1,i}(k,13); 21 | if curl~=inil 22 | record=[record,k]; 23 | inil=curl; 24 | end 25 | end 26 | if length(record)==1 27 | figure(inil+2);hold all 28 | plot(traj{1,i}(:,3),traj{1,i}(:,5)); 29 | text(traj{1,i}(record(1),3),traj{1,i}(record(1),5),num2str(i)); 30 | else 31 | record=[record,length(traj{1,i})+1]; 32 | for j=1:length(record)-1 33 | if traj{1,i}(record(1,j),13)==-1 34 | figure(traj{1,i}(record(j),13)+2);hold all 35 | plot(traj{1,i}(record(j):record(j+1)-1,3),traj{1,i}(record(j):record(j+1)-1,5)); 36 | text(traj{1,i}(record(j),3),traj{1,i}(record(j),5),num2str(i)); 37 | end 38 | if traj{1,i}(record(1,j),13)==0 39 | figure(traj{1,i}(record(j),13)+2);hold all 40 | plot(traj{1,i}(record(j):record(j+1)-1,3),traj{1,i}(record(j):record(j+1)-1,5)); 41 | text(traj{1,i}(record(j),3),traj{1,i}(record(j),5),num2str(i)); 42 | end 43 | if traj{1,i}(record(1,j),13)==1 44 | figure(traj{1,i}(record(j),13)+2);hold all 45 | plot(traj{1,i}(record(j):record(j+1)-1,3),traj{1,i}(record(j):record(j+1)-1,5)); 46 | text(traj{1,i}(record(j),3),traj{1,i}(record(j),5),num2str(i)); 47 | end 48 | if traj{1,i}(record(1,j),13)==2 49 | figure(traj{1,i}(record(j),13)+2);hold all 50 | plot(traj{1,i}(record(j):record(j+1)-1,3),traj{1,i}(record(j):record(j+1)-1,5)); 51 | text(traj{1,i}(record(j),3),traj{1,i}(record(j),5),num2str(i)); 52 | end 53 | end 54 | end 55 | record=[1]; 56 | end 57 | 58 | figure(1); 59 | title('Ramp'); 60 | figure(2); 61 | title('Lane 1'); 62 | figure(3); 63 | title('Lane 2'); 64 | figure(4); 65 | title('Lane 3'); -------------------------------------------------------------------------------- /Trajectory Illustration/traj data.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATS-Lab/Filed-Experiment-Data-HIGH_Sim/b5f9845bd61591fdcd3e9101a22e458ca3afae18/Trajectory Illustration/traj data.mat -------------------------------------------------------------------------------- /data_description.docx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATS-Lab/Filed-Experiment-Data-HIGH_Sim/b5f9845bd61591fdcd3e9101a22e458ca3afae18/data_description.docx -------------------------------------------------------------------------------- /data_description2023.docx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATS-Lab/Filed-Experiment-Data-HIGH_Sim/b5f9845bd61591fdcd3e9101a22e458ca3afae18/data_description2023.docx -------------------------------------------------------------------------------- /step1_image_matching/Project1/Angle_test.cpp: -------------------------------------------------------------------------------- 1 | #include "FRAMES.h" 2 | 3 | void FRAMES::test_angle(string path) { 4 | vector center; 5 | vector angle; 6 | vector fnum; 7 | load_angle_parameters(gen_path + path, fnum, center, angle); 8 | Mat frame, frame_shifted, frame_rotated; 9 | Point c0 = center[0]; 10 | for (int i = 0; i < center.size(); i+=1000) { 11 | load_frame(fnum[i], frame); 12 | int shiftx = center[i].x - c0.x; 13 | int shifty = center[i].y - c0.y; 14 | //Mat rotate_M = getRotationMatrix2D(c0, (angle[i] - PI / 2)*180.0/PI, 1); 15 | //Mat shift_M = (Mat_(2, 3) << 1, 0, -shiftx, 0, 1, -shifty); 16 | //cv::warpAffine(frame, frame_shifted, shift_M, frame.size()); 17 | Mat rotate_M = getRotationMatrix2D(center[i], (angle[i] - PI / 2)*180.0 / PI, 1); 18 | //for left 1st video, shift 8190-5600=2500; 19 | Mat shift_M = (Mat_(2, 3) << 0, 0, -shiftx, 0, 0, -shifty+2500); 20 | rotate_M = rotate_M + shift_M; 21 | cv::warpAffine(frame, frame_rotated, rotate_M, frame.size()); 22 | string img_path = "N:/LEFT/mid_processing/rotated/" + to_string(fnum[i]) + ".jpg"; 23 | cv::imwrite(img_path, frame_rotated); 24 | 25 | } 26 | 27 | } 28 | void FRAMES::load_angle_parameters(string path, vector &fnum, vector ¢er, vector &angle) { 29 | ifstream ifstr(path.c_str()); 30 | string snum; 31 | while (getline(ifstr, snum, ',')) { 32 | fnum.push_back(stoi(snum)); 33 | int i = 0, x = 0, y = 0; 34 | string xs, ys, as; 35 | float a; 36 | getline(ifstr, xs, ','); 37 | x = stoi(xs); 38 | getline(ifstr, ys, ','); 39 | y = stoi(ys); 40 | getline(ifstr, as); 41 | a = stof(as); 42 | center.push_back(Point(x, y)); 43 | angle.push_back(a); 44 | } 45 | } -------------------------------------------------------------------------------- /step1_image_matching/Project1/Calibration.cpp: -------------------------------------------------------------------------------- 1 | #include "myVideo.h" 2 | #include "Veh.h" 3 | void myVideo::cali(Mat frame) { 4 | string file = parapaths + "/cali.txt"; 5 | Mat frame_rted; 6 | if (exists_file(file)) { 7 | ifstream input(file); 8 | string number; 9 | getline(input, number); 10 | AL = atof(number.c_str()); 11 | Mat frame_rted; 12 | Calibration(frame, frame_rted); 13 | //Showframe("video", frame_rted, 5000); 14 | } 15 | else { 16 | namedWindow("video", WINDOW_NORMAL | WINDOW_KEEPRATIO); 17 | cv::moveWindow("video", WINDOWSXP, 0); 18 | cv::resizeWindow("video", 800, 1800); 19 | 20 | AL = 0; 21 | while (1) { 22 | //Showframe("video", frame); 23 | Calibration(frame, frame_rted); 24 | for (int i = 50; i < frame_rted.cols - 100; i += 100) 25 | cv::line(frame_rted, Point(i, 20), Point(i, frame_rted.rows - 20), Scalar(0, 0, 200), 3); 26 | cout << "click on t to set the angle\n"; 27 | Showframe("video", frame_rted, 400, 0, 1800,800); 28 | char c = (char)waitKey(3000);// 29 | switch (c) { 30 | case 'z': 31 | AL -= 5 * PI / 180; 32 | break; 33 | case 'c': 34 | AL += 5 * PI / 180; 35 | break; 36 | case 'q': 37 | AL -= 2 * PI / 180; 38 | break; 39 | case 'e': 40 | AL += 2 * PI / 180; 41 | break; 42 | case 'a': 43 | AL -= 0.5 * PI / 180; 44 | break; 45 | case 'd': 46 | AL += 0.5 * PI / 180; 47 | break; 48 | } 49 | 50 | if (c == 't') 51 | break; 52 | 53 | } 54 | std::ofstream outfile(file); 55 | outfile << to_string(AL) << std::endl; 56 | outfile.close(); 57 | } 58 | Calibration(frame, frame_rted); 59 | background = frame_rted.clone(); 60 | destroyWindow("video"); 61 | //Showframe("video", edgebackground); 62 | //destroyAllWindows(); 63 | } 64 | 65 | void myVideo::Calibration(Mat frame, Mat &dst) { 66 | getangle(); 67 | 68 | float Zy = (FF*cos(AL) + HS / HI * sin(AL) * (HI - HI / 2)); 69 | float yrmax = HS / HI * (HI / 2) * HH / cos(AL) / Zy; 70 | Zy = (FF*cos(AL) - HS / HI * sin(AL) * (HI / 2)); 71 | float yrmin = -HS / HI * (HI / 2.0) * HH / cos(AL) / Zy; 72 | float Z = -yrmin * sin(AL) + HH / cos(AL); 73 | float xrmin = -Z / FF * WS / WI * WI / 2; 74 | float xrmax = Z / FF * WS / WI * WI / 2; 75 | float xrr = xrmax - xrmin + 1; 76 | float yrr = yrmax - yrmin + 1; 77 | float yr, xr, yri, xri; 78 | int x_, y_; 79 | 80 | cv::Mat_ mapx_32f((int)HI, (int)WI), mapy_32f((int)HI, (int)WI); 81 | 82 | //Mat dst2((int)HI,(int)WI, CV_8UC3, Scalar(0, 0, 0)); 83 | for (int y = 0; y pt(3, 1); 90 | pt(0) = x; 91 | pt(1) = y; 92 | pt(2) = 1; 93 | 94 | yri = y; 95 | xri = x; 96 | yr = yrr / HI * yri + yrmin; 97 | xr = xrr / WI * xri + xrmin; 98 | Zy = (FF*cos(AL) * yr / (-yr*sin(AL) + HH / cos(AL))); 99 | 100 | Z = -yr*sin(AL) + HH / cos(AL); 101 | 102 | pt(1) = y_ = HI / 2 + HI / HS * Zy; 103 | pt(0) = x_ = FF / Z * WI / WS * (xr)+WI / 2; 104 | 105 | pt /= pt(2); 106 | buff_mapx[x] = (int)pt(0); 107 | buff_mapy[x] = (int)pt(1); 108 | //Vec3b color = frame_org.at(Point(x, y)); 109 | //dst2.at(Point(x_, y_)) = color; 110 | } 111 | } 112 | cv::convertMaps(mapx_32f, mapy_32f, map1_16u, map2_16u, CV_32FC2, true); 113 | //frame_r = dst2.clone(); 114 | cv::remap(frame, dst, map1_16u, map2_16u, CV_INTER_LINEAR, BORDER_CONSTANT, Scalar(0, 0, 0)); 115 | } 116 | 117 | void myVideo::Calibrateframe(Mat fgmask_rot, Mat &fgmask_cali) { 118 | cv::remap(fgmask_rot, fgmask_cali, map1_16u, map2_16u, CV_INTER_LINEAR, BORDER_CONSTANT, Scalar(0, 0, 0)); 119 | } 120 | 121 | void myVideo::Calibrateframe(GpuMat fgmask, GpuMat &fgmask_cali) { 122 | Mat m1, m2; 123 | fgmask.download(m1); 124 | cv::remap(m1, m2, map1_16u, map2_16u, CV_INTER_LINEAR, BORDER_CONSTANT, Scalar(0, 0, 0)); 125 | 126 | fgmask_cali.upload(m2); 127 | } 128 | 129 | void myVideo::getangle() { 130 | float H, al, a, pi, x, b, bl, abl, r, focal, hs, ws; 131 | H = 400.0; 132 | pi = 3.14159; 133 | FF = focal = 8.8 / 1000.0*0.3048; 134 | HH = H; 135 | WS = 13.2*0.3048 / 1000.0; 136 | HS = 7.425*0.3048 / 1000.0; 137 | } -------------------------------------------------------------------------------- /step1_image_matching/Project1/D2lanenum.m: -------------------------------------------------------------------------------- 1 | color = rand(30,3) -------------------------------------------------------------------------------- /step1_image_matching/Project1/FRAMES.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | #include "Public_functions.h" 48 | 49 | using namespace cv; 50 | using namespace std; 51 | using namespace cuda; 52 | using namespace cv::xfeatures2d; 53 | #define PI 3.14159265 54 | 55 | class FRAMES 56 | { 57 | public: 58 | //ori mat and circle points updated every 1000 frames; 59 | vector circle_mat; 60 | vector circle_mat_depth; 61 | vector circle_point; 62 | vector circle_rad; 63 | 64 | //instore all circle points 65 | vector> vector_c_p; 66 | vector> vector_c_r;//radius of circles, may useless; 67 | //framenum; 68 | vector vfnum; 69 | string gen_path; 70 | string frame_path; 71 | string output_data_path; 72 | string temp_data_path; 73 | string temp_frame_path; 74 | 75 | float thres_cir = 0.8; 76 | 77 | Public_func PF; 78 | 79 | const int IMGW = 4320, IMGH = 8192; 80 | 81 | public: 82 | FRAMES() {}; 83 | FRAMES(string g_path); 84 | void get_motion_parameters(int startframenumber, int freq); 85 | void test_circles(string path); 86 | void test_angle(string path); 87 | 88 | private: 89 | 90 | bool load_frame(int fnum, Mat &cur_frame); 91 | bool load_depth(int fnum, Mat &cur_depth); 92 | void save_results(Mat cur_frame, int, int, int); 93 | void load_circle(Mat cur_frame, Mat cur_depth, int); 94 | bool match_circles(Mat cur_frame); 95 | void push_points(); 96 | bool temp_match(Mat mat_temp, Mat mat_temp_depth, Mat mat_sour, Point2i &result); 97 | void filte_img(Mat &cur_frame); 98 | void show_frame(Mat frame, int duration = 1000, string name = "frame", int startx = 10, int starty = 10); 99 | void show_frame(Mat frame, Point2i p, int rad, int duration = 1000, string name = "frame", int startx = 10, int starty = 10); 100 | void show_frame(Mat frame, Rect rect, int duration = 1000, string name = "frame", int startx = 10, int starty = 10); 101 | bool color_match(Mat mat_temp, Mat mat_sour, Point2i result); 102 | //test circle matching results; 103 | void load_circle_parameter(string path); 104 | void load_angle_parameters(string path, vector &fnum, vector ¢er, vector &angle); 105 | }; 106 | 107 | -------------------------------------------------------------------------------- /step1_image_matching/Project1/LaneNum.cpp: -------------------------------------------------------------------------------- 1 | //**************** 2 | #include "myVideo.h" 3 | #include "Veh.h" 4 | 5 | void LaneNum(vector > V) { 6 | TNetwork tnet; 7 | tnet.img = imread("back.jpg", CV_LOAD_IMAGE_COLOR); 8 | for (auto trajectory : V) { 9 | if (tnet.lanes.empty()) { 10 | tnet.createlane (trajectory); 11 | } 12 | else { 13 | tnet.alter.sign = 1; 14 | //cout << trajectory[0]->num<<"numsign"<lanenum << endl; 25 | } 26 | tnet.del7(); 27 | tnet.fillmat(V); 28 | int save = 1; 29 | tnet.findlane(V); 30 | tnet.show(1,0,save); 31 | 32 | } 33 | 34 | 35 | double Vehdis(Veh* a, Veh* b) { 36 | //veh b is the obj that already in the stack; 37 | //speed limit limit; 38 | double spdlim = 2000;//ft/s 39 | double dis = 50; 40 | if (b->state) { 41 | dis = sqrt(pow((*a).cx - b->forcast[0], 2) + pow((*a).cy - b->forcast[1], 2)); 42 | } 43 | else { 44 | dis = sqrt(pow((*a).cx - (*b).cx, 2) + pow((*a).cy - (*b).cy, 2)); 45 | } 46 | // ////////////////////////////////////////////////////////////////////////////////////////////////////// 47 | double dt; 48 | dt = static_cast(a->vtype) / b->vtype; 49 | if (dis > 50.0 || dt > 2.0 || dt < 0.5) 50 | return 1000; 51 | else if (dt < 1.05 && dt > 0.95 && dis < 1) 52 | return -1; 53 | else 54 | return dis; 55 | } 56 | 57 | void Constructveh(vector > &Fig) { 58 | std::string fil = "output.csv"; 59 | int n, ty, x, y, gtime, w, h; 60 | std::string sn, sty, sx, sy, sgtime, slnum; 61 | ifstream mycsv(fil); 62 | int pvn = 0; 63 | vector onecar; 64 | Rect kiss(XPIXEL*0.0, 0, XPIXEL*0.99, YPIXEL*0.35); 65 | while (1) { 66 | if (!getline(mycsv, sn, ',')) 67 | break; 68 | n = stoi(sn); 69 | getline(mycsv, sgtime, ','); 70 | gtime = stof(sgtime); 71 | getline(mycsv, sx, ','); 72 | x = stof(sx); 73 | getline(mycsv, sy, ','); 74 | y = stof(sy); 75 | getline(mycsv, slnum, ','); 76 | getline(mycsv, sty); 77 | ty = stoi(sty); 78 | if (kiss.contains(Point(x, y))) 79 | continue; 80 | int w = 0, h = 0; 81 | Veh* v = new Veh(n, double(ty),RotatedRect(Point2f(x,y), Size2f(w,h), 0), n, 0); 82 | if (pvn != n) { 83 | Fig.push_back(onecar); 84 | onecar.clear(); 85 | onecar.push_back(v); 86 | pvn = n; 87 | } 88 | else { 89 | if(onecar.size() > 0) 90 | onecar.back()->nv = v; 91 | onecar.push_back(v); 92 | } 93 | } 94 | Rect center(XPIXEL*0.01, YPIXEL*0.5, XPIXEL*0.98, YPIXEL*0.55); 95 | for (int i = 0; i < Fig.size(); i++) { 96 | if(Fig[i].size() < 150) 97 | Fig.erase(Fig.begin() + i--); 98 | else { 99 | if (center.contains(Point2f(Fig[i][0]->cx, Fig[i][0]->cy)) || center.contains(Point2f(Fig[i].back()->cx, Fig[i].back()->cy))) { 100 | Fig.erase(Fig.begin() + i); 101 | i--; 102 | } 103 | } 104 | } 105 | cout << Fig.size(); 106 | } -------------------------------------------------------------------------------- /step1_image_matching/Project1/Load_circle_parameters.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "FRAMES.h" 3 | 4 | void FRAMES::load_circle_parameter(string path) { 5 | ifstream ifstr(path.c_str()); 6 | string snum; 7 | while (getline(ifstr, snum, ',')) { 8 | int i = 0, x = 0, y = 0; 9 | vfnum.push_back(stoi(snum)); 10 | vector points; 11 | vector rad; 12 | while (i < 9) { 13 | getline(ifstr, snum, ','); 14 | switch (i++ % 3) { 15 | case 0: 16 | x = stoi(snum); 17 | break; 18 | case 1: 19 | y = stoi(snum); 20 | points.push_back(Point2i(x, y)); 21 | break; 22 | case 2: 23 | rad.push_back(stoi(snum)); 24 | break; 25 | } 26 | } 27 | getline(ifstr, snum); 28 | vector_c_p.push_back(points); 29 | vector_c_r.push_back(rad); 30 | } 31 | } -------------------------------------------------------------------------------- /step1_image_matching/Project1/Main.cpp: -------------------------------------------------------------------------------- 1 | #include "FRAMES.h" 2 | #include 3 | 4 | int main() 5 | { 6 | string gen_path = "L:/MIDDLE/"; 7 | FRAMES FRAMES(gen_path); 8 | //TODO: 9 | 10 | //1. 11 | FRAMES.get_motion_parameters(10000, 5); 12 | //2. 13 | //string path = "verify/to_show.csv"; 14 | //FRAMES.test_circles(path); 15 | //3. 16 | //string path[2] = { "mid_processing/para_1_11400.csv", "mid_processing/para_138000_216000.csv" }; 17 | //FRAMES.test_angle(path[1]); 18 | //FRAMES.test_angle(path[0]); 19 | return 0; 20 | } 21 | -------------------------------------------------------------------------------- /step1_image_matching/Project1/NOTE.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Documentation: 3 | 0. preprocessing 4 | a. revise rotation algorithm, make it work in 2s frames; 5 | b. revise calibration part... 6 | c. cut the processed image into edge, (save it, optional); 7 | 8 | 1. revise the identification part; 9 | a. resize the image to imagesize*1.2; 10 | b. cut image into 5 pieces and detect them; 11 | c. process results with Rect parameters, coordinate/1.2, and return results; 12 | 13 | 2. tet performance. 14 | 15 | */ 16 | -------------------------------------------------------------------------------- /step1_image_matching/Project1/OperateOneFrame.cpp: -------------------------------------------------------------------------------- 1 | #include "myVideo.h" 2 | #include "Veh.h" 3 | 4 | void myVideo::OperateOneFrame(Mat fgmask_rot, vector> &Fig, int count, int &n, int cnum) { 5 | vector> contours; 6 | vector hierarchy; 7 | findContours(fgmask_rot, contours, hierarchy, RETR_CCOMP, CHAIN_APPROX_SIMPLE); 8 | vector Fig0; 9 | int hi; 10 | for (int i = 0; i < int(contours.size()); i++) { 11 | if (hierarchy[i][3] < 0) { 12 | if (contours[i].size() < 10) 13 | continue; 14 | float ty = static_cast(contourArea(cv::Mat(contours[i]))); 15 | Moments mu = moments(contours[i], false); 16 | Point2f mc = Point2f(static_cast(mu.m10 / mu.m00), static_cast(mu.m01 / mu.m00)); 17 | mc = Point2f(max(0.0f, mc.x), max(0.0f, mc.y)); 18 | Rect rect = boundingRect(contours[i]); 19 | RotatedRect rt = minAreaRect(contours[i]); 20 | float angle = rt.angle; 21 | Veh* v = new Veh(n++, ty, rt, cnum, count, angle); 22 | Point2f pf[4]; 23 | rt.points(pf); 24 | for (int k = 0; k < 4; k++) { 25 | v->ep[2 * k] = pf[k].x; 26 | v->ep[2 * k + 1] = pf[k].y; 27 | if (cv::norm(pf[k] - pf[(k + 1) % 3]) > 1500) 28 | ty = 10; 29 | } 30 | if (ty < MINCONAREA || ty > MAXCONAREA) 31 | continue; 32 | //if (count > 160) 33 | //rectangle(fgmask_rot, rect, Scalar(255), 10, 8, 0); 34 | Fig0.push_back(v); 35 | } 36 | } 37 | if (count == 0) { 38 | Fig.push_back(Fig0); 39 | } 40 | else { 41 | vector Fb = Fig.back(); 42 | double disu = 50.0; 43 | double d1, d2; 44 | int k; 45 | for (int i = 0; i < int(Fig0.size()); i++) { 46 | d2 = disu; 47 | for (int j = 0; j < int(Fb.size()); j++) { 48 | d1 = Vehdis(Fig0[i], Fb[j]); 49 | if (d1 < d2) { 50 | d2 = d1; 51 | k = j; 52 | } 53 | } 54 | int newv = 1; 55 | if (d2 < disu) { 56 | if (Fb[k]->nv) { 57 | float a1, a2, a3; 58 | a1 = Fb[k]->vtype; 59 | a2 = Fb[k]->nv->vtype; 60 | a3 = Fig0[i]->vtype; 61 | float area1, area2; 62 | area1 = (a1 - a2) * (a1 - a2); 63 | area2 = (a1 - a3) * (a1 - a3); 64 | if (area1 < area2) 65 | newv = 0; 66 | else 67 | newv = 2; 68 | } 69 | if (newv) { 70 | if (newv == 2) { 71 | Fb[k]->nv->pv = NULL; 72 | //Fb[k]->nv = NULL; 73 | } 74 | Fig0[i]->pv = Fb[k]; 75 | Fb[k]->nv = Fig0[i]; 76 | Fig0[i]->num = Fb[k]->num; 77 | Fig0[i]->vtype = (Fb[k]->vtype + Fig0[i]->vtype) / 2; 78 | Fb[k]->vtype = Fig0[i]->vtype; 79 | if (!Fig0[i]->state) { 80 | Veh* b; 81 | b = Fig0[i]; 82 | b->forcast[0] = 2 * b->cx - b->pv->cx; 83 | b->forcast[1] = 2 * b->cy - b->pv->cy; 84 | b->state = 1; 85 | } 86 | } 87 | } 88 | } 89 | Fig.push_back(Fig0); 90 | Fig0.clear(); 91 | } 92 | } 93 | 94 | void myVideo::RefreshOutput(vector> &Fig) { 95 | for (int i = 0; i < Fig.size(); i ++) { 96 | for (int j = 0; j < Fig[i].size(); j++) { 97 | Veh* v; 98 | v = Fig[i][j]; 99 | if (v->time == 0) { 100 | int ti = 1; 101 | while (v->nv) { 102 | v = v->nv; 103 | ti++; 104 | } 105 | while (v) { 106 | v->time = ti; 107 | v = v->pv; 108 | } 109 | } 110 | } 111 | } 112 | for (auto &i : Fig) { 113 | for (int j = 0; j < int(i.size()); j++) { 114 | if (i[j]->nv) 115 | if (i[j]->time != i[j]->nv->time) 116 | cout << "wrong"; 117 | else if (i[j]->nv->pv == NULL) 118 | cout << "nopv"; 119 | else if (i[j]->nv->num != i[j]->num) 120 | cout << "number not equal"; 121 | if (i[j]->time < MINFRAMELEN) 122 | i.erase(i.begin() + j--); 123 | } 124 | } 125 | } -------------------------------------------------------------------------------- /step1_image_matching/Project1/POINTS.cpp: -------------------------------------------------------------------------------- 1 | #include "myVideo.h" 2 | #include "Veh.h" 3 | 4 | void myVideo::Points(vector > V) { 5 | Mat img(YPIXEL, XPIXEL, CV_8UC3, Scalar(255, 255, 255)); 6 | int num = 0; 7 | std::ofstream mycsv; 8 | mycsv.open("output.csv"); 9 | for (auto &i : V) { 10 | for (auto &j : i) { 11 | mycsv << j->num << "," << j->glotime << "," << j->cx << "," << j->cy << "," << j->lnum << "," << j->vtype << ",\n"; 12 | num++; 13 | //if (num)// % 2 == 0) 14 | circle(img, cv::Point2f(j->cx, j->cy), 2, Scalar(0, 0, 0), 2, 8, 0); 15 | circle(background, cv::Point2f(j->cx, j->cy), 2, Scalar(0, 0, 255), 2, 8, 0); 16 | } 17 | } 18 | mycsv.close(); 19 | namedWindow("Points", WINDOW_NORMAL | WINDOW_KEEPRATIO); 20 | cv::moveWindow("Points", WINDOWSXP, WINDOWSYP); 21 | cv::resizeWindow("Points", WINDOWSW, WINDOWSH); 22 | imshow("Points", img); // Show our image inside it. 23 | imwrite("a.jpg", img); 24 | imwrite("b.jpg", background); 25 | waitKey(3000); 26 | cv::destroyAllWindows(); 27 | } 28 | -------------------------------------------------------------------------------- /step1_image_matching/Project1/Plot.cpp: -------------------------------------------------------------------------------- 1 | #include "myVideo.h" 2 | #include "Veh.h" 3 | #include 4 | 5 | void Plot(vector > V){ 6 | ofstream out("zline.xls"); 7 | if (!out) { 8 | cout << "open lxs unsuccessful"; 9 | throw; 10 | } 11 | for (auto &i : V){ 12 | for (auto &j : i){ 13 | out << j->num << " \t" << j->glotime << " \t" << j->x << " \t" << j->y\ 14 | << " \t" << j->spd << " \t" << j->acc << " \t" << j->lnum << endl; 15 | } 16 | } 17 | out.close(); 18 | 19 | } 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /step1_image_matching/Project1/extractBackground.cpp: -------------------------------------------------------------------------------- 1 | #include "myVideo.h" 2 | #include "Veh.h" 3 | 4 | using namespace std::this_thread; 5 | using namespace std::chrono; 6 | 7 | vector> myVideo::extractBackground(int startnum) { 8 | string s = outputpaths; 9 | string s_del = "rm -rf "; 10 | s_del += s + "/*.jpg"; 11 | const char* todel = s_del.c_str(); 12 | cout << s_del << "\n" << todel << endl; 13 | system(todel); 14 | int n = 0, cnum = 0, count0 = 0, count = startnum; 15 | 16 | vector> Fig; 17 | Mat frame_org, fgmask, fgmask_cali_rot, fgmask_cali, frame, fgimg, bgimg, contours, frame_cali, frame_cali_rot; 18 | GpuMat d_fgmask, d_fgimg, d_bgimg, d_bgimg_rot, d_fgmask_rot, d_frameb, d_frame, d_frame_cali, d_bg_cali; 19 | Stream stream; 20 | 21 | 22 | int GPU_work = getCudaEnabledDeviceCount(); 23 | if (GPU_work) 24 | cout << "The GPU is working with OPENCV" << endl; 25 | string fname = "s"; 26 | int Win = 300, Hin = 600; 27 | namedWindow(fname, WINDOW_NORMAL | WINDOW_KEEPRATIO); 28 | cv::moveWindow(fname, WINDOWSXP, WINDOWSYP); 29 | cv::resizeWindow(fname, Win, Hin); 30 | //cv::moveWindow(fname, WINDOWSXP + mov, WINDOWSYP); 31 | 32 | 33 | 34 | while (1) { 35 | cout << count << "--"; 36 | readvehframe(frame_org, count+1); 37 | //cv::flip(frame_org, frame_org, -1); 38 | if (count == startnum) { 39 | cali(frame_org); 40 | CalculateVideoDirection(Fig); 41 | } 42 | Calibrateframe(frame_org, frame_cali); 43 | 44 | Size ms = frame_org.size(); 45 | d_frame_cali.upload(frame_cali, stream); 46 | 47 | 48 | //TODO: load area to match in rotation 49 | //FindRotationParameter(d_frame, d_frame_cali, fgmask_cali_rot, count, cnum); 50 | float rotp[3] = { 0,0,0 }; 51 | 52 | RotateFrame(d_frame_cali, d_fgmask_rot, rotp, count); 53 | d_fgmask_rot.download(frame_cali_rot); 54 | 55 | 56 | if(count%100 == 0){ 57 | imshow(fname, frame_cali_rot); 58 | waitKey(10); 59 | } 60 | saveframe(frame_cali_rot, count, count0); 61 | 62 | //OperateOneFrame(fgmask_rot_cali, Fig, count, n, cnum); 63 | 64 | 65 | if (count == OPFRAMENUM + 200) 66 | break; 67 | count++; 68 | } 69 | //writerotparas(); 70 | 71 | cv::destroyAllWindows(); 72 | return Fig; 73 | } 74 | -------------------------------------------------------------------------------- /step1_image_matching/Project1/f2r.py: -------------------------------------------------------------------------------- 1 | import xlrd 2 | import matplotlib 3 | import matplotlib.pyplot as plt 4 | import numpy as np 5 | from mpl_toolkits import mplot3d 6 | import csv 7 | 8 | cor = { 9 | 0: 'b', 10 | 1: 'k', 11 | 2: 'r', 12 | 3: 'g', 13 | 4: 'c', 14 | 5: 'sienna', 15 | 6: 'grey', 16 | 7: 'y', 17 | 8: 'violet', 18 | 9: 'coral', 19 | 10:'darkblue', 20 | 11:'darkgreen', 21 | 12:'indigo', 22 | 13:'maroon', 23 | 14:'navy', 24 | 15:'olive', 25 | 16:'orangered', 26 | 17:'sienna', 27 | 18:'violet', 28 | } 29 | font = {'family' : 'serif', 'size' : 10} 30 | matplotlib.rc('font', **font) 31 | fig = plt.figure() 32 | ax = plt.axes(projection='3d') 33 | 34 | cormap = range(19) 35 | cormap = list(cormap) 36 | initi = 0 37 | fx = [] 38 | fy = [] 39 | ft = [] 40 | fl = [] 41 | def turn(x,y): 42 | pi = 3.1415926 43 | zH = 400.0; 44 | zpi = 3.14159; 45 | zal = 52.0; 46 | za = zal*pi / 180.0; 47 | zx = np.arctan(np.tan(za) - 404.49 / 400.0); 48 | zb = za - zx; 49 | zbl = zb * 180.0 /pi; 50 | zabl = zal + zbl; 51 | zr = zH*(np.tan(zb + za) - np.tan(za - zb)); 52 | zfocal = 27.0 / 100.0 / 0.3048 / 10.0; 53 | zhs = zfocal * np.tan(zb) * 2; 54 | zws = zhs * 4096.0 / 2160.0; 55 | zWS = zws; 56 | zHS = zhs; 57 | zXS0 = zWS / 2.0; 58 | zYS0 = zHS / 2.0; 59 | zFF = zfocal; 60 | zHH = zH; 61 | zAL = za; 62 | zBE = zb; 63 | 64 | WI = 4096; HI = 2160; 65 | XS0 = zWS / 2; YS0 = zHS / 2; 66 | xp = x*zWS / WI - XS0; 67 | yp = y*zHS / HI - YS0; 68 | cs = np.cos(zAL) 69 | ss = np.sin(zAL) 70 | t = zHH / cs; fy = zFF / cs; 71 | xm = 1 72 | ym = 1 73 | qy = int(t*yp/(fy + ss * yp) * ym); 74 | qx = int(xp * (t - ss * qy) / zFF * xm); 75 | mult = 2 76 | return qx*1.5,qy*3 77 | 78 | with open('lanenumbered.csv') as csvDataFile: 79 | csvReader = csv.reader(csvDataFile) 80 | tt = [] 81 | xx = [] 82 | yy = [] 83 | ln = -1 84 | cn = -1 85 | for row in csvReader: 86 | cnum = int(row[0]) 87 | time = float(row[1]) 88 | cx = int(row[2]) 89 | cy = int(row[3]) 90 | lane = int(row[4]) 91 | if ln == -1: 92 | ln = lane 93 | cn = cnum 94 | qx,qy = turn(cx,cy) 95 | tt.append(time/25) 96 | xx.append(qx) 97 | yy.append(qy) 98 | if lane != ln or cnum != cn: 99 | fx.append(xx) 100 | fy.append(yy) 101 | ft.append(tt) 102 | fl.append(ln) 103 | tt = [] 104 | xx = [] 105 | yy = [] 106 | ln = lane 107 | cn = cnum 108 | elif cy > 2160*0.45 and cx > 4096*0.4 and (cy-2160*0.45)*0.4 < 700 and (cx-4096*0.4)*0.4 < 800: 109 | tt.append(time/25) 110 | qx,qy = turn(cx,cy) 111 | xx.append(qx) 112 | yy.append(qy) 113 | 114 | for i in range(len(fx)): 115 | bb = 0 116 | for j in range(len(fx[i]) - 1): 117 | if len(fx[i]) < 100 or fl[i] == 0: 118 | bb = 1 119 | break 120 | if bb: 121 | continue 122 | ax.plot(fx[i],fy[i],ft[i],linewidth = 2, linestyle = '-', color = cor[fl[i]],label = 'lane ' + str(fl[i]) if fl[i] in cormap else "") 123 | if fl[i] in cormap: 124 | cormap.remove(fl[i]) 125 | plt.xlim(0,800) 126 | plt.ylim(0,800) 127 | plt.legend(loc=2) 128 | plt.xlabel("X (ft)") 129 | plt.ylabel("Y (ft)") 130 | ax.set_zlabel("T (s)") 131 | #plt.xticks(np.arange(0, 600, step=200)) 132 | #plt.yticks(np.arange(0, 300, step=100)) 133 | #ax.set_zticks(np.arange(0, 200, step=50)) 134 | #plt.zlabel("Y (ft)") 135 | plt.show() 136 | plt.savefig('lanenumberedtra1.png') 137 | -------------------------------------------------------------------------------- /step1_image_matching/Project1/file_operation_and_inits.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "FRAMES.h" 3 | 4 | FRAMES::FRAMES(string g_path) { 5 | gen_path = g_path; 6 | frame_path = gen_path + "imgs/"; 7 | output_data_path = gen_path + "output/"; 8 | temp_data_path = gen_path + "temp_data/"; 9 | temp_frame_path = gen_path + "temp_frame/"; 10 | 11 | if (!PF.exists_file(output_data_path)) 12 | PF.create_file(output_data_path); 13 | if (!PF.exists_file(temp_data_path)) 14 | PF.create_file(temp_data_path); 15 | if (!PF.exists_file(temp_frame_path)) 16 | PF.create_file(temp_frame_path); 17 | 18 | } 19 | 20 | -------------------------------------------------------------------------------- /step1_image_matching/Project1/get_motion_parameters.cpp: -------------------------------------------------------------------------------- 1 | #include "FRAMES.h" 2 | 3 | void FRAMES::get_motion_parameters(int startframenumber, int freq) { 4 | //when we want to restart the process, load parameters and find initial fnum is needed; 5 | Mat cur_frame, pre_frame, cur_depth; 6 | //138001 or 0 7 | int endnum = 118000; 8 | if (startframenumber > 20000) 9 | endnum = 216000; 10 | for (int fnum = startframenumber; fnum < 216000 ; fnum+=freq) { 11 | if (!load_frame(fnum, cur_frame)){ 12 | save_results(pre_frame, fnum, freq, startframenumber); 13 | break; 14 | } 15 | if (circle_mat.empty()){ 16 | load_depth(fnum, cur_depth); 17 | load_circle(cur_frame, cur_depth, startframenumber); 18 | if (pre_frame.empty()) 19 | pre_frame = cur_frame.clone(); 20 | } 21 | else{ 22 | bool matched = match_circles(cur_frame);//cur 23 | if (!matched) { 24 | cout << "circles matched unsuccefully, reload started" << endl; 25 | load_circle(cur_frame, cur_depth,1); 26 | continue; 27 | } 28 | if (fnum % 1000 == 0) { 29 | load_depth(fnum, cur_depth); 30 | circle_point = vector_c_p.back(); 31 | circle_rad = vector_c_r.back(); 32 | circle_mat.clear(); 33 | circle_mat_depth.clear(); 34 | for (int k = 0; k < circle_rad.size(); k++) { 35 | int r = circle_rad[k]; 36 | Point p = circle_point[k]; 37 | Rect rect = Rect(p.x - r, p.y - r, r*2, r * 2); 38 | circle_mat.push_back(cur_frame(rect).clone()); 39 | circle_mat_depth.push_back(cur_depth(rect).clone()); 40 | } 41 | } 42 | } 43 | 44 | if ((fnum - startframenumber) % (300) == 0)// * 33 45 | save_results(cur_frame, fnum, freq, startframenumber);//cur 46 | cout << fnum << "-"; 47 | 48 | } 49 | 50 | } 51 | 52 | bool FRAMES::load_frame(int fnum, Mat &cur_frame) { 53 | string mat_path = frame_path + to_string(fnum) + ".jpg"; 54 | cur_frame = imread(mat_path); 55 | if (cur_frame.empty()) 56 | return false; 57 | else return true; 58 | } 59 | 60 | bool FRAMES::load_depth(int fnum, Mat &cur_depth) { 61 | string mat_path = frame_path + to_string(fnum) + ".jpg"; 62 | cur_depth = imread(mat_path, IMREAD_ANYDEPTH); 63 | if (cur_depth.empty()) 64 | return false; 65 | else return true; 66 | } 67 | 68 | //save frame with circles and locations(framenum, position (x,y) and radius) 69 | void FRAMES::save_results(Mat cur_frame, int fnum, int freq, int startframenumber) { 70 | for(int i = 0; i < vector_c_p.back().size(); i ++){ 71 | cv::circle(cur_frame, vector_c_p.back()[i], vector_c_r.back()[i], Scalar(255, 0, 0), 5); 72 | cv::putText(cur_frame, to_string(i), vector_c_p.back()[i], 1, 10, Scalar(255, 0, 0), 2); 73 | } 74 | string img_path = temp_frame_path + to_string(fnum) + ".jpg"; 75 | cv::imwrite(img_path, cur_frame); 76 | 77 | ofstream ofstr; 78 | string csv_path_s = temp_data_path + to_string(fnum) + ".csv"; 79 | ofstr.open(csv_path_s.c_str()); 80 | for (int i = 0; i < vector_c_p.size(); i++) { 81 | ofstr << i*freq + startframenumber << ","; 82 | for (int j = 0; j < vector_c_p[i].size(); j++) { 83 | ofstr << vector_c_p[i][j].x << "," << 84 | vector_c_p[i][j].y << "," << vector_c_r[i][j]<<","; 85 | } 86 | ofstr << "\n"; 87 | } 88 | } 89 | 90 | 91 | void FRAMES::push_points() { 92 | vector_c_p.push_back(circle_point); 93 | vector_c_r.push_back(circle_rad); 94 | } -------------------------------------------------------------------------------- /step1_image_matching/Project1/lanenum.py: -------------------------------------------------------------------------------- 1 | from sklearn.cluster import DBSCAN 2 | import numpy as np 3 | import csv 4 | from sklearn import metrics 5 | from sklearn.datasets.samples_generator import make_blobs 6 | from sklearn.preprocessing import StandardScaler 7 | import matplotlib.pyplot as plt 8 | 9 | data = [] 10 | with open('output.csv') as mycsv: 11 | csvread = csv.reader(mycsv) 12 | for row in csvread: 13 | t = [i for i in row] 14 | data.append(t) 15 | data = np.array(data) 16 | for k, col in zip(unique_labels, colors): 17 | plt.plot(xy[:, 0], xy[:, 1], 'o', markerfacecolor=tuple(col), 18 | markeredgecolor='w', markersize=4) 19 | plt.title('Estimated number of clusters: %d' % n_clusters_) 20 | plt.show() 21 | -------------------------------------------------------------------------------- /step1_image_matching/Project1/lanenumber1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATS-Lab/Filed-Experiment-Data-HIGH_Sim/b5f9845bd61591fdcd3e9101a22e458ca3afae18/step1_image_matching/Project1/lanenumber1.jpg -------------------------------------------------------------------------------- /step1_image_matching/Project1/lanenumberedtra1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATS-Lab/Filed-Experiment-Data-HIGH_Sim/b5f9845bd61591fdcd3e9101a22e458ca3afae18/step1_image_matching/Project1/lanenumberedtra1.png -------------------------------------------------------------------------------- /step1_image_matching/Project1/lanenumberedtraroundabouts.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATS-Lab/Filed-Experiment-Data-HIGH_Sim/b5f9845bd61591fdcd3e9101a22e458ca3afae18/step1_image_matching/Project1/lanenumberedtraroundabouts.jpg -------------------------------------------------------------------------------- /step1_image_matching/Project1/load_and_store_2.cpp: -------------------------------------------------------------------------------- 1 | #include "myVideo.h" 2 | #include "Veh.h" 3 | 4 | void myVideo::store2(const char* file, vector> V) { 5 | if (edge.empty()) 6 | edge = addedge(); 7 | //_mkdir(file); 8 | string s = file; 9 | string s_del = "rm -r "; 10 | s_del += s + "/*"; 11 | const char* todel = s_del.c_str(); 12 | cout << s_del << "\n" << todel << endl; 13 | //system(todel); 14 | int n = 0, cnum = 0, count = 0; 15 | 16 | ofstream myfile; 17 | string f_csv = s; 18 | const char* f_cs = f_csv.c_str(); 19 | myfile.open(f_cs); 20 | myfile << "i->num" << "," << "i->cx" << "," << "i->cy" << "," 21 | << "i->rect.x" << "," << "i->rect.y" << "," << "i->rect.width" << "," 22 | << "i->rect.height" << "," << "i->state" << ",i->ep[i],,,,,,,," << "time" << "\n"; 23 | while (1) { 24 | if (count == V.size()) 25 | break; 26 | Point2f pf[2]; 27 | for (auto &i : V[count]) { 28 | if (i->state > 0 && i->rect.x >= edge.x) { 29 | myfile << i->num << "," << i->cx << "," << i->cy << "," << i->rect.x << "," << i->rect.y << "," << i->rect.width << "," 30 | << i->rect.height << "," << i->state; 31 | for (int p = 0; p < 4; p++) 32 | myfile << "," << i->ep[2 * p] << "," << i->ep[2 * p + 1]; 33 | myfile << "," << i->ct; 34 | myfile << "\n"; 35 | } 36 | 37 | } 38 | count++; 39 | } 40 | cv::destroyAllWindows(); 41 | } 42 | 43 | void myVideo::load2(const char* file, vector> &V) { 44 | 45 | int num, x, y, w, h, state, count; 46 | float cx, cy, ep[8]; 47 | string snum, sx, sy, sw, sh, sstate, scx, scy, sep[8], scount; 48 | ifstream datafile(file); 49 | getline(datafile, snum); 50 | int count0 = 0, precount = 0; 51 | vector V0; 52 | while (getline(datafile, snum, ',')) { 53 | if (snum.length() < 2) { 54 | getline(datafile, snum); 55 | continue; 56 | } 57 | getline(datafile, scx, ','); 58 | getline(datafile, scy, ','); 59 | getline(datafile, sx, ','); 60 | getline(datafile, sy, ','); 61 | getline(datafile, sw, ','); 62 | getline(datafile, sh, ','); 63 | getline(datafile, sstate, ','); 64 | for (int i = 0; i < 8; i++) { 65 | getline(datafile, sep[i], ','); 66 | ep[i] = stof(sep[i]); 67 | } 68 | getline(datafile, scount); 69 | num = stoi(snum); 70 | cx = stof(scx); 71 | cy = stof(scy); 72 | x = stoi(sx); 73 | y = stoi(sy); 74 | w = stoi(sw); 75 | h = stoi(sh); 76 | state = stoi(sstate); 77 | count = stoi(scount); 78 | 79 | Veh *vehi = new Veh(num, cx, cy, x, y, w, h, state, ep, count); 80 | for (auto vehs : V0) 81 | if (vehs->num == vehi->num && count == precount) 82 | goto cont; 83 | if ((vehi->rect & block).area() > 0 || vehi->x > edge.x+edge.width || vehi->rect.width > 70 || !inEdge(vehi)) 84 | continue; 85 | //copy here new. 86 | if (count > V.size() + 1) { 87 | V.push_back(V0); 88 | V0.clear(); 89 | for (int i = 0; i < count - V.size() - 1; i++) 90 | V.push_back(V0); 91 | } 92 | if (count == precount) { 93 | if (!V.empty()) 94 | for (int i = 0; i < V.back().size(); i++) { 95 | if (V.back()[i]->num == vehi->num) { 96 | vehi->pv = V.back()[i]; 97 | V.back()[i]->nv = vehi; 98 | break; 99 | } 100 | } 101 | V0.push_back(vehi); 102 | } 103 | else { 104 | V.push_back(V0); 105 | V0.clear(); 106 | if (!V.empty()) 107 | for (int i = 0; i < V.back().size(); i++) { 108 | if (V.back()[i]->num == vehi->num) { 109 | vehi->pv = V.back()[i]; 110 | V.back()[i]->nv = vehi; 111 | break; 112 | } 113 | } 114 | V0.push_back(vehi); 115 | precount = count; 116 | } 117 | cont: 118 | continue; 119 | } 120 | V.push_back(V0); 121 | } -------------------------------------------------------------------------------- /step1_image_matching/Project1/load_block.cpp: -------------------------------------------------------------------------------- 1 | #include "myVideo.h" 2 | #include "Veh.h" 3 | Mat blockground; 4 | string mousewinname; 5 | void Cross_block(cv::Mat frame, cv::Point point, cv::Scalar s) { 6 | int cl = 10; 7 | cv::Point a(cl, 0), b(-cl, 0), c(0, cl), d(0, -cl); 8 | line(frame, point + a, point + b, s, 4); 9 | line(frame, point + c, point + d, s, 4); 10 | } 11 | 12 | void Mouseclick_block(int evt, int x, int y, int flags, void* edgepoints) { 13 | cv::Scalar s(0, 0, 255); 14 | std::vector* edgep = (std::vector*)edgepoints; 15 | if (evt == CV_EVENT_LBUTTONDOWN) { 16 | edgep->push_back(Point(x, y)); 17 | if (edgep->size() >= 1) { 18 | Point tem = edgep->back(); 19 | Cross(blockground, tem, s); 20 | } 21 | imshow(mousewinname, blockground); 22 | } 23 | } 24 | 25 | Rect myVideo::load_block() { 26 | vector blockpoints; 27 | string file = parapaths + "/block.txt"; 28 | Rect output; 29 | blockground = background.clone(); 30 | if (exists_file(file)) { 31 | ifstream input(file); 32 | string number; 33 | int paras[4]; 34 | int i = 0; 35 | while (i < 4) { 36 | getline(input, number); 37 | paras[i] = atoi(number.c_str()); 38 | i++; 39 | } 40 | output = Rect(paras[0], paras[1], paras[2], paras[3]); 41 | } 42 | else { 43 | namedWindow("Background", WINDOW_NORMAL | WINDOW_KEEPRATIO); 44 | cv::moveWindow("Background", 50, 50); 45 | cv::resizeWindow("Background", 1500, 750); 46 | cv::setMouseCallback("Background", Mouseclick_block, &blockpoints); 47 | imshow("Background", blockground); 48 | mousewinname = "Background"; 49 | char c = waitKey(0); 50 | if (c == 't') { 51 | if (blockpoints.size() > 3) { 52 | vector outpoints(blockpoints.end() - 4, blockpoints.end()); 53 | output = boundingRect(outpoints); 54 | } 55 | 56 | else { 57 | int x = 0.15 * blockground.cols, y = 0.05 * blockground.rows; 58 | int w = 0.7 * blockground.cols, h = 0.93 * blockground.rows; 59 | output = Rect(x, y, w, h); 60 | cout << "x" << x << "y" << y << "w" << w << "h" << h; 61 | } 62 | } 63 | output.x = max(50, output.x); 64 | output.height = min(YPIXEL - 50 - output.y, output.height); 65 | rectangle(blockground, output, Scalar(255), 5, 6, 0); 66 | imshow("Background", blockground); 67 | c = waitKey(3000); 68 | destroyAllWindows(); 69 | std::ofstream outfile(file); 70 | outfile << to_string(output.x) << std::endl; 71 | outfile << to_string(output.y) << std::endl; 72 | outfile << to_string(output.width) << std::endl; 73 | outfile << to_string(output.height) << std::endl; 74 | outfile.close(); 75 | } 76 | return output; 77 | } 78 | 79 | 80 | 81 | 82 | Rect myVideo::load_rect_etc(Mat frame_rted) { 83 | cout << "loading rect etc" << endl; 84 | vector blockpoints; 85 | string file = parapaths + "/rect_etc.txt"; 86 | Rect output; 87 | blockground = frame_rted.clone(); 88 | if (exists_file(file)) { 89 | ifstream input(file); 90 | string number; 91 | int paras[4]; 92 | int i = 0; 93 | while (i < 4) { 94 | getline(input, number); 95 | paras[i] = atoi(number.c_str()); 96 | i++; 97 | } 98 | output = Rect(paras[0], paras[1], paras[2], paras[3]); 99 | } 100 | else { 101 | string name = "rect_etc"; 102 | mousewinname = name; 103 | namedWindow(name, WINDOW_NORMAL | WINDOW_KEEPRATIO); 104 | cv::moveWindow(name, WINDOWSXP, 0); 105 | cv::resizeWindow(name, 400, 800); 106 | cv::setMouseCallback(name, Mouseclick_block, &blockpoints); 107 | reshow: 108 | imshow(name, blockground); 109 | char c = waitKey(0); 110 | if (c == 't') { 111 | if (blockpoints.size() > 3) { 112 | output = boundingRect(blockpoints); 113 | } 114 | else { 115 | cout << "rect_etc is not correctly loaded" << endl; 116 | blockground = frame_rted.clone(); 117 | blockpoints.clear(); 118 | goto reshow; 119 | } 120 | } 121 | rectangle(blockground, output, Scalar(255), 5, 6, 0); 122 | imshow(name, blockground); 123 | c = waitKey(2000); 124 | std::ofstream outfile(file); 125 | outfile << to_string(output.x) << std::endl; 126 | outfile << to_string(output.y) << std::endl; 127 | outfile << to_string(output.width) << std::endl; 128 | outfile << to_string(output.height) << std::endl; 129 | outfile.close(); 130 | destroyWindow(name); 131 | } 132 | return output; 133 | } 134 | -------------------------------------------------------------------------------- /step1_image_matching/Project1/openh264-1.7.0-win64.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATS-Lab/Filed-Experiment-Data-HIGH_Sim/b5f9845bd61591fdcd3e9101a22e458ca3afae18/step1_image_matching/Project1/openh264-1.7.0-win64.dll -------------------------------------------------------------------------------- /step2_training_img_generation/Project1/Angle_test.cpp: -------------------------------------------------------------------------------- 1 | #include "FRAMES.h" 2 | 3 | void FRAMES::test_angle(string path, bool st1) { 4 | vector center; 5 | vector angle; 6 | vector fnum; 7 | load_angle_parameters(gen_path + path, fnum, center, angle); 8 | Mat frame, frame_shifted, frame_rotated; 9 | Point c0 = center[0]; 10 | for (int i = 0; i < center.size(); i+=1000) { 11 | load_frame(fnum[i], frame); 12 | int shiftx = center[i].x - c0.x; 13 | int shifty = center[i].y - c0.y; 14 | //Mat rotate_M = getRotationMatrix2D(c0, (angle[i] - PI / 2)*180.0/PI, 1); 15 | //Mat shift_M = (Mat_(2, 3) << 1, 0, -shiftx, 0, 1, -shifty); 16 | //cv::warpAffine(frame, frame_shifted, shift_M, frame.size()); 17 | Mat rotate_M = getRotationMatrix2D(center[i], (angle[i] - PI / 2)*180.0 / PI, 1); 18 | //for left 1st video, shift 8190-5600=2500; 19 | Mat shift_M = (Mat_(2, 3) << 0, 0, -shiftx, 0, 0, -shifty+2500*st1); 20 | rotate_M = rotate_M + shift_M; 21 | cv::warpAffine(frame, frame_rotated, rotate_M, frame.size()); 22 | string img_path = "N:/LEFT/mid_processing/rotated/" + to_string(fnum[i]) + ".jpg"; 23 | cv::imwrite(img_path, frame_rotated); 24 | 25 | } 26 | 27 | } 28 | void FRAMES::load_angle_parameters(string path, vector &fnum, vector ¢er, vector &angle) { 29 | ifstream ifstr(path.c_str()); 30 | string snum; 31 | while (getline(ifstr, snum, ',')) { 32 | fnum.push_back(stoi(snum)); 33 | int i = 0, x = 0, y = 0; 34 | string xs, ys, as; 35 | float a; 36 | getline(ifstr, xs, ','); 37 | x = stoi(xs); 38 | getline(ifstr, ys, ','); 39 | y = stoi(ys); 40 | getline(ifstr, as); 41 | a = stof(as); 42 | center.push_back(Point(x, y)); 43 | angle.push_back(a); 44 | } 45 | } 46 | 47 | 48 | void FRAMES::load_circle_parameters(string path, vector &fnum, vector> ¢ers) { 49 | ifstream ifstr(path.c_str()); 50 | string snum; 51 | getline(ifstr, snum, ','); 52 | while (1) { 53 | if (snum.size() < 1) 54 | break; 55 | fnum.push_back(stoi(snum)); 56 | int i = 0, x = 0, y = 0, r = 0; 57 | string xs, ys, rs; 58 | vector center; 59 | while(1){ 60 | getline(ifstr, xs, ','); 61 | if(xs[0] == '\n'){ 62 | centers.push_back(center); 63 | xs.erase(xs.begin()); 64 | snum = xs; 65 | break; 66 | } 67 | x = stoi(xs); 68 | getline(ifstr, ys, ','); 69 | y = stoi(ys); 70 | getline(ifstr, rs, ','); 71 | r = stof(rs); 72 | center.push_back(Point(x, y)); 73 | } 74 | } 75 | } -------------------------------------------------------------------------------- /step2_training_img_generation/Project1/Calibration.cpp: -------------------------------------------------------------------------------- 1 | #include "myVideo.h" 2 | #include "Veh.h" 3 | void myVideo::cali(Mat frame) { 4 | string file = parapaths + "/cali.txt"; 5 | Mat frame_rted; 6 | if (exists_file(file)) { 7 | ifstream input(file); 8 | string number; 9 | getline(input, number); 10 | AL = atof(number.c_str()); 11 | Mat frame_rted; 12 | Calibration(frame, frame_rted); 13 | //Showframe("video", frame_rted, 5000); 14 | } 15 | else { 16 | namedWindow("video", WINDOW_NORMAL | WINDOW_KEEPRATIO); 17 | cv::moveWindow("video", WINDOWSXP, 0); 18 | cv::resizeWindow("video", 800, 1800); 19 | 20 | AL = 0; 21 | while (1) { 22 | //Showframe("video", frame); 23 | Calibration(frame, frame_rted); 24 | for (int i = 50; i < frame_rted.cols - 100; i += 100) 25 | cv::line(frame_rted, Point(i, 20), Point(i, frame_rted.rows - 20), Scalar(0, 0, 200), 3); 26 | cout << "click on t to set the angle\n"; 27 | Showframe("video", frame_rted, 400, 0, 1800,800); 28 | char c = (char)waitKey(3000);// 29 | switch (c) { 30 | case 'z': 31 | AL -= 5 * PI / 180; 32 | break; 33 | case 'c': 34 | AL += 5 * PI / 180; 35 | break; 36 | case 'q': 37 | AL -= 2 * PI / 180; 38 | break; 39 | case 'e': 40 | AL += 2 * PI / 180; 41 | break; 42 | case 'a': 43 | AL -= 0.5 * PI / 180; 44 | break; 45 | case 'd': 46 | AL += 0.5 * PI / 180; 47 | break; 48 | } 49 | 50 | if (c == 't') 51 | break; 52 | 53 | } 54 | std::ofstream outfile(file); 55 | outfile << to_string(AL) << std::endl; 56 | outfile.close(); 57 | } 58 | Calibration(frame, frame_rted); 59 | background = frame_rted.clone(); 60 | destroyWindow("video"); 61 | //Showframe("video", edgebackground); 62 | //destroyAllWindows(); 63 | } 64 | 65 | void myVideo::Calibration(Mat frame, Mat &dst) { 66 | getangle(); 67 | 68 | float Zy = (FF*cos(AL) + HS / HI * sin(AL) * (HI - HI / 2)); 69 | float yrmax = HS / HI * (HI / 2) * HH / cos(AL) / Zy; 70 | Zy = (FF*cos(AL) - HS / HI * sin(AL) * (HI / 2)); 71 | float yrmin = -HS / HI * (HI / 2.0) * HH / cos(AL) / Zy; 72 | float Z = -yrmin * sin(AL) + HH / cos(AL); 73 | float xrmin = -Z / FF * WS / WI * WI / 2; 74 | float xrmax = Z / FF * WS / WI * WI / 2; 75 | float xrr = xrmax - xrmin + 1; 76 | float yrr = yrmax - yrmin + 1; 77 | float yr, xr, yri, xri; 78 | int x_, y_; 79 | 80 | cv::Mat_ mapx_32f((int)HI, (int)WI), mapy_32f((int)HI, (int)WI); 81 | 82 | //Mat dst2((int)HI,(int)WI, CV_8UC3, Scalar(0, 0, 0)); 83 | for (int y = 0; y pt(3, 1); 90 | pt(0) = x; 91 | pt(1) = y; 92 | pt(2) = 1; 93 | 94 | yri = y; 95 | xri = x; 96 | yr = yrr / HI * yri + yrmin; 97 | xr = xrr / WI * xri + xrmin; 98 | Zy = (FF*cos(AL) * yr / (-yr*sin(AL) + HH / cos(AL))); 99 | 100 | Z = -yr*sin(AL) + HH / cos(AL); 101 | 102 | pt(1) = y_ = HI / 2 + HI / HS * Zy; 103 | pt(0) = x_ = FF / Z * WI / WS * (xr)+WI / 2; 104 | 105 | pt /= pt(2); 106 | buff_mapx[x] = (int)pt(0); 107 | buff_mapy[x] = (int)pt(1); 108 | //Vec3b color = frame_org.at(Point(x, y)); 109 | //dst2.at(Point(x_, y_)) = color; 110 | } 111 | } 112 | cv::convertMaps(mapx_32f, mapy_32f, map1_16u, map2_16u, CV_32FC2, true); 113 | //frame_r = dst2.clone(); 114 | cv::remap(frame, dst, map1_16u, map2_16u, CV_INTER_LINEAR, BORDER_CONSTANT, Scalar(0, 0, 0)); 115 | } 116 | 117 | void myVideo::Calibrateframe(Mat fgmask_rot, Mat &fgmask_cali) { 118 | cv::remap(fgmask_rot, fgmask_cali, map1_16u, map2_16u, CV_INTER_LINEAR, BORDER_CONSTANT, Scalar(0, 0, 0)); 119 | } 120 | 121 | void myVideo::Calibrateframe(GpuMat fgmask, GpuMat &fgmask_cali) { 122 | Mat m1, m2; 123 | fgmask.download(m1); 124 | cv::remap(m1, m2, map1_16u, map2_16u, CV_INTER_LINEAR, BORDER_CONSTANT, Scalar(0, 0, 0)); 125 | 126 | fgmask_cali.upload(m2); 127 | } 128 | 129 | void myVideo::getangle() { 130 | float H, al, a, pi, x, b, bl, abl, r, focal, hs, ws; 131 | H = 400.0; 132 | pi = 3.14159; 133 | FF = focal = 8.8 / 1000.0*0.3048; 134 | HH = H; 135 | WS = 13.2*0.3048 / 1000.0; 136 | HS = 7.425*0.3048 / 1000.0; 137 | } -------------------------------------------------------------------------------- /step2_training_img_generation/Project1/D2lanenum.m: -------------------------------------------------------------------------------- 1 | color = rand(30,3) -------------------------------------------------------------------------------- /step2_training_img_generation/Project1/Get_learning_imgs.cpp: -------------------------------------------------------------------------------- 1 | #include "FRAMES.h" 2 | 3 | void FRAMES::get_learning_imgs(string path, int sti, int stj, int &imgnum, const int W, const int H) { 4 | vector> center; 5 | vector fnum; 6 | load_circle_parameters(gen_path + path, fnum, center); 7 | string edge_path = gen_path + "temp_data/" + to_string(stj) + "_edge.csv"; 8 | vector edge; 9 | 10 | for(auto &ca:center) 11 | for(auto & ci:ca) 12 | ci = ci + Point(IMGW / 2, IMGH / 2); 13 | if (sti == 2 && stj == 1) 14 | for (auto &ca : center){ 15 | if (ca.size() == 4) 16 | ca.erase(ca.begin() + 2); 17 | } 18 | load_edge_function(center, edge_path, fnum, edge); 19 | Mat frame, frame_shifted, frame_rotated, affine_M; 20 | for (int i = 0; i < center.size(); i += 500) { 21 | load_frame(fnum[i], frame); 22 | Size large_size = frame.size() * 2; 23 | int frame_W = large_size.width, frame_H = large_size.height; 24 | //cv::Mat large_f(frame.size()*2, CV_8UC3, cv::Scalar(0, 0, 0)); 25 | if (center[0].size() > 3) 26 | affine_M = findHomography(center[i], center[0]); 27 | else { 28 | Point2f a0[3] = { center[0][0], center[0][1], center[0][2] }; 29 | Point2f a1[3] = { center[i][0], center[i][1], center[i][2] }; 30 | affine_M = getAffineTransform(a1, a0); 31 | } 32 | Mat shift_M = (Mat_(2, 3) << 1, 0, large_size.width / 4, 0, 1, large_size.height / 4); 33 | cv::warpAffine(frame, frame_shifted, shift_M, large_size); 34 | if (center[0].size() > 3) 35 | cv::warpPerspective(frame_shifted, frame_rotated, affine_M, large_size); 36 | else 37 | cv::warpAffine(frame_shifted, frame_rotated, affine_M, large_size); 38 | Size2i resized(frame_rotated.cols*ratio_frames, frame_rotated.rows*ratio_frames); 39 | cv::resize(frame_rotated, frame_rotated, resized); 40 | cut_and_save_img(sti, frame_rotated, imgnum, edge, W, H); 41 | cout << i << "#"; 42 | //show_frame(frame_shifted, 300000); 43 | string img_path = gen_path + "temp_frame/" + to_string(fnum[i]) + "_calied.jpg"; 44 | cv::imwrite(img_path, frame_rotated); 45 | } 46 | } 47 | 48 | void FRAMES::cut_and_save_img(int sti, Mat frame_rotated, int &imgnum, vector edge, const int W, const int H) { 49 | 50 | int x0 = edge[0]; 51 | int y0 = 0; 52 | int intend = 200; 53 | int high = frame_rotated.rows - 1; 54 | int bot = high; 55 | int frameW = frame_rotated.cols, frameH = frame_rotated.rows; 56 | for (y0 = 1; y0 < frame_rotated.rows; y0++) { 57 | Point p1(edge[y0], y0); 58 | Point p2(edge[y0]+W, y0); 59 | if (edge[y0] < W || edge[y0] + W > frameW) 60 | continue; 61 | Vec3b cor1 = frame_rotated.at(p1); 62 | Vec3b cor2 = frame_rotated.at(p2); 63 | if (cor1[0] > 0 || cor2[0] > 0) 64 | break; 65 | } 66 | 67 | for (bot = y0; bot < high; bot ++) { 68 | Point p1(edge[bot], bot); 69 | Point p2(edge[bot]+W, bot); 70 | if (p1.x < 800) 71 | break; 72 | Vec3b cor1 = frame_rotated.at(p1); 73 | Vec3b cor2 = frame_rotated.at(p2); 74 | if (cor1[0] == 0 && cor2[0] == 0) 75 | break; 76 | } 77 | //cout << bot << endl; 78 | //show_frame(frame_rotated, 50000); 79 | Rect cut; 80 | while (1) { 81 | double angle = atan(double(edge[y0 + H] - edge[y0]) / double(H)); 82 | x0 = edge[y0]; 83 | Mat rot_mat = getRotationMatrix2D(Point(x0,y0), -angle*180/PI, 1); 84 | Mat to_cut; 85 | cv::warpAffine(frame_rotated, to_cut, rot_mat, frame_rotated.size()); 86 | if(sti == 0) 87 | cut = Rect(x0 - 350, y0, W, H); 88 | else if (sti == 1) 89 | cut = Rect(x0 - 310, y0, W, H); 90 | else 91 | cut = Rect(x0 - 200, y0, W, H); 92 | //rectangle(to_cut, cut, Scalar(0, 255, 0), 12); 93 | 94 | 95 | Mat subimg = to_cut(cut); 96 | //show_frame(subimg, 500); 97 | string img_path = "N:/LEFT/learning_imgs/imgs/" + to_string(imgnum++) + ".jpg"; 98 | cv::imwrite(img_path, subimg); 99 | for (double sum = 0; sum < H; y0++) { 100 | sum += sqrt(pow(edge[y0 + 1] - edge[y0], 2) + 1); 101 | if (sum > H - intend) 102 | break; 103 | } 104 | if (y0 >= bot) 105 | break; 106 | } 107 | } -------------------------------------------------------------------------------- /step2_training_img_generation/Project1/Lanefunctions.cpp: -------------------------------------------------------------------------------- 1 | #include "FRAMES.h" 2 | #include "Poly.h" 3 | #include "Lanes.h" 4 | #include "Public_functions.h" 5 | 6 | void LaneS::loadlanenum() { 7 | string file = path + "/lanenum.txt"; 8 | string lanenum_s; 9 | char c; 10 | Public_func PB; 11 | if (PB.exists_file(file)) { 12 | ifstream input(file); 13 | getline(input, lanenum_s); 14 | lanenum = atoi(lanenum_s.c_str()); 15 | } 16 | else { 17 | cout << "lanenumtxt not exist.\n"; 18 | getline(cin, lanenum_s); 19 | lanenum = atoi(lanenum_s.c_str()); 20 | std::ofstream outfile(file); 21 | outfile << to_string(lanenum) << std::endl; 22 | outfile.close(); 23 | } 24 | for (int i = 0; i < lanenum; i++) { 25 | vector vec; 26 | vehpointx.push_back(vec); 27 | vehpointy.push_back(vec); 28 | } 29 | } 30 | 31 | void LaneS::loadorderandpoints() { 32 | 33 | loadlanenum(); 34 | order = 2; 35 | string file = path + "/lanes.txt"; 36 | Public_func PB; 37 | if (PB.exists_file(file)) {// 38 | ifstream input(file); 39 | for (int j = 0; j < lanenum; j++) { 40 | 41 | vector coefs; 42 | string ys; 43 | float y; 44 | int i = 0; 45 | while (i++ < order + 1) { 46 | getline(input, ys); 47 | y = atof(ys.c_str()); 48 | coefs.push_back(y); 49 | } 50 | fy.push_back(coefs); 51 | } 52 | } 53 | else { 54 | cout << "file not found\n"; 55 | } 56 | 57 | } 58 | 59 | void LaneS::findlanenum(Veh * vehi) { 60 | float x, y; 61 | int cutH = 2000; 62 | if (vehi->pv != NULL && vehi->y > cutH){ 63 | vehi->lnum = vehi->pv->lnum; 64 | return; 65 | } 66 | 67 | x = vehi->cx; 68 | y = vehi->cy; 69 | 70 | 71 | vector xdet;; 72 | for (int i = 0; i < lanenum; i++) { 73 | float xi = fy[i][0] + fy[i][1] * y + fy[i][2] * y*y; 74 | xdet.push_back(abs(x - xi)); 75 | } 76 | int output = 0; 77 | float dis0 = 10; 78 | float dis = dis0; 79 | for (int i = 0; i < lanenum; i++) { 80 | if (xdet[i] < dis) { 81 | output = i; 82 | dis = xdet[i]; 83 | } 84 | } 85 | if(dis < dis0){ 86 | vehi->lnum = output; 87 | if(dis < 3){ 88 | vehpointx[output].push_back(x); 89 | vehpointy[output].push_back(y); 90 | } 91 | } 92 | else { 93 | vehi->lnum = -1; 94 | } 95 | 96 | } 97 | 98 | LaneS::LaneS() { 99 | 100 | } 101 | 102 | LaneS::~LaneS() { 103 | 104 | } 105 | 106 | void LaneS::regresslanes() { 107 | for (int i = 0; i < lanenum; i++) { 108 | if (vehpointx[i].size() < 200 || vehpointy[i].size() != vehpointx[i].size()) 109 | continue; 110 | PolynomialRegression poly; 111 | poly.fitIt(vehpointy[i], vehpointx[i], order, fy[i]); 112 | } 113 | } 114 | 115 | void LaneS::showlane(Mat frame) { 116 | string windowname = "Lanes"; 117 | namedWindow(windowname, WINDOW_NORMAL | WINDOW_KEEPRATIO); 118 | cv::moveWindow(windowname, 5, 5); 119 | cv::resizeWindow(windowname, 384 * 4, 216 * 4); 120 | imshow(windowname, frame); 121 | for(float y = 0; y < 8190; y = y + 1.0) 122 | for (int i = 0; i < lanenum; i++){ 123 | float xi = fy[i][0] + fy[i][1] * y + fy[i][2] * y*y; 124 | cv::circle(frame, Point(xi, y), 2, Scalar(0, 0, 60 * i), 1); 125 | } 126 | imshow(windowname, frame); 127 | waitKey(200); 128 | destroyWindow(windowname); 129 | } -------------------------------------------------------------------------------- /step2_training_img_generation/Project1/Lanes.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "FRAMES.h" 3 | #include "Poly.h" 4 | 5 | class LaneS { 6 | public: 7 | vector> vehpointx; 8 | vector> vehpointy; 9 | 10 | int order; 11 | int lanenum; 12 | float time0; 13 | vector> fy; 14 | Mat frame_0; 15 | string path; 16 | 17 | 18 | public: 19 | LaneS(); 20 | ~LaneS(); 21 | 22 | void loadlanenum(); 23 | void loadorderandpoints(); 24 | void regresslanes(); 25 | void findlanenum(Veh * v); 26 | void showlane(Mat frame); 27 | }; 28 | -------------------------------------------------------------------------------- /step2_training_img_generation/Project1/Load_circle_parameters.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "FRAMES.h" 3 | 4 | void FRAMES::load_circle_parameter(string path) { 5 | ifstream ifstr(path.c_str()); 6 | string snum; 7 | while (getline(ifstr, snum, ',')) { 8 | int i = 0, x = 0, y = 0; 9 | vfnum.push_back(stoi(snum)); 10 | vector points; 11 | vector rad; 12 | while (i < 9) { 13 | getline(ifstr, snum, ','); 14 | switch (i++ % 3) { 15 | case 0: 16 | x = stoi(snum); 17 | break; 18 | case 1: 19 | y = stoi(snum); 20 | points.push_back(Point2i(x, y)); 21 | break; 22 | case 2: 23 | rad.push_back(stoi(snum)); 24 | break; 25 | } 26 | } 27 | getline(ifstr, snum); 28 | vector_c_p.push_back(points); 29 | vector_c_r.push_back(rad); 30 | } 31 | } -------------------------------------------------------------------------------- /step2_training_img_generation/Project1/Load_cut_edge.cpp: -------------------------------------------------------------------------------- 1 | #include "FRAMES.h" 2 | #include "Poly.h" 3 | 4 | void FRAMES::load_edge_function(vector> center, string edge_path, vector fnum, vector &output) { 5 | Public_func pf; 6 | Mat frame, frame_shifted, frame_rotated, frame_merged; 7 | vector points; 8 | Size2i resized(IMGW*2*ratio_frames, IMGH * 2 * ratio_frames); 9 | Mat affine_M; 10 | if (pf.exists_file(edge_path)) { 11 | load_edge_from_file(edge_path, points); 12 | } 13 | else { 14 | for (int i = 0; i < center.size(); i += 4000) { 15 | load_frame(fnum[i], frame); 16 | Size large_size = frame.size() * 2; 17 | if (center[0].size() > 3) 18 | affine_M = findHomography(center[i], center[0]); 19 | else{ 20 | Point2f a0[3] = {center[0][0], center[0][1], center[0][2]}; 21 | Point2f a1[3] = { center[i][0], center[i][1], center[i][2] }; 22 | affine_M = getAffineTransform(a1, a0); 23 | } 24 | Mat shift_M = (Mat_(2, 3) << 1, 0, large_size.width / 4, 0, 1, large_size.height / 4); 25 | cv::warpAffine(frame, frame_shifted, shift_M, large_size); 26 | if (center[0].size() > 3) 27 | cv::warpPerspective(frame_shifted, frame_rotated, affine_M, large_size); 28 | else 29 | cv::warpAffine(frame_shifted, frame_rotated, affine_M, large_size); 30 | //show_frame(frame_rotated, 10000); 31 | if (!frame_merged.empty()) 32 | cv::addWeighted(frame_merged, 0.5, frame_rotated, 0.5, 0, frame_merged); 33 | else 34 | frame_merged = frame_rotated.clone(); 35 | } 36 | cv::resize(frame_merged, frame_merged, resized); 37 | load_edge_from_frame(frame_merged, points); 38 | ofstream outcsv(edge_path.c_str()); 39 | for (auto p : points) 40 | outcsv << p.x << "," << p.y << "\n"; 41 | } 42 | 43 | PolynomialRegression poly; 44 | vector vy, vx, f; 45 | for (auto p : points) { 46 | vy.push_back(long double(p.y)); 47 | vx.push_back(long double(p.x)); 48 | } 49 | poly.fitIt(vy, vx, EDGE_FIT_ORDER, f); 50 | output.clear(); 51 | for (double y = 0; y < resized.height; y = y + 1.0) { 52 | double xi = 0; 53 | for (int j = 0; j < EDGE_FIT_ORDER + 1; j++) 54 | xi += f[j] * pow(y, j); 55 | output.push_back(xi); 56 | } 57 | } 58 | 59 | void FRAMES::load_edge_from_file(string edge_path, vector &points) { 60 | ifstream input(edge_path.c_str()); 61 | string xs, ys; 62 | float x, y; 63 | while (getline(input, xs, ',')) { 64 | x = stoi(xs); 65 | getline(input, ys); 66 | y = stof(ys); 67 | points.push_back(Point(x, y)); 68 | } 69 | } 70 | 71 | Mat frame_draw; 72 | void Cross(cv::Mat frame, cv::Point point, cv::Scalar s) { 73 | int cl = 10; 74 | cv::Point a(cl, 0), b(-cl, 0), c(0, cl), d(0, -cl); 75 | line(frame, point + a, point + b, s, 4); 76 | line(frame, point + c, point + d, s, 4); 77 | } 78 | 79 | void Mouseclick(int evt, int x, int y, int flags, void* edgepoints) { 80 | cv::Scalar s(0, 0, 255); 81 | std::vector* edgep = (std::vector*)edgepoints; 82 | if (evt == CV_EVENT_LBUTTONDOWN) { 83 | edgep->push_back(Point(x, y)); 84 | if (edgep->size() >= 1) { 85 | Point tem = edgep->back(); 86 | Cross(frame_draw, tem, s); 87 | } 88 | imshow("Merged_frame", frame_draw); 89 | } 90 | } 91 | void FRAMES::load_edge_from_frame(Mat frame_merged, vector &points){ 92 | frame_draw = frame_merged.clone(); 93 | string windowname = "Merged_frame"; 94 | namedWindow(windowname, WINDOW_NORMAL | WINDOW_KEEPRATIO); 95 | cv::moveWindow(windowname, 0, 0); 96 | cv::resizeWindow(windowname, 1820, 1000); 97 | 98 | cv::setMouseCallback(windowname, Mouseclick, &points); 99 | 100 | imshow(windowname, frame_draw); 101 | reload: 102 | points.clear(); 103 | char c = waitKey(0); 104 | if (c == 'c') { 105 | if (points.size() > 3) { 106 | destroyWindow(windowname); 107 | return; 108 | } 109 | else { 110 | cout << "please enter more points"; 111 | goto reload; 112 | } 113 | } 114 | else 115 | goto reload; 116 | 117 | } -------------------------------------------------------------------------------- /step2_training_img_generation/Project1/Main.cpp: -------------------------------------------------------------------------------- 1 | #include "FRAMES.h" 2 | #include 3 | 4 | int main() 5 | { 6 | string gen_path[3] = {"N:/LEFT/","L:/MIDDLE/","N:/RIGHT/" }; 7 | 8 | //1. 9 | //FRAMES.get_motion_parameters(); 10 | //2. 11 | //string path = "verify/to_show.csv"; 12 | //FRAMES.test_circles(path); 13 | //3.test angle paramters and generate rotated imgs. 14 | // 15 | /* 16 | string path[2] = { "temp_data/117700.csv", "temp_data/215400.csv" }; 17 | double ratio[2][3] = { { 0.497326203, 1, 0.486631016 }, { 0.455782313, 1, 0.469387755 } }; 18 | int order[3] = { 1,9,5 }; 19 | //FRAMES.test_angle(path[1]); 20 | //4. generate learning imgs, with W nad H; 21 | int learn_w = 480, learn_h = 800; 22 | for(int i = 0, imgnum = 0; i < 3; i ++){ 23 | for(int j = 0; j < 2; j ++){ 24 | FRAMES FRAMES(gen_path[i]); 25 | FRAMES.ratio_frames = ratio[j][i]; 26 | FRAMES.EDGE_FIT_ORDER = order[i]; 27 | cout << "\n" << i << j <<":"<< endl; 28 | if(i == 0 && j == 1) 29 | FRAMES.get_learning_imgs("temp_data/206700.csv", i, j, imgnum, learn_w, 800); 30 | else 31 | FRAMES.get_learning_imgs(path[j], i, j, imgnum, learn_w,800); 32 | } 33 | } 34 | */ 35 | //5. generate learning data 36 | FRAMES FRAMES(gen_path[0]); 37 | string imgpath = "learning_imgs/imgs/"; 38 | string darknet = "C:/Users/dongfangzhao/Documents/OPENCV/darknet-master/build/darknet/x64/"; 39 | string gen_cnn_path = darknet + "backup/I75/all"; 40 | string learning_img_path = darknet + "data/obj/"; 41 | int interval = 1; 42 | string last_cnn_path = gen_cnn_path + "_" + to_string(interval * 10) + "/"; 43 | string cnn_path = gen_cnn_path + "_" + to_string(interval) + "/"; 44 | 45 | FRAMES.generate_learning_data(imgpath, last_cnn_path, cnn_path, learning_img_path, interval); 46 | 47 | return 0; 48 | } 49 | -------------------------------------------------------------------------------- /step2_training_img_generation/Project1/NOTE.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Documentation: 3 | 0. preprocessing 4 | a. revise rotation algorithm, make it work in 2s frames; 5 | b. revise calibration part... 6 | c. cut the processed image into edge, (save it, optional); 7 | 8 | 1. revise the identification part; 9 | a. resize the image to imagesize*1.2; 10 | b. cut image into 5 pieces and detect them; 11 | c. process results with Rect parameters, coordinate/1.2, and return results; 12 | 13 | 2. tet performance. 14 | 15 | */ 16 | -------------------------------------------------------------------------------- /step2_training_img_generation/Project1/OperateOneFrame.cpp: -------------------------------------------------------------------------------- 1 | #include "myVideo.h" 2 | #include "Veh.h" 3 | 4 | void myVideo::OperateOneFrame(Mat fgmask_rot, vector> &Fig, int count, int &n, int cnum) { 5 | vector> contours; 6 | vector hierarchy; 7 | findContours(fgmask_rot, contours, hierarchy, RETR_CCOMP, CHAIN_APPROX_SIMPLE); 8 | vector Fig0; 9 | int hi; 10 | for (int i = 0; i < int(contours.size()); i++) { 11 | if (hierarchy[i][3] < 0) { 12 | if (contours[i].size() < 10) 13 | continue; 14 | float ty = static_cast(contourArea(cv::Mat(contours[i]))); 15 | Moments mu = moments(contours[i], false); 16 | Point2f mc = Point2f(static_cast(mu.m10 / mu.m00), static_cast(mu.m01 / mu.m00)); 17 | mc = Point2f(max(0.0f, mc.x), max(0.0f, mc.y)); 18 | Rect rect = boundingRect(contours[i]); 19 | RotatedRect rt = minAreaRect(contours[i]); 20 | float angle = rt.angle; 21 | Veh* v = new Veh(n++, ty, rt, cnum, count, angle); 22 | Point2f pf[4]; 23 | rt.points(pf); 24 | for (int k = 0; k < 4; k++) { 25 | v->ep[2 * k] = pf[k].x; 26 | v->ep[2 * k + 1] = pf[k].y; 27 | if (cv::norm(pf[k] - pf[(k + 1) % 3]) > 1500) 28 | ty = 10; 29 | } 30 | if (ty < MINCONAREA || ty > MAXCONAREA) 31 | continue; 32 | //if (count > 160) 33 | //rectangle(fgmask_rot, rect, Scalar(255), 10, 8, 0); 34 | Fig0.push_back(v); 35 | } 36 | } 37 | if (count == 0) { 38 | Fig.push_back(Fig0); 39 | } 40 | else { 41 | vector Fb = Fig.back(); 42 | double disu = 50.0; 43 | double d1, d2; 44 | int k; 45 | for (int i = 0; i < int(Fig0.size()); i++) { 46 | d2 = disu; 47 | for (int j = 0; j < int(Fb.size()); j++) { 48 | d1 = Vehdis(Fig0[i], Fb[j]); 49 | if (d1 < d2) { 50 | d2 = d1; 51 | k = j; 52 | } 53 | } 54 | int newv = 1; 55 | if (d2 < disu) { 56 | if (Fb[k]->nv) { 57 | float a1, a2, a3; 58 | a1 = Fb[k]->vtype; 59 | a2 = Fb[k]->nv->vtype; 60 | a3 = Fig0[i]->vtype; 61 | float area1, area2; 62 | area1 = (a1 - a2) * (a1 - a2); 63 | area2 = (a1 - a3) * (a1 - a3); 64 | if (area1 < area2) 65 | newv = 0; 66 | else 67 | newv = 2; 68 | } 69 | if (newv) { 70 | if (newv == 2) { 71 | Fb[k]->nv->pv = NULL; 72 | //Fb[k]->nv = NULL; 73 | } 74 | Fig0[i]->pv = Fb[k]; 75 | Fb[k]->nv = Fig0[i]; 76 | Fig0[i]->num = Fb[k]->num; 77 | Fig0[i]->vtype = (Fb[k]->vtype + Fig0[i]->vtype) / 2; 78 | Fb[k]->vtype = Fig0[i]->vtype; 79 | if (!Fig0[i]->state) { 80 | Veh* b; 81 | b = Fig0[i]; 82 | b->forcast[0] = 2 * b->cx - b->pv->cx; 83 | b->forcast[1] = 2 * b->cy - b->pv->cy; 84 | b->state = 1; 85 | } 86 | } 87 | } 88 | } 89 | Fig.push_back(Fig0); 90 | Fig0.clear(); 91 | } 92 | } 93 | 94 | void myVideo::RefreshOutput(vector> &Fig) { 95 | for (int i = 0; i < Fig.size(); i ++) { 96 | for (int j = 0; j < Fig[i].size(); j++) { 97 | Veh* v; 98 | v = Fig[i][j]; 99 | if (v->time == 0) { 100 | int ti = 1; 101 | while (v->nv) { 102 | v = v->nv; 103 | ti++; 104 | } 105 | while (v) { 106 | v->time = ti; 107 | v = v->pv; 108 | } 109 | } 110 | } 111 | } 112 | for (auto &i : Fig) { 113 | for (int j = 0; j < int(i.size()); j++) { 114 | if (i[j]->nv) 115 | if (i[j]->time != i[j]->nv->time) 116 | cout << "wrong"; 117 | else if (i[j]->nv->pv == NULL) 118 | cout << "nopv"; 119 | else if (i[j]->nv->num != i[j]->num) 120 | cout << "number not equal"; 121 | if (i[j]->time < MINFRAMELEN) 122 | i.erase(i.begin() + j--); 123 | } 124 | } 125 | } -------------------------------------------------------------------------------- /step2_training_img_generation/Project1/POINTS.cpp: -------------------------------------------------------------------------------- 1 | #include "myVideo.h" 2 | #include "Veh.h" 3 | 4 | void myVideo::Points(vector > V) { 5 | Mat img(YPIXEL, XPIXEL, CV_8UC3, Scalar(255, 255, 255)); 6 | int num = 0; 7 | std::ofstream mycsv; 8 | mycsv.open("output.csv"); 9 | for (auto &i : V) { 10 | for (auto &j : i) { 11 | mycsv << j->num << "," << j->glotime << "," << j->cx << "," << j->cy << "," << j->lnum << "," << j->vtype << ",\n"; 12 | num++; 13 | //if (num)// % 2 == 0) 14 | circle(img, cv::Point2f(j->cx, j->cy), 2, Scalar(0, 0, 0), 2, 8, 0); 15 | circle(background, cv::Point2f(j->cx, j->cy), 2, Scalar(0, 0, 255), 2, 8, 0); 16 | } 17 | } 18 | mycsv.close(); 19 | namedWindow("Points", WINDOW_NORMAL | WINDOW_KEEPRATIO); 20 | cv::moveWindow("Points", WINDOWSXP, WINDOWSYP); 21 | cv::resizeWindow("Points", WINDOWSW, WINDOWSH); 22 | imshow("Points", img); // Show our image inside it. 23 | imwrite("a.jpg", img); 24 | imwrite("b.jpg", background); 25 | waitKey(3000); 26 | cv::destroyAllWindows(); 27 | } 28 | -------------------------------------------------------------------------------- /step2_training_img_generation/Project1/Plot.cpp: -------------------------------------------------------------------------------- 1 | #include "myVideo.h" 2 | #include "Veh.h" 3 | #include 4 | 5 | void Plot(vector > V){ 6 | ofstream out("zline.xls"); 7 | if (!out) { 8 | cout << "open lxs unsuccessful"; 9 | throw; 10 | } 11 | for (auto &i : V){ 12 | for (auto &j : i){ 13 | out << j->num << " \t" << j->glotime << " \t" << j->x << " \t" << j->y\ 14 | << " \t" << j->spd << " \t" << j->acc << " \t" << j->lnum << endl; 15 | } 16 | } 17 | out.close(); 18 | 19 | } 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /step2_training_img_generation/Project1/Poly.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #ifndef _POLYNOMIAL_REGRESSION_H 3 | #define _POLYNOMIAL_REGRESSION_H __POLYNOMIAL_REGRESSION_H 4 | 5 | /** 6 | * PURPOSE: 7 | * 8 | * Polynomial Regression aims to fit a non-linear relationship to a set of 9 | * points. It approximates this by solving a series of linear equations using 10 | * a least-squares approach. 11 | * 12 | * We can model the expected value y as an nth degree polynomial, yielding 13 | * the general polynomial regression model: 14 | * 15 | * y = a0 + a1 * x + a2 * x^2 + ... + an * x^n 16 | * @author Chris Engelsma 17 | */ 18 | 19 | #include 20 | #include 21 | 22 | template 23 | class PolynomialRegression { 24 | public: 25 | 26 | PolynomialRegression(); 27 | virtual ~PolynomialRegression() {}; 28 | 29 | bool fitIt( 30 | const std::vector & x, 31 | const std::vector & y, 32 | const int & order, 33 | std::vector & coeffs); 34 | }; 35 | 36 | template 37 | PolynomialRegression::PolynomialRegression() {}; 38 | 39 | template 40 | bool PolynomialRegression::fitIt( 41 | const std::vector & x, 42 | const std::vector & y, 43 | const int & order, 44 | std::vector & coeffs) 45 | { 46 | // The size of xValues and yValues should be same 47 | if (x.size() != y.size()) { 48 | throw std::runtime_error("The size of x & y arrays are different"); 49 | return false; 50 | } 51 | // The size of xValues and yValues cannot be 0, should not happen 52 | if (x.size() == 0 || y.size() == 0) { 53 | throw std::runtime_error("The size of x or y arrays is 0"); 54 | return false; 55 | } 56 | 57 | size_t N = x.size(); 58 | int n = order; 59 | int np1 = n + 1; 60 | int np2 = n + 2; 61 | int tnp1 = 2 * n + 1; 62 | TYPE tmp; 63 | 64 | // X = vector that stores values of sigma(xi^2n) 65 | std::vector X(tnp1); 66 | for (int i = 0; i < tnp1; ++i) { 67 | X[i] = 0; 68 | for (int j = 0; j < N; ++j) 69 | X[i] += (TYPE)pow(x[j], i); 70 | } 71 | 72 | // a = vector to store final coefficients. 73 | std::vector a(np1); 74 | 75 | // B = normal augmented matrix that stores the equations. 76 | std::vector > B(np1, std::vector(np2, 0)); 77 | 78 | for (int i = 0; i <= n; ++i) 79 | for (int j = 0; j <= n; ++j) 80 | B[i][j] = X[i + j]; 81 | 82 | // Y = vector to store values of sigma(xi^n * yi) 83 | std::vector Y(np1); 84 | for (int i = 0; i < np1; ++i) { 85 | Y[i] = (TYPE)0; 86 | for (int j = 0; j < N; ++j) { 87 | Y[i] += (TYPE)pow(x[j], i)*y[j]; 88 | } 89 | } 90 | 91 | // Load values of Y as last column of B 92 | for (int i = 0; i <= n; ++i) 93 | B[i][np1] = Y[i]; 94 | 95 | n += 1; 96 | int nm1 = n - 1; 97 | 98 | // Pivotisation of the B matrix. 99 | for (int i = 0; i < n; ++i) 100 | for (int k = i + 1; k < n; ++k) 101 | if (B[i][i] < B[k][i]) 102 | for (int j = 0; j <= n; ++j) { 103 | tmp = B[i][j]; 104 | B[i][j] = B[k][j]; 105 | B[k][j] = tmp; 106 | } 107 | 108 | // Performs the Gaussian elimination. 109 | // (1) Make all elements below the pivot equals to zero 110 | // or eliminate the variable. 111 | for (int i = 0; i= 0; --i) { 123 | a[i] = B[i][n]; // (1) 124 | for (int j = 0; j> myVideo::extractBackground(int startnum) { 8 | string s = outputpaths; 9 | string s_del = "rm -rf "; 10 | s_del += s + "/*.jpg"; 11 | const char* todel = s_del.c_str(); 12 | cout << s_del << "\n" << todel << endl; 13 | system(todel); 14 | int n = 0, cnum = 0, count0 = 0, count = startnum; 15 | 16 | vector> Fig; 17 | Mat frame_org, fgmask, fgmask_cali_rot, fgmask_cali, frame, fgimg, bgimg, contours, frame_cali, frame_cali_rot; 18 | GpuMat d_fgmask, d_fgimg, d_bgimg, d_bgimg_rot, d_fgmask_rot, d_frameb, d_frame, d_frame_cali, d_bg_cali; 19 | Stream stream; 20 | 21 | 22 | int GPU_work = getCudaEnabledDeviceCount(); 23 | if (GPU_work) 24 | cout << "The GPU is working with OPENCV" << endl; 25 | string fname = "s"; 26 | int Win = 300, Hin = 600; 27 | namedWindow(fname, WINDOW_NORMAL | WINDOW_KEEPRATIO); 28 | cv::moveWindow(fname, WINDOWSXP, WINDOWSYP); 29 | cv::resizeWindow(fname, Win, Hin); 30 | //cv::moveWindow(fname, WINDOWSXP + mov, WINDOWSYP); 31 | 32 | 33 | 34 | while (1) { 35 | cout << count << "--"; 36 | readvehframe(frame_org, count+1); 37 | //cv::flip(frame_org, frame_org, -1); 38 | if (count == startnum) { 39 | cali(frame_org); 40 | CalculateVideoDirection(Fig); 41 | } 42 | Calibrateframe(frame_org, frame_cali); 43 | 44 | Size ms = frame_org.size(); 45 | d_frame_cali.upload(frame_cali, stream); 46 | 47 | 48 | //TODO: load area to match in rotation 49 | //FindRotationParameter(d_frame, d_frame_cali, fgmask_cali_rot, count, cnum); 50 | float rotp[3] = { 0,0,0 }; 51 | 52 | RotateFrame(d_frame_cali, d_fgmask_rot, rotp, count); 53 | d_fgmask_rot.download(frame_cali_rot); 54 | 55 | 56 | if(count%100 == 0){ 57 | imshow(fname, frame_cali_rot); 58 | waitKey(10); 59 | } 60 | saveframe(frame_cali_rot, count, count0); 61 | 62 | //OperateOneFrame(fgmask_rot_cali, Fig, count, n, cnum); 63 | 64 | 65 | if (count == OPFRAMENUM + 200) 66 | break; 67 | count++; 68 | } 69 | //writerotparas(); 70 | 71 | cv::destroyAllWindows(); 72 | return Fig; 73 | } 74 | -------------------------------------------------------------------------------- /step2_training_img_generation/Project1/f2r.py: -------------------------------------------------------------------------------- 1 | import xlrd 2 | import matplotlib 3 | import matplotlib.pyplot as plt 4 | import numpy as np 5 | from mpl_toolkits import mplot3d 6 | import csv 7 | 8 | cor = { 9 | 0: 'b', 10 | 1: 'k', 11 | 2: 'r', 12 | 3: 'g', 13 | 4: 'c', 14 | 5: 'sienna', 15 | 6: 'grey', 16 | 7: 'y', 17 | 8: 'violet', 18 | 9: 'coral', 19 | 10:'darkblue', 20 | 11:'darkgreen', 21 | 12:'indigo', 22 | 13:'maroon', 23 | 14:'navy', 24 | 15:'olive', 25 | 16:'orangered', 26 | 17:'sienna', 27 | 18:'violet', 28 | } 29 | font = {'family' : 'serif', 'size' : 10} 30 | matplotlib.rc('font', **font) 31 | fig = plt.figure() 32 | ax = plt.axes(projection='3d') 33 | 34 | cormap = range(19) 35 | cormap = list(cormap) 36 | initi = 0 37 | fx = [] 38 | fy = [] 39 | ft = [] 40 | fl = [] 41 | def turn(x,y): 42 | pi = 3.1415926 43 | zH = 400.0; 44 | zpi = 3.14159; 45 | zal = 52.0; 46 | za = zal*pi / 180.0; 47 | zx = np.arctan(np.tan(za) - 404.49 / 400.0); 48 | zb = za - zx; 49 | zbl = zb * 180.0 /pi; 50 | zabl = zal + zbl; 51 | zr = zH*(np.tan(zb + za) - np.tan(za - zb)); 52 | zfocal = 27.0 / 100.0 / 0.3048 / 10.0; 53 | zhs = zfocal * np.tan(zb) * 2; 54 | zws = zhs * 4096.0 / 2160.0; 55 | zWS = zws; 56 | zHS = zhs; 57 | zXS0 = zWS / 2.0; 58 | zYS0 = zHS / 2.0; 59 | zFF = zfocal; 60 | zHH = zH; 61 | zAL = za; 62 | zBE = zb; 63 | 64 | WI = 4096; HI = 2160; 65 | XS0 = zWS / 2; YS0 = zHS / 2; 66 | xp = x*zWS / WI - XS0; 67 | yp = y*zHS / HI - YS0; 68 | cs = np.cos(zAL) 69 | ss = np.sin(zAL) 70 | t = zHH / cs; fy = zFF / cs; 71 | xm = 1 72 | ym = 1 73 | qy = int(t*yp/(fy + ss * yp) * ym); 74 | qx = int(xp * (t - ss * qy) / zFF * xm); 75 | mult = 2 76 | return qx*1.5,qy*3 77 | 78 | with open('lanenumbered.csv') as csvDataFile: 79 | csvReader = csv.reader(csvDataFile) 80 | tt = [] 81 | xx = [] 82 | yy = [] 83 | ln = -1 84 | cn = -1 85 | for row in csvReader: 86 | cnum = int(row[0]) 87 | time = float(row[1]) 88 | cx = int(row[2]) 89 | cy = int(row[3]) 90 | lane = int(row[4]) 91 | if ln == -1: 92 | ln = lane 93 | cn = cnum 94 | qx,qy = turn(cx,cy) 95 | tt.append(time/25) 96 | xx.append(qx) 97 | yy.append(qy) 98 | if lane != ln or cnum != cn: 99 | fx.append(xx) 100 | fy.append(yy) 101 | ft.append(tt) 102 | fl.append(ln) 103 | tt = [] 104 | xx = [] 105 | yy = [] 106 | ln = lane 107 | cn = cnum 108 | elif cy > 2160*0.45 and cx > 4096*0.4 and (cy-2160*0.45)*0.4 < 700 and (cx-4096*0.4)*0.4 < 800: 109 | tt.append(time/25) 110 | qx,qy = turn(cx,cy) 111 | xx.append(qx) 112 | yy.append(qy) 113 | 114 | for i in range(len(fx)): 115 | bb = 0 116 | for j in range(len(fx[i]) - 1): 117 | if len(fx[i]) < 100 or fl[i] == 0: 118 | bb = 1 119 | break 120 | if bb: 121 | continue 122 | ax.plot(fx[i],fy[i],ft[i],linewidth = 2, linestyle = '-', color = cor[fl[i]],label = 'lane ' + str(fl[i]) if fl[i] in cormap else "") 123 | if fl[i] in cormap: 124 | cormap.remove(fl[i]) 125 | plt.xlim(0,800) 126 | plt.ylim(0,800) 127 | plt.legend(loc=2) 128 | plt.xlabel("X (ft)") 129 | plt.ylabel("Y (ft)") 130 | ax.set_zlabel("T (s)") 131 | #plt.xticks(np.arange(0, 600, step=200)) 132 | #plt.yticks(np.arange(0, 300, step=100)) 133 | #ax.set_zticks(np.arange(0, 200, step=50)) 134 | #plt.zlabel("Y (ft)") 135 | plt.show() 136 | plt.savefig('lanenumberedtra1.png') 137 | -------------------------------------------------------------------------------- /step2_training_img_generation/Project1/file_operation_and_inits.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "FRAMES.h" 3 | 4 | FRAMES::FRAMES(string g_path) { 5 | gen_path = g_path; 6 | frame_path = gen_path + "imgs/"; 7 | output_data_path = gen_path + "output/"; 8 | temp_data_path = gen_path + "temp_data/"; 9 | temp_frame_path = gen_path + "temp_frame/"; 10 | 11 | if (!PF.exists_file(output_data_path)) 12 | PF.create_file(output_data_path); 13 | if (!PF.exists_file(temp_data_path)) 14 | PF.create_file(temp_data_path); 15 | if (!PF.exists_file(temp_frame_path)) 16 | PF.create_file(temp_frame_path); 17 | 18 | } 19 | 20 | -------------------------------------------------------------------------------- /step2_training_img_generation/Project1/frame_load.cpp: -------------------------------------------------------------------------------- 1 | #include "FRAMES.h" 2 | 3 | bool FRAMES::load_frame(int fnum, Mat &cur_frame) { 4 | string mat_path = frame_path + to_string(fnum) + ".jpg"; 5 | cur_frame = imread(mat_path); 6 | if (cur_frame.empty()) 7 | return false; 8 | else return true; 9 | } 10 | 11 | 12 | bool FRAMES::load_frame(string frame_path, Mat &cur_frame) { 13 | cur_frame = imread(frame_path); 14 | if (cur_frame.empty()) 15 | return false; 16 | else return true; 17 | } -------------------------------------------------------------------------------- /step2_training_img_generation/Project1/generate_learning_data.cpp: -------------------------------------------------------------------------------- 1 | #include "FRAMES.h" 2 | 3 | void create_learning_name_file(string imgpath, int freq){ 4 | string name_file_path = "C:/Users/dongfangzhao/Documents/OPENCV/darknet-master/build/darknet/x64/data/"; 5 | 6 | Public_func pf; 7 | string train_file = name_file_path + "train.txt"; 8 | string test_file = name_file_path + "test.txt"; 9 | ofstream trainfile, testfile; 10 | trainfile.open(train_file); 11 | testfile.open(test_file); 12 | string string0 = "data/obj/"; 13 | for (int i = 0; pf.exists_file(imgpath + to_string(i*freq) + ".jpg"); i++) { 14 | if (i % 5) 15 | trainfile << string0 + to_string(i) + ".jpg\n"; 16 | else 17 | testfile << string0 + to_string(i) + ".jpg\n"; 18 | } 19 | } 20 | 21 | std::vector objects_names_from_file(std::string const filename) { 22 | std::ifstream file(filename); 23 | std::vector file_lines; 24 | if (!file.is_open()) return file_lines; 25 | for (std::string line; getline(file, line);) file_lines.push_back(line); 26 | std::cout << "object names loaded \n"; 27 | return file_lines; 28 | } 29 | 30 | void FRAMES::generate_learning_data(string imgpath, string last_cnn_path, string cnn_path, string learning_img_path, int freq) { 31 | imgpath = gen_path + imgpath; 32 | create_learning_name_file(imgpath, freq); 33 | Public_func pf; 34 | string dele = "rm -r " + learning_img_path + "*"; 35 | //system(dele.c_str()); 36 | 37 | 38 | namedWindow("video", WINDOW_NORMAL | WINDOW_KEEPRATIO); 39 | cv::moveWindow("video", 0, 0); 40 | cv::resizeWindow("video", 480, 960); 41 | 42 | std::string genpath = "C:/Users/dongfangzhao/Documents/Opencv/darknet-master/build/darknet/x64"; 43 | std::string names_file = genpath + "/data/obj.names"; 44 | std::string cfg_file = genpath + "/yolov3_I75_left.cfg"; 45 | std::string weights_file = last_cnn_path + "yolov3_I75_left_last.weights"; 46 | 47 | LIB_API::Detector yolo(cfg_file, weights_file, 0); 48 | auto obj_names = objects_names_from_file(names_file); 49 | vector bbox0; 50 | for (int i = 0; pf.exists_file(imgpath + to_string(i*freq) + ".jpg"); i++) { 51 | string img_i_path = imgpath + to_string(i*freq) + ".jpg"; 52 | Mat img; 53 | int imgnum = i*freq; 54 | load_frame(img_i_path, img); 55 | string img_path = learning_img_path + to_string(i) + ".jpg"; 56 | cv::imwrite(img_path, img); 57 | 58 | string txt_path = learning_img_path + to_string(i) + ".txt"; 59 | cout << i << "#"; 60 | bbox0 = yolo.detect(img, 0.5, true); 61 | writedata(txt_path, img, bbox0); 62 | } 63 | } 64 | 65 | 66 | 67 | 68 | -------------------------------------------------------------------------------- /step2_training_img_generation/Project1/get_motion_parameters.cpp: -------------------------------------------------------------------------------- 1 | #include "FRAMES.h" 2 | 3 | void FRAMES::get_motion_parameters() { 4 | //when we want to restart the process, load parameters and find initial fnum is needed; 5 | Mat cur_frame, pre_frame, cur_depth; 6 | //138001 or 0 7 | for (int fnum = 138001; fnum < 216000 ; fnum+=30) { 8 | if (!load_frame(fnum, cur_frame)){ 9 | save_results(pre_frame, fnum); 10 | break; 11 | } 12 | if (circle_mat.empty()){ 13 | load_depth(fnum, cur_depth); 14 | load_circle(cur_frame, cur_depth); 15 | if (pre_frame.empty()) 16 | pre_frame = cur_frame.clone(); 17 | } 18 | else{ 19 | bool matched = match_circles(cur_frame);//cur 20 | if (!matched) { 21 | cout << "circles matched unsuccefully, reload started" << endl; 22 | load_circle(cur_frame, cur_depth); 23 | continue; 24 | } 25 | } 26 | 27 | if ((fnum-1) % (1200) == 0)// * 33 28 | save_results(cur_frame, fnum);//cur 29 | cout << fnum << "-"; 30 | 31 | } 32 | 33 | } 34 | 35 | 36 | 37 | bool FRAMES::load_depth(int fnum, Mat &cur_depth) { 38 | string mat_path = frame_path + to_string(fnum) + ".jpg"; 39 | cur_depth = imread(mat_path, IMREAD_ANYDEPTH); 40 | if (cur_depth.empty()) 41 | return false; 42 | else return true; 43 | } 44 | 45 | //save frame with circles and locations(framenum, position (x,y) and radius) 46 | void FRAMES::save_results(Mat cur_frame, int fnum) { 47 | for(int i = 0; i < vector_c_p.back().size(); i ++) 48 | cv::circle(cur_frame, vector_c_p.back()[i], vector_c_r.back()[i], Scalar(255, 0, 0), 5); 49 | string img_path = temp_frame_path + to_string(fnum) + ".jpg"; 50 | cv::imwrite(img_path, cur_frame); 51 | 52 | ofstream ofstr; 53 | string csv_path_s = temp_data_path + to_string(fnum) + ".csv"; 54 | ofstr.open(csv_path_s.c_str()); 55 | for (int i = 0; i < vector_c_p.size(); i++) { 56 | ofstr << i << ","; 57 | for (int j = 0; j < vector_c_p[i].size(); j++) { 58 | ofstr << vector_c_p[i][j].x << "," << 59 | vector_c_p[i][j].y << "," << vector_c_r[i][j]<<","; 60 | } 61 | ofstr << "\n"; 62 | } 63 | } 64 | 65 | 66 | void FRAMES::push_points() { 67 | vector_c_p.push_back(circle_point); 68 | vector_c_r.push_back(circle_rad); 69 | } -------------------------------------------------------------------------------- /step2_training_img_generation/Project1/lanenum.py: -------------------------------------------------------------------------------- 1 | from sklearn.cluster import DBSCAN 2 | import numpy as np 3 | import csv 4 | from sklearn import metrics 5 | from sklearn.datasets.samples_generator import make_blobs 6 | from sklearn.preprocessing import StandardScaler 7 | import matplotlib.pyplot as plt 8 | 9 | data = [] 10 | with open('output.csv') as mycsv: 11 | csvread = csv.reader(mycsv) 12 | for row in csvread: 13 | t = [i for i in row] 14 | data.append(t) 15 | data = np.array(data) 16 | for k, col in zip(unique_labels, colors): 17 | plt.plot(xy[:, 0], xy[:, 1], 'o', markerfacecolor=tuple(col), 18 | markeredgecolor='w', markersize=4) 19 | plt.title('Estimated number of clusters: %d' % n_clusters_) 20 | plt.show() 21 | -------------------------------------------------------------------------------- /step2_training_img_generation/Project1/lanenumber1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATS-Lab/Filed-Experiment-Data-HIGH_Sim/b5f9845bd61591fdcd3e9101a22e458ca3afae18/step2_training_img_generation/Project1/lanenumber1.jpg -------------------------------------------------------------------------------- /step2_training_img_generation/Project1/lanenumberedtra1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATS-Lab/Filed-Experiment-Data-HIGH_Sim/b5f9845bd61591fdcd3e9101a22e458ca3afae18/step2_training_img_generation/Project1/lanenumberedtra1.png -------------------------------------------------------------------------------- /step2_training_img_generation/Project1/lanenumberedtraroundabouts.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATS-Lab/Filed-Experiment-Data-HIGH_Sim/b5f9845bd61591fdcd3e9101a22e458ca3afae18/step2_training_img_generation/Project1/lanenumberedtraroundabouts.jpg -------------------------------------------------------------------------------- /step2_training_img_generation/Project1/load_and_store_2.cpp: -------------------------------------------------------------------------------- 1 | #include "myVideo.h" 2 | #include "Veh.h" 3 | 4 | void myVideo::store2(const char* file, vector> V) { 5 | if (edge.empty()) 6 | edge = addedge(); 7 | //_mkdir(file); 8 | string s = file; 9 | string s_del = "rm -r "; 10 | s_del += s + "/*"; 11 | const char* todel = s_del.c_str(); 12 | cout << s_del << "\n" << todel << endl; 13 | //system(todel); 14 | int n = 0, cnum = 0, count = 0; 15 | 16 | ofstream myfile; 17 | string f_csv = s; 18 | const char* f_cs = f_csv.c_str(); 19 | myfile.open(f_cs); 20 | myfile << "i->num" << "," << "i->cx" << "," << "i->cy" << "," 21 | << "i->rect.x" << "," << "i->rect.y" << "," << "i->rect.width" << "," 22 | << "i->rect.height" << "," << "i->state" << ",i->ep[i],,,,,,,," << "time" << "\n"; 23 | while (1) { 24 | if (count == V.size()) 25 | break; 26 | Point2f pf[2]; 27 | for (auto &i : V[count]) { 28 | if (i->state > 0 && i->rect.x >= edge.x) { 29 | myfile << i->num << "," << i->cx << "," << i->cy << "," << i->rect.x << "," << i->rect.y << "," << i->rect.width << "," 30 | << i->rect.height << "," << i->state; 31 | for (int p = 0; p < 4; p++) 32 | myfile << "," << i->ep[2 * p] << "," << i->ep[2 * p + 1]; 33 | myfile << "," << i->ct; 34 | myfile << "\n"; 35 | } 36 | 37 | } 38 | count++; 39 | } 40 | cv::destroyAllWindows(); 41 | } 42 | 43 | void myVideo::load2(const char* file, vector> &V) { 44 | 45 | int num, x, y, w, h, state, count; 46 | float cx, cy, ep[8]; 47 | string snum, sx, sy, sw, sh, sstate, scx, scy, sep[8], scount; 48 | ifstream datafile(file); 49 | getline(datafile, snum); 50 | int count0 = 0, precount = 0; 51 | vector V0; 52 | while (getline(datafile, snum, ',')) { 53 | if (snum.length() < 2) { 54 | getline(datafile, snum); 55 | continue; 56 | } 57 | getline(datafile, scx, ','); 58 | getline(datafile, scy, ','); 59 | getline(datafile, sx, ','); 60 | getline(datafile, sy, ','); 61 | getline(datafile, sw, ','); 62 | getline(datafile, sh, ','); 63 | getline(datafile, sstate, ','); 64 | for (int i = 0; i < 8; i++) { 65 | getline(datafile, sep[i], ','); 66 | ep[i] = stof(sep[i]); 67 | } 68 | getline(datafile, scount); 69 | num = stoi(snum); 70 | cx = stof(scx); 71 | cy = stof(scy); 72 | x = stoi(sx); 73 | y = stoi(sy); 74 | w = stoi(sw); 75 | h = stoi(sh); 76 | state = stoi(sstate); 77 | count = stoi(scount); 78 | 79 | Veh *vehi = new Veh(num, cx, cy, x, y, w, h, state, ep, count); 80 | for (auto vehs : V0) 81 | if (vehs->num == vehi->num && count == precount) 82 | goto cont; 83 | if ((vehi->rect & block).area() > 0 || vehi->x > edge.x+edge.width || vehi->rect.width > 70 || !inEdge(vehi)) 84 | continue; 85 | //copy here new. 86 | if (count > V.size() + 1) { 87 | V.push_back(V0); 88 | V0.clear(); 89 | for (int i = 0; i < count - V.size() - 1; i++) 90 | V.push_back(V0); 91 | } 92 | if (count == precount) { 93 | if (!V.empty()) 94 | for (int i = 0; i < V.back().size(); i++) { 95 | if (V.back()[i]->num == vehi->num) { 96 | vehi->pv = V.back()[i]; 97 | V.back()[i]->nv = vehi; 98 | break; 99 | } 100 | } 101 | V0.push_back(vehi); 102 | } 103 | else { 104 | V.push_back(V0); 105 | V0.clear(); 106 | if (!V.empty()) 107 | for (int i = 0; i < V.back().size(); i++) { 108 | if (V.back()[i]->num == vehi->num) { 109 | vehi->pv = V.back()[i]; 110 | V.back()[i]->nv = vehi; 111 | break; 112 | } 113 | } 114 | V0.push_back(vehi); 115 | precount = count; 116 | } 117 | cont: 118 | continue; 119 | } 120 | V.push_back(V0); 121 | } -------------------------------------------------------------------------------- /step2_training_img_generation/Project1/manually_add_train_data.cpp: -------------------------------------------------------------------------------- 1 | #include "FRAMES.h" 2 | 3 | Mat theframe; 4 | void Cross2(cv::Mat &frame, cv::Point point, cv::Scalar s) { 5 | int cl = 6; 6 | cv::Point a(cl, 0), b(-cl, 0), c(0, cl), d(0, -cl); 7 | line(frame, point + a, point + b, s, 2); 8 | line(frame, point + c, point + d, s, 2); 9 | } 10 | 11 | void Mouseclick3(int evt, int x, int y, int flags, void* edgepoints) { 12 | cv::Scalar s(0, 0, 255); 13 | std::vector* edgep = (std::vector*)edgepoints; 14 | if (evt == CV_EVENT_LBUTTONDOWN) { 15 | Mat frame = theframe.clone(); 16 | edgep->push_back(Point(x, y)); 17 | if (edgep->size() >= 1) { 18 | for (int i = 0; i * 2 + 1 < edgep->size(); i++) 19 | rectangle(frame, Rect((*edgep)[i * 2], (*edgep)[i * 2 + 1]), Scalar(255, 100, 0), 3); 20 | Cross2(frame, edgep->back(), Scalar(0, 0, 255)); 21 | } 22 | imshow("video", frame); 23 | } 24 | if (evt == CV_EVENT_RBUTTONDOWN) { 25 | Mat frame = theframe.clone(); 26 | edgep->pop_back(); 27 | if (edgep->size() >= 1) { 28 | for (int i = 0; i * 2 + 1 < edgep->size(); i++) 29 | rectangle(frame, Rect((*edgep)[i * 2], (*edgep)[i * 2 + 1]), Scalar(255, 100, 0), 3); 30 | Cross2(frame, edgep->back(), Scalar(0, 0, 255)); 31 | } 32 | imshow("video", frame); 33 | } 34 | } 35 | 36 | void FRAMES::writedata(string txt_path, Mat img, vector bbox0) { 37 | vector contourpoints; 38 | cv::setMouseCallback("video", Mouseclick3, &contourpoints); 39 | double w = img.cols, h = img.rows; 40 | ofstream txtfile; 41 | txtfile.open(txt_path); 42 | 43 | for (auto i : bbox0){ 44 | rectangle(img, Rect(i.x, i.y, i.w, i.h), Scalar(0, 0, 255), 2); 45 | //txtfile<< (i.h > truck_len) << " " << (i.x+i.w/2) / w << " " << (i.y+i.h/2) / h << " " << i.w / w << " " << i.h / h << "\n"; 46 | } 47 | theframe = img.clone(); 48 | imshow("video", img); 49 | reload: 50 | char c = waitKey(0); 51 | if (c == 'c') { 52 | if (contourpoints.size() % 2) { 53 | cout << "points not correct, plz reload. \n"; 54 | contourpoints.clear(); 55 | goto reload; 56 | } 57 | for (int i = 0; i < contourpoints.size(); i += 2) { 58 | Point2i a = contourpoints[i], b = contourpoints[i + 1]; 59 | Rect r(a, b); 60 | rectangle(img, Rect(r.x, r.y, r.width, r.height), Scalar(0, 255, 255), 2); 61 | txtfile << (r.height > truck_len) << " " << (r.x+ r.width/2) / w << " " << (r.y+ r.height/2) / h << " " << r.width / w << " " << r.height / h << "\n"; 62 | for (int j = 0; j < bbox0.size(); j++){ 63 | Rect box(bbox0[j].x, bbox0[j].y, bbox0[j].w, bbox0[j].h); 64 | if ((box&r).area()>0 || box.area() < 50) 65 | bbox0.erase(bbox0.begin() + j--); 66 | } 67 | 68 | } 69 | for (auto i : bbox0) { 70 | txtfile << (i.h > truck_len) << " " << (i.x + i.w / 2) / w << " " << (i.y + i.h / 2) / h << " " << i.w / w << " " << i.h / h << "\n"; 71 | } 72 | } 73 | } -------------------------------------------------------------------------------- /step2_training_img_generation/Project1/openh264-1.7.0-win64.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATS-Lab/Filed-Experiment-Data-HIGH_Sim/b5f9845bd61591fdcd3e9101a22e458ca3afae18/step2_training_img_generation/Project1/openh264-1.7.0-win64.dll -------------------------------------------------------------------------------- /step3_lanedetection/Project1/Angle_test.cpp: -------------------------------------------------------------------------------- 1 | #include "FRAMES.h" 2 | 3 | void FRAMES::test_angle(string path, bool st1) { 4 | vector center; 5 | vector angle; 6 | vector fnum; 7 | load_angle_parameters(gen_path + path, fnum, center, angle); 8 | Mat frame, frame_shifted, frame_rotated; 9 | Point c0 = center[0]; 10 | for (int i = 0; i < center.size(); i+=1000) { 11 | load_frame(fnum[i], frame); 12 | int shiftx = center[i].x - c0.x; 13 | int shifty = center[i].y - c0.y; 14 | //Mat rotate_M = getRotationMatrix2D(c0, (angle[i] - PI / 2)*180.0/PI, 1); 15 | //Mat shift_M = (Mat_(2, 3) << 1, 0, -shiftx, 0, 1, -shifty); 16 | //cv::warpAffine(frame, frame_shifted, shift_M, frame.size()); 17 | Mat rotate_M = getRotationMatrix2D(center[i], (angle[i] - PI / 2)*180.0 / PI, 1); 18 | //for left 1st video, shift 8190-5600=2500; 19 | Mat shift_M = (Mat_(2, 3) << 0, 0, -shiftx, 0, 0, -shifty+2500*st1); 20 | rotate_M = rotate_M + shift_M; 21 | cv::warpAffine(frame, frame_rotated, rotate_M, frame.size()); 22 | string img_path = "N:/LEFT/mid_processing/rotated/" + to_string(fnum[i]) + ".jpg"; 23 | cv::imwrite(img_path, frame_rotated); 24 | 25 | } 26 | 27 | } 28 | void FRAMES::load_angle_parameters(string path, vector &fnum, vector ¢er, vector &angle) { 29 | ifstream ifstr(path.c_str()); 30 | string snum; 31 | while (getline(ifstr, snum, ',')) { 32 | fnum.push_back(stoi(snum)); 33 | int i = 0, x = 0, y = 0; 34 | string xs, ys, as; 35 | float a; 36 | getline(ifstr, xs, ','); 37 | x = stoi(xs); 38 | getline(ifstr, ys, ','); 39 | y = stoi(ys); 40 | getline(ifstr, as); 41 | a = stof(as); 42 | center.push_back(Point(x, y)); 43 | angle.push_back(a); 44 | } 45 | } 46 | 47 | -------------------------------------------------------------------------------- /step3_lanedetection/Project1/Calibration.cpp: -------------------------------------------------------------------------------- 1 | #include "myVideo.h" 2 | #include "Veh.h" 3 | void myVideo::cali(Mat frame) { 4 | string file = parapaths + "/cali.txt"; 5 | Mat frame_rted; 6 | if (exists_file(file)) { 7 | ifstream input(file); 8 | string number; 9 | getline(input, number); 10 | AL = atof(number.c_str()); 11 | Mat frame_rted; 12 | Calibration(frame, frame_rted); 13 | //Showframe("video", frame_rted, 5000); 14 | } 15 | else { 16 | namedWindow("video", WINDOW_NORMAL | WINDOW_KEEPRATIO); 17 | cv::moveWindow("video", WINDOWSXP, 0); 18 | cv::resizeWindow("video", 800, 1800); 19 | 20 | AL = 0; 21 | while (1) { 22 | //Showframe("video", frame); 23 | Calibration(frame, frame_rted); 24 | for (int i = 50; i < frame_rted.cols - 100; i += 100) 25 | cv::line(frame_rted, Point(i, 20), Point(i, frame_rted.rows - 20), Scalar(0, 0, 200), 3); 26 | cout << "click on t to set the angle\n"; 27 | Showframe("video", frame_rted, 400, 0, 1800,800); 28 | char c = (char)waitKey(3000);// 29 | switch (c) { 30 | case 'z': 31 | AL -= 5 * PI / 180; 32 | break; 33 | case 'c': 34 | AL += 5 * PI / 180; 35 | break; 36 | case 'q': 37 | AL -= 2 * PI / 180; 38 | break; 39 | case 'e': 40 | AL += 2 * PI / 180; 41 | break; 42 | case 'a': 43 | AL -= 0.5 * PI / 180; 44 | break; 45 | case 'd': 46 | AL += 0.5 * PI / 180; 47 | break; 48 | } 49 | 50 | if (c == 't') 51 | break; 52 | 53 | } 54 | std::ofstream outfile(file); 55 | outfile << to_string(AL) << std::endl; 56 | outfile.close(); 57 | } 58 | Calibration(frame, frame_rted); 59 | background = frame_rted.clone(); 60 | destroyWindow("video"); 61 | //Showframe("video", edgebackground); 62 | //destroyAllWindows(); 63 | } 64 | 65 | void myVideo::Calibration(Mat frame, Mat &dst) { 66 | getangle(); 67 | 68 | float Zy = (FF*cos(AL) + HS / HI * sin(AL) * (HI - HI / 2)); 69 | float yrmax = HS / HI * (HI / 2) * HH / cos(AL) / Zy; 70 | Zy = (FF*cos(AL) - HS / HI * sin(AL) * (HI / 2)); 71 | float yrmin = -HS / HI * (HI / 2.0) * HH / cos(AL) / Zy; 72 | float Z = -yrmin * sin(AL) + HH / cos(AL); 73 | float xrmin = -Z / FF * WS / WI * WI / 2; 74 | float xrmax = Z / FF * WS / WI * WI / 2; 75 | float xrr = xrmax - xrmin + 1; 76 | float yrr = yrmax - yrmin + 1; 77 | float yr, xr, yri, xri; 78 | int x_, y_; 79 | 80 | cv::Mat_ mapx_32f((int)HI, (int)WI), mapy_32f((int)HI, (int)WI); 81 | 82 | //Mat dst2((int)HI,(int)WI, CV_8UC3, Scalar(0, 0, 0)); 83 | for (int y = 0; y pt(3, 1); 90 | pt(0) = x; 91 | pt(1) = y; 92 | pt(2) = 1; 93 | 94 | yri = y; 95 | xri = x; 96 | yr = yrr / HI * yri + yrmin; 97 | xr = xrr / WI * xri + xrmin; 98 | Zy = (FF*cos(AL) * yr / (-yr*sin(AL) + HH / cos(AL))); 99 | 100 | Z = -yr*sin(AL) + HH / cos(AL); 101 | 102 | pt(1) = y_ = HI / 2 + HI / HS * Zy; 103 | pt(0) = x_ = FF / Z * WI / WS * (xr)+WI / 2; 104 | 105 | pt /= pt(2); 106 | buff_mapx[x] = (int)pt(0); 107 | buff_mapy[x] = (int)pt(1); 108 | //Vec3b color = frame_org.at(Point(x, y)); 109 | //dst2.at(Point(x_, y_)) = color; 110 | } 111 | } 112 | cv::convertMaps(mapx_32f, mapy_32f, map1_16u, map2_16u, CV_32FC2, true); 113 | //frame_r = dst2.clone(); 114 | cv::remap(frame, dst, map1_16u, map2_16u, CV_INTER_LINEAR, BORDER_CONSTANT, Scalar(0, 0, 0)); 115 | } 116 | 117 | void myVideo::Calibrateframe(Mat fgmask_rot, Mat &fgmask_cali) { 118 | cv::remap(fgmask_rot, fgmask_cali, map1_16u, map2_16u, CV_INTER_LINEAR, BORDER_CONSTANT, Scalar(0, 0, 0)); 119 | } 120 | 121 | void myVideo::Calibrateframe(GpuMat fgmask, GpuMat &fgmask_cali) { 122 | Mat m1, m2; 123 | fgmask.download(m1); 124 | cv::remap(m1, m2, map1_16u, map2_16u, CV_INTER_LINEAR, BORDER_CONSTANT, Scalar(0, 0, 0)); 125 | 126 | fgmask_cali.upload(m2); 127 | } 128 | 129 | void myVideo::getangle() { 130 | float H, al, a, pi, x, b, bl, abl, r, focal, hs, ws; 131 | H = 400.0; 132 | pi = 3.14159; 133 | FF = focal = 8.8 / 1000.0*0.3048; 134 | HH = H; 135 | WS = 13.2*0.3048 / 1000.0; 136 | HS = 7.425*0.3048 / 1000.0; 137 | } -------------------------------------------------------------------------------- /step3_lanedetection/Project1/D2lanenum.m: -------------------------------------------------------------------------------- 1 | color = rand(30,3) -------------------------------------------------------------------------------- /step3_lanedetection/Project1/Lanefunctions.cpp: -------------------------------------------------------------------------------- 1 | #include "FRAMES.h" 2 | #include "Poly.h" 3 | #include "Lanes.h" 4 | #include "Public_functions.h" 5 | 6 | void LaneS::loadlanenum() { 7 | string file = path + "/lanenum.txt"; 8 | string lanenum_s; 9 | char c; 10 | Public_func PB; 11 | if (PB.exists_file(file)) { 12 | ifstream input(file); 13 | getline(input, lanenum_s); 14 | lanenum = atoi(lanenum_s.c_str()); 15 | } 16 | else { 17 | cout << "lanenumtxt not exist.\n"; 18 | getline(cin, lanenum_s); 19 | lanenum = atoi(lanenum_s.c_str()); 20 | std::ofstream outfile(file); 21 | outfile << to_string(lanenum) << std::endl; 22 | outfile.close(); 23 | } 24 | for (int i = 0; i < lanenum; i++) { 25 | vector vec; 26 | vehpointx.push_back(vec); 27 | vehpointy.push_back(vec); 28 | } 29 | } 30 | 31 | void LaneS::loadorderandpoints() { 32 | 33 | loadlanenum(); 34 | order = 2; 35 | string file = path + "/lanes.txt"; 36 | Public_func PB; 37 | if (PB.exists_file(file)) {// 38 | ifstream input(file); 39 | for (int j = 0; j < lanenum; j++) { 40 | 41 | vector coefs; 42 | string ys; 43 | float y; 44 | int i = 0; 45 | while (i++ < order + 1) { 46 | getline(input, ys); 47 | y = atof(ys.c_str()); 48 | coefs.push_back(y); 49 | } 50 | fy.push_back(coefs); 51 | } 52 | } 53 | else { 54 | cout << "file not found\n"; 55 | } 56 | 57 | } 58 | 59 | void LaneS::findlanenum(Veh * vehi) { 60 | float x, y; 61 | int cutH = 2000; 62 | if (vehi->pv != NULL && vehi->y > cutH){ 63 | vehi->lnum = vehi->pv->lnum; 64 | return; 65 | } 66 | 67 | x = vehi->cx; 68 | y = vehi->cy; 69 | 70 | 71 | vector xdet;; 72 | for (int i = 0; i < lanenum; i++) { 73 | float xi = fy[i][0] + fy[i][1] * y + fy[i][2] * y*y; 74 | xdet.push_back(abs(x - xi)); 75 | } 76 | int output = 0; 77 | float dis0 = 10; 78 | float dis = dis0; 79 | for (int i = 0; i < lanenum; i++) { 80 | if (xdet[i] < dis) { 81 | output = i; 82 | dis = xdet[i]; 83 | } 84 | } 85 | if(dis < dis0){ 86 | vehi->lnum = output; 87 | if(dis < 3){ 88 | vehpointx[output].push_back(x); 89 | vehpointy[output].push_back(y); 90 | } 91 | } 92 | else { 93 | vehi->lnum = -1; 94 | } 95 | 96 | } 97 | 98 | LaneS::LaneS() { 99 | 100 | } 101 | 102 | LaneS::~LaneS() { 103 | 104 | } 105 | 106 | void LaneS::regresslanes() { 107 | for (int i = 0; i < lanenum; i++) { 108 | if (vehpointx[i].size() < 200 || vehpointy[i].size() != vehpointx[i].size()) 109 | continue; 110 | PolynomialRegression poly; 111 | poly.fitIt(vehpointy[i], vehpointx[i], order, fy[i]); 112 | } 113 | } 114 | 115 | void LaneS::showlane(Mat frame) { 116 | string windowname = "Lanes"; 117 | namedWindow(windowname, WINDOW_NORMAL | WINDOW_KEEPRATIO); 118 | cv::moveWindow(windowname, 5, 5); 119 | cv::resizeWindow(windowname, 384 * 4, 216 * 4); 120 | imshow(windowname, frame); 121 | for(float y = 0; y < 8190; y = y + 1.0) 122 | for (int i = 0; i < lanenum; i++){ 123 | float xi = fy[i][0] + fy[i][1] * y + fy[i][2] * y*y; 124 | cv::circle(frame, Point(xi, y), 2, Scalar(0, 0, 60 * i), 1); 125 | } 126 | imshow(windowname, frame); 127 | waitKey(200); 128 | destroyWindow(windowname); 129 | } -------------------------------------------------------------------------------- /step3_lanedetection/Project1/Lanes.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "FRAMES.h" 3 | #include "Poly.h" 4 | 5 | class LaneS { 6 | public: 7 | vector> vehpointx; 8 | vector> vehpointy; 9 | 10 | int order; 11 | int lanenum; 12 | float time0; 13 | vector> fy; 14 | Mat frame_0; 15 | string path; 16 | 17 | 18 | public: 19 | LaneS(); 20 | ~LaneS(); 21 | 22 | void loadlanenum(); 23 | void loadorderandpoints(); 24 | void regresslanes(); 25 | void findlanenum(Veh * v); 26 | void showlane(Mat frame); 27 | }; 28 | -------------------------------------------------------------------------------- /step3_lanedetection/Project1/Load_circle_parameters.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "FRAMES.h" 3 | 4 | void FRAMES::load_circle_parameters(string path, vector &fnum, vector> ¢ers, int sti, int stj, int detect) { 5 | ifstream ifstr(path.c_str()); 6 | string snum; 7 | getline(ifstr, snum, ','); 8 | while (1) { 9 | if (snum.size() < 1) 10 | break; 11 | fnum.push_back(stoi(snum)); 12 | int i = 0, x = 0, y = 0, r = 0; 13 | string xs, ys, rs; 14 | vector center; 15 | while (1) { 16 | getline(ifstr, xs, ','); 17 | if (xs[0] == '\n') { 18 | centers.push_back(center); 19 | xs.erase(xs.begin()); 20 | snum = xs; 21 | break; 22 | } 23 | x = stoi(xs); 24 | getline(ifstr, ys, ','); 25 | y = stoi(ys); 26 | getline(ifstr, rs, ','); 27 | r = stof(rs); 28 | center.push_back(Point(x, y)); 29 | } 30 | } 31 | 32 | for (auto &ca : centers) 33 | for (auto & ci : ca) 34 | ci = ci + Point(IMGW / 2, IMGH / 2); 35 | if (sti == 2 && stj == 1) 36 | for (auto &ca : centers) { 37 | if (ca.size() == 4) 38 | ca.erase(ca.begin() + 2); 39 | } 40 | if(detect == 1){ 41 | vector fnum2; 42 | vector> center2; 43 | for (int i = 0, ni = fnum[0]; i < fnum.size(); ni++) { 44 | int n = fnum[i]; 45 | if (ni == n) { 46 | fnum2.push_back(ni); 47 | center2.push_back(centers[i]); 48 | i++; 49 | } 50 | else { 51 | fnum2.push_back(ni); 52 | vector pi = centers[i-1]; 53 | double k = ni - fnum[i-1], kn = fnum[i] - fnum[i-1]; 54 | for (int j = 0; j < pi.size(); j++) { 55 | pi[j].x = double(centers[i][j].x - pi[j].x) * k / kn + pi[j].x; 56 | pi[j].y = double(centers[i][j].y - pi[j].y) * k / kn + pi[j].y; 57 | } 58 | center2.push_back(pi); 59 | } 60 | } 61 | fnum = fnum2; 62 | centers = center2; 63 | } 64 | } 65 | 66 | -------------------------------------------------------------------------------- /step3_lanedetection/Project1/Load_cut_edge.cpp: -------------------------------------------------------------------------------- 1 | #include "FRAMES.h" 2 | #include "Poly.h" 3 | 4 | void FRAMES::load_edge_function(vector> center, string edge_path, vector fnum, vector &output) { 5 | Public_func pf; 6 | Mat frame, frame_shifted, frame_rotated, frame_merged; 7 | vector points; 8 | Size2i resized(IMGW*2*ratio_frames, IMGH * 2 * ratio_frames); 9 | Mat affine_M; 10 | if (pf.exists_file(edge_path)) { 11 | load_edge_from_file(edge_path, points); 12 | } 13 | else { 14 | for (int i = 0; i < center.size(); i += 4000) { 15 | load_frame(fnum[i], frame); 16 | Size large_size = frame.size() * 2; 17 | if (center[0].size() > 3) 18 | affine_M = findHomography(center[i], center[0]); 19 | else{ 20 | Point2f a0[3] = {center[0][0], center[0][1], center[0][2]}; 21 | Point2f a1[3] = { center[i][0], center[i][1], center[i][2] }; 22 | affine_M = getAffineTransform(a1, a0); 23 | } 24 | Mat shift_M = (Mat_(2, 3) << 1, 0, large_size.width / 4, 0, 1, large_size.height / 4); 25 | cv::warpAffine(frame, frame_shifted, shift_M, large_size); 26 | if (center[0].size() > 3) 27 | cv::warpPerspective(frame_shifted, frame_rotated, affine_M, large_size); 28 | else 29 | cv::warpAffine(frame_shifted, frame_rotated, affine_M, large_size); 30 | //show_frame(frame_rotated, 10000); 31 | if (!frame_merged.empty()) 32 | cv::addWeighted(frame_merged, 0.5, frame_rotated, 0.5, 0, frame_merged); 33 | else 34 | frame_merged = frame_rotated.clone(); 35 | } 36 | cv::resize(frame_merged, frame_merged, resized); 37 | load_edge_from_frame(frame_merged, points); 38 | ofstream outcsv(edge_path.c_str()); 39 | for (auto p : points) 40 | outcsv << p.x << "," << p.y << "\n"; 41 | } 42 | 43 | PolynomialRegression poly; 44 | vector vy, vx, f; 45 | for (auto p : points) { 46 | vy.push_back(long double(p.y)); 47 | vx.push_back(long double(p.x)); 48 | } 49 | poly.fitIt(vy, vx, EDGE_FIT_ORDER, f); 50 | output.clear(); 51 | for (double y = 0; y < resized.height; y = y + 1.0) { 52 | double xi = 0; 53 | for (int j = 0; j < EDGE_FIT_ORDER + 1; j++) 54 | xi += f[j] * pow(y, j); 55 | output.push_back(xi); 56 | } 57 | } 58 | 59 | void FRAMES::load_edge_from_file(string edge_path, vector &points) { 60 | ifstream input(edge_path.c_str()); 61 | string xs, ys; 62 | float x, y; 63 | while (getline(input, xs, ',')) { 64 | x = stoi(xs); 65 | getline(input, ys); 66 | y = stof(ys); 67 | points.push_back(Point(x, y)); 68 | } 69 | } 70 | 71 | Mat frame_draw; 72 | void Cross(cv::Mat frame, cv::Point point, cv::Scalar s) { 73 | int cl = 10; 74 | cv::Point a(cl, 0), b(-cl, 0), c(0, cl), d(0, -cl); 75 | line(frame, point + a, point + b, s, 4); 76 | line(frame, point + c, point + d, s, 4); 77 | } 78 | 79 | void Mouseclick(int evt, int x, int y, int flags, void* edgepoints) { 80 | cv::Scalar s(0, 0, 255); 81 | std::vector* edgep = (std::vector*)edgepoints; 82 | if (evt == CV_EVENT_LBUTTONDOWN) { 83 | edgep->push_back(Point(x, y)); 84 | if (edgep->size() >= 1) { 85 | Point tem = edgep->back(); 86 | Cross(frame_draw, tem, s); 87 | } 88 | imshow("Merged_frame", frame_draw); 89 | } 90 | } 91 | void FRAMES::load_edge_from_frame(Mat frame_merged, vector &points){ 92 | frame_draw = frame_merged.clone(); 93 | string windowname = "Merged_frame"; 94 | namedWindow(windowname, WINDOW_NORMAL | WINDOW_KEEPRATIO); 95 | cv::moveWindow(windowname, 0, 0); 96 | cv::resizeWindow(windowname, 1820, 1000); 97 | 98 | cv::setMouseCallback(windowname, Mouseclick, &points); 99 | 100 | imshow(windowname, frame_draw); 101 | reload: 102 | points.clear(); 103 | char c = waitKey(0); 104 | if (c == 'c') { 105 | if (points.size() > 3) { 106 | destroyWindow(windowname); 107 | return; 108 | } 109 | else { 110 | cout << "please enter more points"; 111 | goto reload; 112 | } 113 | } 114 | else 115 | goto reload; 116 | 117 | } -------------------------------------------------------------------------------- /step3_lanedetection/Project1/Main.cpp: -------------------------------------------------------------------------------- 1 | #include "FRAMES.h" 2 | #include 3 | 4 | int main() 5 | { 6 | string gen_path[3] = {"N:/LEFT/","L:/MIDDLE/","N:/RIGHT/" }; 7 | 8 | //1. 9 | //FRAMES.get_motion_parameters(); 10 | //2. 11 | //string path = "verify/to_show.csv"; 12 | //FRAMES.test_circles(path); 13 | //3.test angle paramters and generate rotated imgs. 14 | // 15 | 16 | string path[2] = { "temp_data/117700.csv", "temp_data/206700.csv" }; 17 | int order[3] = { 1,9,2 }; 18 | //FRAMES.test_angle(path[1]); 19 | //4. generate learning imgs, with W nad H; 20 | int learn_w = 480, learn_h = 800; 21 | string cnn_path = "C:/Users/dongfangzhao/Documents/OPENCV/darknet-master/build/darknet/x64/backup/yolov3_I75_left_last.weights"; 22 | for(int i = 0, imgnum = 0; i < 3; i ++){ 23 | for(int j = 1; j < 2; j ++){ 24 | FRAMES FRAMES(gen_path[i]); 25 | FRAMES.ratio_frames = FRAMES.frame_ratios[j][i]; 26 | FRAMES.EDGE_FIT_ORDER = order[i]; 27 | FRAMES.iffirst = j; 28 | cout << "\n" << i << j <<":"<< endl; 29 | FRAMES.detect_lane_marker(i, j, path[j]); 30 | return 1; 31 | } 32 | } 33 | 34 | //5. generate learning data 35 | /* 36 | FRAMES FRAMES(gen_path[0]); 37 | string imgpath = "learning_imgs/imgs/"; 38 | string darknet = "C:/Users/dongfangzhao/Documents/OPENCV/darknet-master/build/darknet/x64/"; 39 | string gen_cnn_path = darknet + "backup/I75/all"; 40 | string learning_img_path = darknet + "data/obj/"; 41 | int interval = 1; 42 | string last_cnn_path = gen_cnn_path + "_" + to_string(interval * 10) + "/"; 43 | string cnn_path = gen_cnn_path + "_" + to_string(interval) + "/"; 44 | 45 | FRAMES.generate_learning_data(imgpath, last_cnn_path, cnn_path, learning_img_path, interval); 46 | */ 47 | return 0; 48 | } 49 | -------------------------------------------------------------------------------- /step3_lanedetection/Project1/NOTE.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Documentation: 3 | 0. preprocessing 4 | a. revise rotation algorithm, make it work in 2s frames; 5 | b. revise calibration part... 6 | c. cut the processed image into edge, (save it, optional); 7 | 8 | 1. revise the identification part; 9 | a. resize the image to imagesize*1.2; 10 | b. cut image into 5 pieces and detect them; 11 | c. process results with Rect parameters, coordinate/1.2, and return results; 12 | 13 | 2. tet performance. 14 | 15 | */ 16 | -------------------------------------------------------------------------------- /step3_lanedetection/Project1/OperateOneFrame.cpp: -------------------------------------------------------------------------------- 1 | #include "myVideo.h" 2 | #include "Veh.h" 3 | 4 | void myVideo::OperateOneFrame(Mat fgmask_rot, vector> &Fig, int count, int &n, int cnum) { 5 | vector> contours; 6 | vector hierarchy; 7 | findContours(fgmask_rot, contours, hierarchy, RETR_CCOMP, CHAIN_APPROX_SIMPLE); 8 | vector Fig0; 9 | int hi; 10 | for (int i = 0; i < int(contours.size()); i++) { 11 | if (hierarchy[i][3] < 0) { 12 | if (contours[i].size() < 10) 13 | continue; 14 | float ty = static_cast(contourArea(cv::Mat(contours[i]))); 15 | Moments mu = moments(contours[i], false); 16 | Point2f mc = Point2f(static_cast(mu.m10 / mu.m00), static_cast(mu.m01 / mu.m00)); 17 | mc = Point2f(max(0.0f, mc.x), max(0.0f, mc.y)); 18 | Rect rect = boundingRect(contours[i]); 19 | RotatedRect rt = minAreaRect(contours[i]); 20 | float angle = rt.angle; 21 | Veh* v = new Veh(n++, ty, rt, cnum, count, angle); 22 | Point2f pf[4]; 23 | rt.points(pf); 24 | for (int k = 0; k < 4; k++) { 25 | v->ep[2 * k] = pf[k].x; 26 | v->ep[2 * k + 1] = pf[k].y; 27 | if (cv::norm(pf[k] - pf[(k + 1) % 3]) > 1500) 28 | ty = 10; 29 | } 30 | if (ty < MINCONAREA || ty > MAXCONAREA) 31 | continue; 32 | //if (count > 160) 33 | //rectangle(fgmask_rot, rect, Scalar(255), 10, 8, 0); 34 | Fig0.push_back(v); 35 | } 36 | } 37 | if (count == 0) { 38 | Fig.push_back(Fig0); 39 | } 40 | else { 41 | vector Fb = Fig.back(); 42 | double disu = 50.0; 43 | double d1, d2; 44 | int k; 45 | for (int i = 0; i < int(Fig0.size()); i++) { 46 | d2 = disu; 47 | for (int j = 0; j < int(Fb.size()); j++) { 48 | d1 = Vehdis(Fig0[i], Fb[j]); 49 | if (d1 < d2) { 50 | d2 = d1; 51 | k = j; 52 | } 53 | } 54 | int newv = 1; 55 | if (d2 < disu) { 56 | if (Fb[k]->nv) { 57 | float a1, a2, a3; 58 | a1 = Fb[k]->vtype; 59 | a2 = Fb[k]->nv->vtype; 60 | a3 = Fig0[i]->vtype; 61 | float area1, area2; 62 | area1 = (a1 - a2) * (a1 - a2); 63 | area2 = (a1 - a3) * (a1 - a3); 64 | if (area1 < area2) 65 | newv = 0; 66 | else 67 | newv = 2; 68 | } 69 | if (newv) { 70 | if (newv == 2) { 71 | Fb[k]->nv->pv = NULL; 72 | //Fb[k]->nv = NULL; 73 | } 74 | Fig0[i]->pv = Fb[k]; 75 | Fb[k]->nv = Fig0[i]; 76 | Fig0[i]->num = Fb[k]->num; 77 | Fig0[i]->vtype = (Fb[k]->vtype + Fig0[i]->vtype) / 2; 78 | Fb[k]->vtype = Fig0[i]->vtype; 79 | if (!Fig0[i]->state) { 80 | Veh* b; 81 | b = Fig0[i]; 82 | b->forcast[0] = 2 * b->cx - b->pv->cx; 83 | b->forcast[1] = 2 * b->cy - b->pv->cy; 84 | b->state = 1; 85 | } 86 | } 87 | } 88 | } 89 | Fig.push_back(Fig0); 90 | Fig0.clear(); 91 | } 92 | } 93 | 94 | void myVideo::RefreshOutput(vector> &Fig) { 95 | for (int i = 0; i < Fig.size(); i ++) { 96 | for (int j = 0; j < Fig[i].size(); j++) { 97 | Veh* v; 98 | v = Fig[i][j]; 99 | if (v->time == 0) { 100 | int ti = 1; 101 | while (v->nv) { 102 | v = v->nv; 103 | ti++; 104 | } 105 | while (v) { 106 | v->time = ti; 107 | v = v->pv; 108 | } 109 | } 110 | } 111 | } 112 | for (auto &i : Fig) { 113 | for (int j = 0; j < int(i.size()); j++) { 114 | if (i[j]->nv) 115 | if (i[j]->time != i[j]->nv->time) 116 | cout << "wrong"; 117 | else if (i[j]->nv->pv == NULL) 118 | cout << "nopv"; 119 | else if (i[j]->nv->num != i[j]->num) 120 | cout << "number not equal"; 121 | if (i[j]->time < MINFRAMELEN) 122 | i.erase(i.begin() + j--); 123 | } 124 | } 125 | } -------------------------------------------------------------------------------- /step3_lanedetection/Project1/POINTS.cpp: -------------------------------------------------------------------------------- 1 | #include "myVideo.h" 2 | #include "Veh.h" 3 | 4 | void myVideo::Points(vector > V) { 5 | Mat img(YPIXEL, XPIXEL, CV_8UC3, Scalar(255, 255, 255)); 6 | int num = 0; 7 | std::ofstream mycsv; 8 | mycsv.open("output.csv"); 9 | for (auto &i : V) { 10 | for (auto &j : i) { 11 | mycsv << j->num << "," << j->glotime << "," << j->cx << "," << j->cy << "," << j->lnum << "," << j->vtype << ",\n"; 12 | num++; 13 | //if (num)// % 2 == 0) 14 | circle(img, cv::Point2f(j->cx, j->cy), 2, Scalar(0, 0, 0), 2, 8, 0); 15 | circle(background, cv::Point2f(j->cx, j->cy), 2, Scalar(0, 0, 255), 2, 8, 0); 16 | } 17 | } 18 | mycsv.close(); 19 | namedWindow("Points", WINDOW_NORMAL | WINDOW_KEEPRATIO); 20 | cv::moveWindow("Points", WINDOWSXP, WINDOWSYP); 21 | cv::resizeWindow("Points", WINDOWSW, WINDOWSH); 22 | imshow("Points", img); // Show our image inside it. 23 | imwrite("a.jpg", img); 24 | imwrite("b.jpg", background); 25 | waitKey(3000); 26 | cv::destroyAllWindows(); 27 | } 28 | -------------------------------------------------------------------------------- /step3_lanedetection/Project1/Plot.cpp: -------------------------------------------------------------------------------- 1 | #include "myVideo.h" 2 | #include "Veh.h" 3 | #include 4 | 5 | void Plot(vector > V){ 6 | ofstream out("zline.xls"); 7 | if (!out) { 8 | cout << "open lxs unsuccessful"; 9 | throw; 10 | } 11 | for (auto &i : V){ 12 | for (auto &j : i){ 13 | out << j->num << " \t" << j->glotime << " \t" << j->x << " \t" << j->y\ 14 | << " \t" << j->spd << " \t" << j->acc << " \t" << j->lnum << endl; 15 | } 16 | } 17 | out.close(); 18 | 19 | } 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /step3_lanedetection/Project1/Poly.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #ifndef _POLYNOMIAL_REGRESSION_H 3 | #define _POLYNOMIAL_REGRESSION_H __POLYNOMIAL_REGRESSION_H 4 | 5 | /** 6 | * PURPOSE: 7 | * 8 | * Polynomial Regression aims to fit a non-linear relationship to a set of 9 | * points. It approximates this by solving a series of linear equations using 10 | * a least-squares approach. 11 | * 12 | * We can model the expected value y as an nth degree polynomial, yielding 13 | * the general polynomial regression model: 14 | * 15 | * y = a0 + a1 * x + a2 * x^2 + ... + an * x^n 16 | * @author Chris Engelsma 17 | */ 18 | 19 | #include 20 | #include 21 | 22 | template 23 | class PolynomialRegression { 24 | public: 25 | 26 | PolynomialRegression(); 27 | virtual ~PolynomialRegression() {}; 28 | 29 | bool fitIt( 30 | const std::vector & x, 31 | const std::vector & y, 32 | const int & order, 33 | std::vector & coeffs); 34 | }; 35 | 36 | template 37 | PolynomialRegression::PolynomialRegression() {}; 38 | 39 | template 40 | bool PolynomialRegression::fitIt( 41 | const std::vector & x, 42 | const std::vector & y, 43 | const int & order, 44 | std::vector & coeffs) 45 | { 46 | // The size of xValues and yValues should be same 47 | if (x.size() != y.size()) { 48 | throw std::runtime_error("The size of x & y arrays are different"); 49 | return false; 50 | } 51 | // The size of xValues and yValues cannot be 0, should not happen 52 | if (x.size() == 0 || y.size() == 0) { 53 | throw std::runtime_error("The size of x or y arrays is 0"); 54 | return false; 55 | } 56 | 57 | size_t N = x.size(); 58 | int n = order; 59 | int np1 = n + 1; 60 | int np2 = n + 2; 61 | int tnp1 = 2 * n + 1; 62 | TYPE tmp; 63 | 64 | // X = vector that stores values of sigma(xi^2n) 65 | std::vector X(tnp1); 66 | for (int i = 0; i < tnp1; ++i) { 67 | X[i] = 0; 68 | for (int j = 0; j < N; ++j) 69 | X[i] += (TYPE)pow(x[j], i); 70 | } 71 | 72 | // a = vector to store final coefficients. 73 | std::vector a(np1); 74 | 75 | // B = normal augmented matrix that stores the equations. 76 | std::vector > B(np1, std::vector(np2, 0)); 77 | 78 | for (int i = 0; i <= n; ++i) 79 | for (int j = 0; j <= n; ++j) 80 | B[i][j] = X[i + j]; 81 | 82 | // Y = vector to store values of sigma(xi^n * yi) 83 | std::vector Y(np1); 84 | for (int i = 0; i < np1; ++i) { 85 | Y[i] = (TYPE)0; 86 | for (int j = 0; j < N; ++j) { 87 | Y[i] += (TYPE)pow(x[j], i)*y[j]; 88 | } 89 | } 90 | 91 | // Load values of Y as last column of B 92 | for (int i = 0; i <= n; ++i) 93 | B[i][np1] = Y[i]; 94 | 95 | n += 1; 96 | int nm1 = n - 1; 97 | 98 | // Pivotisation of the B matrix. 99 | for (int i = 0; i < n; ++i) 100 | for (int k = i + 1; k < n; ++k) 101 | if (B[i][i] < B[k][i]) 102 | for (int j = 0; j <= n; ++j) { 103 | tmp = B[i][j]; 104 | B[i][j] = B[k][j]; 105 | B[k][j] = tmp; 106 | } 107 | 108 | // Performs the Gaussian elimination. 109 | // (1) Make all elements below the pivot equals to zero 110 | // or eliminate the variable. 111 | for (int i = 0; i= 0; --i) { 123 | a[i] = B[i][n]; // (1) 124 | for (int j = 0; j> myVideo::extractBackground(int startnum) { 8 | string s = outputpaths; 9 | string s_del = "rm -rf "; 10 | s_del += s + "/*.jpg"; 11 | const char* todel = s_del.c_str(); 12 | cout << s_del << "\n" << todel << endl; 13 | system(todel); 14 | int n = 0, cnum = 0, count0 = 0, count = startnum; 15 | 16 | vector> Fig; 17 | Mat frame_org, fgmask, fgmask_cali_rot, fgmask_cali, frame, fgimg, bgimg, contours, frame_cali, frame_cali_rot; 18 | GpuMat d_fgmask, d_fgimg, d_bgimg, d_bgimg_rot, d_fgmask_rot, d_frameb, d_frame, d_frame_cali, d_bg_cali; 19 | Stream stream; 20 | 21 | 22 | int GPU_work = getCudaEnabledDeviceCount(); 23 | if (GPU_work) 24 | cout << "The GPU is working with OPENCV" << endl; 25 | string fname = "s"; 26 | int Win = 300, Hin = 600; 27 | namedWindow(fname, WINDOW_NORMAL | WINDOW_KEEPRATIO); 28 | cv::moveWindow(fname, WINDOWSXP, WINDOWSYP); 29 | cv::resizeWindow(fname, Win, Hin); 30 | //cv::moveWindow(fname, WINDOWSXP + mov, WINDOWSYP); 31 | 32 | 33 | 34 | while (1) { 35 | cout << count << "--"; 36 | readvehframe(frame_org, count+1); 37 | //cv::flip(frame_org, frame_org, -1); 38 | if (count == startnum) { 39 | cali(frame_org); 40 | CalculateVideoDirection(Fig); 41 | } 42 | Calibrateframe(frame_org, frame_cali); 43 | 44 | Size ms = frame_org.size(); 45 | d_frame_cali.upload(frame_cali, stream); 46 | 47 | 48 | //TODO: load area to match in rotation 49 | //FindRotationParameter(d_frame, d_frame_cali, fgmask_cali_rot, count, cnum); 50 | float rotp[3] = { 0,0,0 }; 51 | 52 | RotateFrame(d_frame_cali, d_fgmask_rot, rotp, count); 53 | d_fgmask_rot.download(frame_cali_rot); 54 | 55 | 56 | if(count%100 == 0){ 57 | imshow(fname, frame_cali_rot); 58 | waitKey(10); 59 | } 60 | saveframe(frame_cali_rot, count, count0); 61 | 62 | //OperateOneFrame(fgmask_rot_cali, Fig, count, n, cnum); 63 | 64 | 65 | if (count == OPFRAMENUM + 200) 66 | break; 67 | count++; 68 | } 69 | //writerotparas(); 70 | 71 | cv::destroyAllWindows(); 72 | return Fig; 73 | } 74 | -------------------------------------------------------------------------------- /step3_lanedetection/Project1/f2r.py: -------------------------------------------------------------------------------- 1 | import xlrd 2 | import matplotlib 3 | import matplotlib.pyplot as plt 4 | import numpy as np 5 | from mpl_toolkits import mplot3d 6 | import csv 7 | 8 | cor = { 9 | 0: 'b', 10 | 1: 'k', 11 | 2: 'r', 12 | 3: 'g', 13 | 4: 'c', 14 | 5: 'sienna', 15 | 6: 'grey', 16 | 7: 'y', 17 | 8: 'violet', 18 | 9: 'coral', 19 | 10:'darkblue', 20 | 11:'darkgreen', 21 | 12:'indigo', 22 | 13:'maroon', 23 | 14:'navy', 24 | 15:'olive', 25 | 16:'orangered', 26 | 17:'sienna', 27 | 18:'violet', 28 | } 29 | font = {'family' : 'serif', 'size' : 10} 30 | matplotlib.rc('font', **font) 31 | fig = plt.figure() 32 | ax = plt.axes(projection='3d') 33 | 34 | cormap = range(19) 35 | cormap = list(cormap) 36 | initi = 0 37 | fx = [] 38 | fy = [] 39 | ft = [] 40 | fl = [] 41 | def turn(x,y): 42 | pi = 3.1415926 43 | zH = 400.0; 44 | zpi = 3.14159; 45 | zal = 52.0; 46 | za = zal*pi / 180.0; 47 | zx = np.arctan(np.tan(za) - 404.49 / 400.0); 48 | zb = za - zx; 49 | zbl = zb * 180.0 /pi; 50 | zabl = zal + zbl; 51 | zr = zH*(np.tan(zb + za) - np.tan(za - zb)); 52 | zfocal = 27.0 / 100.0 / 0.3048 / 10.0; 53 | zhs = zfocal * np.tan(zb) * 2; 54 | zws = zhs * 4096.0 / 2160.0; 55 | zWS = zws; 56 | zHS = zhs; 57 | zXS0 = zWS / 2.0; 58 | zYS0 = zHS / 2.0; 59 | zFF = zfocal; 60 | zHH = zH; 61 | zAL = za; 62 | zBE = zb; 63 | 64 | WI = 4096; HI = 2160; 65 | XS0 = zWS / 2; YS0 = zHS / 2; 66 | xp = x*zWS / WI - XS0; 67 | yp = y*zHS / HI - YS0; 68 | cs = np.cos(zAL) 69 | ss = np.sin(zAL) 70 | t = zHH / cs; fy = zFF / cs; 71 | xm = 1 72 | ym = 1 73 | qy = int(t*yp/(fy + ss * yp) * ym); 74 | qx = int(xp * (t - ss * qy) / zFF * xm); 75 | mult = 2 76 | return qx*1.5,qy*3 77 | 78 | with open('lanenumbered.csv') as csvDataFile: 79 | csvReader = csv.reader(csvDataFile) 80 | tt = [] 81 | xx = [] 82 | yy = [] 83 | ln = -1 84 | cn = -1 85 | for row in csvReader: 86 | cnum = int(row[0]) 87 | time = float(row[1]) 88 | cx = int(row[2]) 89 | cy = int(row[3]) 90 | lane = int(row[4]) 91 | if ln == -1: 92 | ln = lane 93 | cn = cnum 94 | qx,qy = turn(cx,cy) 95 | tt.append(time/25) 96 | xx.append(qx) 97 | yy.append(qy) 98 | if lane != ln or cnum != cn: 99 | fx.append(xx) 100 | fy.append(yy) 101 | ft.append(tt) 102 | fl.append(ln) 103 | tt = [] 104 | xx = [] 105 | yy = [] 106 | ln = lane 107 | cn = cnum 108 | elif cy > 2160*0.45 and cx > 4096*0.4 and (cy-2160*0.45)*0.4 < 700 and (cx-4096*0.4)*0.4 < 800: 109 | tt.append(time/25) 110 | qx,qy = turn(cx,cy) 111 | xx.append(qx) 112 | yy.append(qy) 113 | 114 | for i in range(len(fx)): 115 | bb = 0 116 | for j in range(len(fx[i]) - 1): 117 | if len(fx[i]) < 100 or fl[i] == 0: 118 | bb = 1 119 | break 120 | if bb: 121 | continue 122 | ax.plot(fx[i],fy[i],ft[i],linewidth = 2, linestyle = '-', color = cor[fl[i]],label = 'lane ' + str(fl[i]) if fl[i] in cormap else "") 123 | if fl[i] in cormap: 124 | cormap.remove(fl[i]) 125 | plt.xlim(0,800) 126 | plt.ylim(0,800) 127 | plt.legend(loc=2) 128 | plt.xlabel("X (ft)") 129 | plt.ylabel("Y (ft)") 130 | ax.set_zlabel("T (s)") 131 | #plt.xticks(np.arange(0, 600, step=200)) 132 | #plt.yticks(np.arange(0, 300, step=100)) 133 | #ax.set_zticks(np.arange(0, 200, step=50)) 134 | #plt.zlabel("Y (ft)") 135 | plt.show() 136 | plt.savefig('lanenumberedtra1.png') 137 | -------------------------------------------------------------------------------- /step3_lanedetection/Project1/file_operation_and_inits.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "FRAMES.h" 3 | 4 | FRAMES::FRAMES(string g_path) { 5 | gen_path = g_path; 6 | frame_path = gen_path + "imgs/"; 7 | output_data_path = gen_path + "output/"; 8 | temp_data_path = gen_path + "temp_data/"; 9 | temp_frame_path = gen_path + "temp_frame/"; 10 | 11 | if (!PF.exists_file(output_data_path)) 12 | PF.create_file(output_data_path); 13 | if (!PF.exists_file(temp_data_path)) 14 | PF.create_file(temp_data_path); 15 | if (!PF.exists_file(temp_frame_path)) 16 | PF.create_file(temp_frame_path); 17 | 18 | } 19 | 20 | -------------------------------------------------------------------------------- /step3_lanedetection/Project1/frame_load.cpp: -------------------------------------------------------------------------------- 1 | #include "FRAMES.h" 2 | 3 | bool FRAMES::load_frame(int fnum, Mat &cur_frame) { 4 | string mat_path = frame_path + to_string(fnum) + ".jpg"; 5 | cur_frame = imread(mat_path); 6 | if (cur_frame.empty()) 7 | return false; 8 | else return true; 9 | } 10 | 11 | 12 | bool FRAMES::load_frame(string frame_path, Mat &cur_frame) { 13 | cur_frame = imread(frame_path); 14 | if (cur_frame.empty()) 15 | return false; 16 | else return true; 17 | } -------------------------------------------------------------------------------- /step3_lanedetection/Project1/generate_learning_data.cpp: -------------------------------------------------------------------------------- 1 | #include "FRAMES.h" 2 | 3 | void create_learning_name_file(string imgpath, int freq){ 4 | string name_file_path = "C:/Users/dongfangzhao/Documents/OPENCV/darknet-master/build/darknet/x64/data/"; 5 | 6 | Public_func pf; 7 | string train_file = name_file_path + "train.txt"; 8 | string test_file = name_file_path + "test.txt"; 9 | ofstream trainfile, testfile; 10 | trainfile.open(train_file); 11 | testfile.open(test_file); 12 | string string0 = "data/obj/"; 13 | for (int i = 0; pf.exists_file(imgpath + to_string(i*freq) + ".jpg"); i++) { 14 | if (i % 5) 15 | trainfile << string0 + to_string(i) + ".jpg\n"; 16 | else 17 | testfile << string0 + to_string(i) + ".jpg\n"; 18 | } 19 | } 20 | 21 | std::vector objects_names_from_file(std::string const filename) { 22 | std::ifstream file(filename); 23 | std::vector file_lines; 24 | if (!file.is_open()) return file_lines; 25 | for (std::string line; getline(file, line);) file_lines.push_back(line); 26 | std::cout << "object names loaded \n"; 27 | return file_lines; 28 | } 29 | 30 | void FRAMES::generate_learning_data(string imgpath, string last_cnn_path, string cnn_path, string learning_img_path, int freq) { 31 | imgpath = gen_path + imgpath; 32 | create_learning_name_file(imgpath, freq); 33 | Public_func pf; 34 | string dele = "rm -r " + learning_img_path + "*"; 35 | //system(dele.c_str()); 36 | 37 | 38 | namedWindow("video", WINDOW_NORMAL | WINDOW_KEEPRATIO); 39 | cv::moveWindow("video", 0, 0); 40 | cv::resizeWindow("video", 480, 960); 41 | 42 | std::string genpath = "C:/Users/dongfangzhao/Documents/Opencv/darknet-master/build/darknet/x64"; 43 | std::string names_file = genpath + "/data/obj.names"; 44 | std::string cfg_file = genpath + "/yolov3_I75_left.cfg"; 45 | std::string weights_file = last_cnn_path + "yolov3_I75_left_last.weights"; 46 | 47 | LIB_API::Detector yolo(cfg_file, weights_file, 0); 48 | auto obj_names = objects_names_from_file(names_file); 49 | vector bbox0; 50 | for (int i = 0; pf.exists_file(imgpath + to_string(i*freq) + ".jpg"); i++) { 51 | string img_i_path = imgpath + to_string(i*freq) + ".jpg"; 52 | Mat img; 53 | int imgnum = i*freq; 54 | load_frame(img_i_path, img); 55 | string img_path = learning_img_path + to_string(i) + ".jpg"; 56 | cv::imwrite(img_path, img); 57 | 58 | string txt_path = learning_img_path + to_string(i) + ".txt"; 59 | cout << i << "#"; 60 | bbox0 = yolo.detect(img, 0.5, true); 61 | writedata(txt_path, img, bbox0); 62 | } 63 | } 64 | 65 | 66 | 67 | 68 | -------------------------------------------------------------------------------- /step3_lanedetection/Project1/get_mergedmarker.cpp: -------------------------------------------------------------------------------- 1 | #include "FRAMES.h" 2 | 3 | void FRAMES::get_mergedmarker_loc(vector frames, vector edge) { 4 | Public_func pf; 5 | vector g_path = { "N:/LEFT/","L:/MIDDLE/","N:/RIGHT/" }; 6 | string markers_path = "N:/LEFT/temp_data/" + to_string(iffirst) + "_marker_loc.csv"; 7 | if (!pf.exists_file(markers_path)){ 8 | load_markers_from_file(markers_path); 9 | } 10 | else { 11 | vector markers_p; 12 | vector left_markers, right_markers, middle_markers; 13 | load_left_right_trans_marker(left_markers, right_markers); 14 | load_middle_markers(frames[1], edge, middle_markers); 15 | calculate_load(left_markers, middle_markers, right_markers, frames); 16 | save_initial_markers(); 17 | } 18 | 19 | 20 | } -------------------------------------------------------------------------------- /step3_lanedetection/Project1/get_motion_parameters.cpp: -------------------------------------------------------------------------------- 1 | #include "FRAMES.h" 2 | 3 | void FRAMES::get_motion_parameters() { 4 | //when we want to restart the process, load parameters and find initial fnum is needed; 5 | Mat cur_frame, pre_frame, cur_depth; 6 | //138001 or 0 7 | for (int fnum = 138001; fnum < 216000 ; fnum+=30) { 8 | if (!load_frame(fnum, cur_frame)){ 9 | save_results(pre_frame, fnum); 10 | break; 11 | } 12 | if (circle_mat.empty()){ 13 | load_depth(fnum, cur_depth); 14 | load_circle(cur_frame, cur_depth); 15 | if (pre_frame.empty()) 16 | pre_frame = cur_frame.clone(); 17 | } 18 | else{ 19 | bool matched = match_circles(cur_frame);//cur 20 | if (!matched) { 21 | cout << "circles matched unsuccefully, reload started" << endl; 22 | load_circle(cur_frame, cur_depth); 23 | continue; 24 | } 25 | } 26 | 27 | if ((fnum-1) % (1200) == 0)// * 33 28 | save_results(cur_frame, fnum);//cur 29 | cout << fnum << "-"; 30 | 31 | } 32 | 33 | } 34 | 35 | 36 | 37 | bool FRAMES::load_depth(int fnum, Mat &cur_depth) { 38 | string mat_path = frame_path + to_string(fnum) + ".jpg"; 39 | cur_depth = imread(mat_path, IMREAD_ANYDEPTH); 40 | if (cur_depth.empty()) 41 | return false; 42 | else return true; 43 | } 44 | 45 | //save frame with circles and locations(framenum, position (x,y) and radius) 46 | void FRAMES::save_results(Mat cur_frame, int fnum) { 47 | for(int i = 0; i < vector_c_p.back().size(); i ++) 48 | cv::circle(cur_frame, vector_c_p.back()[i], vector_c_r.back()[i], Scalar(255, 0, 0), 5); 49 | string img_path = temp_frame_path + to_string(fnum) + ".jpg"; 50 | cv::imwrite(img_path, cur_frame); 51 | 52 | ofstream ofstr; 53 | string csv_path_s = temp_data_path + to_string(fnum) + ".csv"; 54 | ofstr.open(csv_path_s.c_str()); 55 | for (int i = 0; i < vector_c_p.size(); i++) { 56 | ofstr << i << ","; 57 | for (int j = 0; j < vector_c_p[i].size(); j++) { 58 | ofstr << vector_c_p[i][j].x << "," << 59 | vector_c_p[i][j].y << "," << vector_c_r[i][j]<<","; 60 | } 61 | ofstr << "\n"; 62 | } 63 | } 64 | 65 | 66 | void FRAMES::push_points() { 67 | vector_c_p.push_back(circle_point); 68 | vector_c_r.push_back(circle_rad); 69 | } -------------------------------------------------------------------------------- /step3_lanedetection/Project1/lanenum.py: -------------------------------------------------------------------------------- 1 | from sklearn.cluster import DBSCAN 2 | import numpy as np 3 | import csv 4 | from sklearn import metrics 5 | from sklearn.datasets.samples_generator import make_blobs 6 | from sklearn.preprocessing import StandardScaler 7 | import matplotlib.pyplot as plt 8 | 9 | data = [] 10 | with open('output.csv') as mycsv: 11 | csvread = csv.reader(mycsv) 12 | for row in csvread: 13 | t = [i for i in row] 14 | data.append(t) 15 | data = np.array(data) 16 | for k, col in zip(unique_labels, colors): 17 | plt.plot(xy[:, 0], xy[:, 1], 'o', markerfacecolor=tuple(col), 18 | markeredgecolor='w', markersize=4) 19 | plt.title('Estimated number of clusters: %d' % n_clusters_) 20 | plt.show() 21 | -------------------------------------------------------------------------------- /step3_lanedetection/Project1/lanenumber1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATS-Lab/Filed-Experiment-Data-HIGH_Sim/b5f9845bd61591fdcd3e9101a22e458ca3afae18/step3_lanedetection/Project1/lanenumber1.jpg -------------------------------------------------------------------------------- /step3_lanedetection/Project1/lanenumberedtra1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATS-Lab/Filed-Experiment-Data-HIGH_Sim/b5f9845bd61591fdcd3e9101a22e458ca3afae18/step3_lanedetection/Project1/lanenumberedtra1.png -------------------------------------------------------------------------------- /step3_lanedetection/Project1/lanenumberedtraroundabouts.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATS-Lab/Filed-Experiment-Data-HIGH_Sim/b5f9845bd61591fdcd3e9101a22e458ca3afae18/step3_lanedetection/Project1/lanenumberedtraroundabouts.jpg -------------------------------------------------------------------------------- /step3_lanedetection/Project1/load_and_store_2.cpp: -------------------------------------------------------------------------------- 1 | #include "myVideo.h" 2 | #include "Veh.h" 3 | 4 | void myVideo::store2(const char* file, vector> V) { 5 | if (edge.empty()) 6 | edge = addedge(); 7 | //_mkdir(file); 8 | string s = file; 9 | string s_del = "rm -r "; 10 | s_del += s + "/*"; 11 | const char* todel = s_del.c_str(); 12 | cout << s_del << "\n" << todel << endl; 13 | //system(todel); 14 | int n = 0, cnum = 0, count = 0; 15 | 16 | ofstream myfile; 17 | string f_csv = s; 18 | const char* f_cs = f_csv.c_str(); 19 | myfile.open(f_cs); 20 | myfile << "i->num" << "," << "i->cx" << "," << "i->cy" << "," 21 | << "i->rect.x" << "," << "i->rect.y" << "," << "i->rect.width" << "," 22 | << "i->rect.height" << "," << "i->state" << ",i->ep[i],,,,,,,," << "time" << "\n"; 23 | while (1) { 24 | if (count == V.size()) 25 | break; 26 | Point2f pf[2]; 27 | for (auto &i : V[count]) { 28 | if (i->state > 0 && i->rect.x >= edge.x) { 29 | myfile << i->num << "," << i->cx << "," << i->cy << "," << i->rect.x << "," << i->rect.y << "," << i->rect.width << "," 30 | << i->rect.height << "," << i->state; 31 | for (int p = 0; p < 4; p++) 32 | myfile << "," << i->ep[2 * p] << "," << i->ep[2 * p + 1]; 33 | myfile << "," << i->ct; 34 | myfile << "\n"; 35 | } 36 | 37 | } 38 | count++; 39 | } 40 | cv::destroyAllWindows(); 41 | } 42 | 43 | void myVideo::load2(const char* file, vector> &V) { 44 | 45 | int num, x, y, w, h, state, count; 46 | float cx, cy, ep[8]; 47 | string snum, sx, sy, sw, sh, sstate, scx, scy, sep[8], scount; 48 | ifstream datafile(file); 49 | getline(datafile, snum); 50 | int count0 = 0, precount = 0; 51 | vector V0; 52 | while (getline(datafile, snum, ',')) { 53 | if (snum.length() < 2) { 54 | getline(datafile, snum); 55 | continue; 56 | } 57 | getline(datafile, scx, ','); 58 | getline(datafile, scy, ','); 59 | getline(datafile, sx, ','); 60 | getline(datafile, sy, ','); 61 | getline(datafile, sw, ','); 62 | getline(datafile, sh, ','); 63 | getline(datafile, sstate, ','); 64 | for (int i = 0; i < 8; i++) { 65 | getline(datafile, sep[i], ','); 66 | ep[i] = stof(sep[i]); 67 | } 68 | getline(datafile, scount); 69 | num = stoi(snum); 70 | cx = stof(scx); 71 | cy = stof(scy); 72 | x = stoi(sx); 73 | y = stoi(sy); 74 | w = stoi(sw); 75 | h = stoi(sh); 76 | state = stoi(sstate); 77 | count = stoi(scount); 78 | 79 | Veh *vehi = new Veh(num, cx, cy, x, y, w, h, state, ep, count); 80 | for (auto vehs : V0) 81 | if (vehs->num == vehi->num && count == precount) 82 | goto cont; 83 | if ((vehi->rect & block).area() > 0 || vehi->x > edge.x+edge.width || vehi->rect.width > 70 || !inEdge(vehi)) 84 | continue; 85 | //copy here new. 86 | if (count > V.size() + 1) { 87 | V.push_back(V0); 88 | V0.clear(); 89 | for (int i = 0; i < count - V.size() - 1; i++) 90 | V.push_back(V0); 91 | } 92 | if (count == precount) { 93 | if (!V.empty()) 94 | for (int i = 0; i < V.back().size(); i++) { 95 | if (V.back()[i]->num == vehi->num) { 96 | vehi->pv = V.back()[i]; 97 | V.back()[i]->nv = vehi; 98 | break; 99 | } 100 | } 101 | V0.push_back(vehi); 102 | } 103 | else { 104 | V.push_back(V0); 105 | V0.clear(); 106 | if (!V.empty()) 107 | for (int i = 0; i < V.back().size(); i++) { 108 | if (V.back()[i]->num == vehi->num) { 109 | vehi->pv = V.back()[i]; 110 | V.back()[i]->nv = vehi; 111 | break; 112 | } 113 | } 114 | V0.push_back(vehi); 115 | precount = count; 116 | } 117 | cont: 118 | continue; 119 | } 120 | V.push_back(V0); 121 | } -------------------------------------------------------------------------------- /step3_lanedetection/Project1/load_block.cpp: -------------------------------------------------------------------------------- 1 | #include "myVideo.h" 2 | #include "Veh.h" 3 | Mat blockground; 4 | string mousewinname; 5 | void Cross_block(cv::Mat frame, cv::Point point, cv::Scalar s) { 6 | int cl = 10; 7 | cv::Point a(cl, 0), b(-cl, 0), c(0, cl), d(0, -cl); 8 | line(frame, point + a, point + b, s, 4); 9 | line(frame, point + c, point + d, s, 4); 10 | } 11 | 12 | void Mouseclick_block(int evt, int x, int y, int flags, void* edgepoints) { 13 | cv::Scalar s(0, 0, 255); 14 | std::vector* edgep = (std::vector*)edgepoints; 15 | if (evt == CV_EVENT_LBUTTONDOWN) { 16 | edgep->push_back(Point(x, y)); 17 | if (edgep->size() >= 1) { 18 | Point tem = edgep->back(); 19 | Cross(blockground, tem, s); 20 | } 21 | imshow(mousewinname, blockground); 22 | } 23 | } 24 | 25 | Rect myVideo::load_block() { 26 | vector blockpoints; 27 | string file = parapaths + "/block.txt"; 28 | Rect output; 29 | blockground = background.clone(); 30 | if (exists_file(file)) { 31 | ifstream input(file); 32 | string number; 33 | int paras[4]; 34 | int i = 0; 35 | while (i < 4) { 36 | getline(input, number); 37 | paras[i] = atoi(number.c_str()); 38 | i++; 39 | } 40 | output = Rect(paras[0], paras[1], paras[2], paras[3]); 41 | } 42 | else { 43 | namedWindow("Background", WINDOW_NORMAL | WINDOW_KEEPRATIO); 44 | cv::moveWindow("Background", 50, 50); 45 | cv::resizeWindow("Background", 1500, 750); 46 | cv::setMouseCallback("Background", Mouseclick_block, &blockpoints); 47 | imshow("Background", blockground); 48 | mousewinname = "Background"; 49 | char c = waitKey(0); 50 | if (c == 't') { 51 | if (blockpoints.size() > 3) { 52 | vector outpoints(blockpoints.end() - 4, blockpoints.end()); 53 | output = boundingRect(outpoints); 54 | } 55 | 56 | else { 57 | int x = 0.15 * blockground.cols, y = 0.05 * blockground.rows; 58 | int w = 0.7 * blockground.cols, h = 0.93 * blockground.rows; 59 | output = Rect(x, y, w, h); 60 | cout << "x" << x << "y" << y << "w" << w << "h" << h; 61 | } 62 | } 63 | output.x = max(50, output.x); 64 | output.height = min(YPIXEL - 50 - output.y, output.height); 65 | rectangle(blockground, output, Scalar(255), 5, 6, 0); 66 | imshow("Background", blockground); 67 | c = waitKey(3000); 68 | destroyAllWindows(); 69 | std::ofstream outfile(file); 70 | outfile << to_string(output.x) << std::endl; 71 | outfile << to_string(output.y) << std::endl; 72 | outfile << to_string(output.width) << std::endl; 73 | outfile << to_string(output.height) << std::endl; 74 | outfile.close(); 75 | } 76 | return output; 77 | } 78 | 79 | 80 | 81 | 82 | Rect myVideo::load_rect_etc(Mat frame_rted) { 83 | cout << "loading rect etc" << endl; 84 | vector blockpoints; 85 | string file = parapaths + "/rect_etc.txt"; 86 | Rect output; 87 | blockground = frame_rted.clone(); 88 | if (exists_file(file)) { 89 | ifstream input(file); 90 | string number; 91 | int paras[4]; 92 | int i = 0; 93 | while (i < 4) { 94 | getline(input, number); 95 | paras[i] = atoi(number.c_str()); 96 | i++; 97 | } 98 | output = Rect(paras[0], paras[1], paras[2], paras[3]); 99 | } 100 | else { 101 | string name = "rect_etc"; 102 | mousewinname = name; 103 | namedWindow(name, WINDOW_NORMAL | WINDOW_KEEPRATIO); 104 | cv::moveWindow(name, WINDOWSXP, 0); 105 | cv::resizeWindow(name, 400, 800); 106 | cv::setMouseCallback(name, Mouseclick_block, &blockpoints); 107 | reshow: 108 | imshow(name, blockground); 109 | char c = waitKey(0); 110 | if (c == 't') { 111 | if (blockpoints.size() > 3) { 112 | output = boundingRect(blockpoints); 113 | } 114 | else { 115 | cout << "rect_etc is not correctly loaded" << endl; 116 | blockground = frame_rted.clone(); 117 | blockpoints.clear(); 118 | goto reshow; 119 | } 120 | } 121 | rectangle(blockground, output, Scalar(255), 5, 6, 0); 122 | imshow(name, blockground); 123 | c = waitKey(2000); 124 | std::ofstream outfile(file); 125 | outfile << to_string(output.x) << std::endl; 126 | outfile << to_string(output.y) << std::endl; 127 | outfile << to_string(output.width) << std::endl; 128 | outfile << to_string(output.height) << std::endl; 129 | outfile.close(); 130 | destroyWindow(name); 131 | } 132 | return output; 133 | } 134 | -------------------------------------------------------------------------------- /step3_lanedetection/Project1/load_cir_edge.cpp: -------------------------------------------------------------------------------- 1 | #include "FRAMES.h" 2 | 3 | void FRAMES::load_cir_edge_paras(vector>> &v_center, vector> &v_fnum, vector> &v_edge, vector &frames, string path) { 4 | vector g_path = { "N:/LEFT/","L:/MIDDLE/","N:/RIGHT/" }; 5 | v_center.resize(3); 6 | v_fnum.resize(3); 7 | v_edge.resize(3); 8 | frames.resize(3); 9 | double ratio[2][3] = { { 0.497326203, 1, 0.486631016 },{ 0.455782313, 1, 0.469387755 } }; 10 | int order[3] = { 1,9,5 }; 11 | for (int j = 0; j < g_path.size(); j++) { 12 | 13 | ratio_frames = ratio[iffirst][j]; 14 | //load_circle_parameters(g_path[j] + path, v_fnum[j], v_center[j], sti, j, 1); 15 | string edge_path = g_path[j] + "temp_data/" + to_string(iffirst) + "_edge.csv"; 16 | load_circle_parameters(g_path[j] + path, v_fnum[j], v_center[j], j, iffirst, 1); 17 | EDGE_FIT_ORDER = order[j]; 18 | load_edge_function(v_center[j], edge_path, v_fnum[j], v_edge[j]); 19 | load_frame(g_path[j] + "imgs/" + to_string(v_fnum[j][0]) + ".jpg", frames[j]); 20 | Size large_size = frames[j].size() * 2; 21 | int frame_W = large_size.width, frame_H = large_size.height; 22 | Mat shift_M = (Mat_(2, 3) << 1, 0, large_size.width / 4, 0, 1, large_size.height / 4); 23 | cv::warpAffine(frames[j], frames[j], shift_M, large_size); 24 | 25 | Size2i resized(frames[j].cols*ratio[iffirst][j], frames[j].rows*ratio[iffirst][j]); 26 | cv::resize(frames[j], frames[j], resized); 27 | } 28 | 29 | } -------------------------------------------------------------------------------- /step3_lanedetection/Project1/manually_add_train_data.cpp: -------------------------------------------------------------------------------- 1 | #include "FRAMES.h" 2 | 3 | Mat theframe; 4 | void Cross2(cv::Mat &frame, cv::Point point, cv::Scalar s) { 5 | int cl = 6; 6 | cv::Point a(cl, 0), b(-cl, 0), c(0, cl), d(0, -cl); 7 | line(frame, point + a, point + b, s, 2); 8 | line(frame, point + c, point + d, s, 2); 9 | } 10 | 11 | void Mouseclick3(int evt, int x, int y, int flags, void* edgepoints) { 12 | cv::Scalar s(0, 0, 255); 13 | std::vector* edgep = (std::vector*)edgepoints; 14 | if (evt == CV_EVENT_LBUTTONDOWN) { 15 | Mat frame = theframe.clone(); 16 | edgep->push_back(Point(x, y)); 17 | if (edgep->size() >= 1) { 18 | for (int i = 0; i * 2 + 1 < edgep->size(); i++) 19 | rectangle(frame, Rect((*edgep)[i * 2], (*edgep)[i * 2 + 1]), Scalar(255, 100, 0), 3); 20 | Cross2(frame, edgep->back(), Scalar(0, 0, 255)); 21 | } 22 | imshow("video", frame); 23 | } 24 | if (evt == CV_EVENT_RBUTTONDOWN) { 25 | Mat frame = theframe.clone(); 26 | edgep->pop_back(); 27 | if (edgep->size() >= 1) { 28 | for (int i = 0; i * 2 + 1 < edgep->size(); i++) 29 | rectangle(frame, Rect((*edgep)[i * 2], (*edgep)[i * 2 + 1]), Scalar(255, 100, 0), 3); 30 | Cross2(frame, edgep->back(), Scalar(0, 0, 255)); 31 | } 32 | imshow("video", frame); 33 | } 34 | } 35 | 36 | void FRAMES::writedata(string txt_path, Mat img, vector bbox0) { 37 | vector contourpoints; 38 | cv::setMouseCallback("video", Mouseclick3, &contourpoints); 39 | double w = img.cols, h = img.rows; 40 | ofstream txtfile; 41 | txtfile.open(txt_path); 42 | 43 | for (auto i : bbox0){ 44 | rectangle(img, Rect(i.x, i.y, i.w, i.h), Scalar(0, 0, 255), 2); 45 | //txtfile<< (i.h > truck_len) << " " << (i.x+i.w/2) / w << " " << (i.y+i.h/2) / h << " " << i.w / w << " " << i.h / h << "\n"; 46 | } 47 | theframe = img.clone(); 48 | imshow("video", img); 49 | reload: 50 | char c = waitKey(0); 51 | if (c == 'c') { 52 | if (contourpoints.size() % 2) { 53 | cout << "points not correct, plz reload. \n"; 54 | contourpoints.clear(); 55 | goto reload; 56 | } 57 | for (int i = 0; i < contourpoints.size(); i += 2) { 58 | Point2i a = contourpoints[i], b = contourpoints[i + 1]; 59 | Rect r(a, b); 60 | rectangle(img, Rect(r.x, r.y, r.width, r.height), Scalar(0, 255, 255), 2); 61 | txtfile << (r.height > truck_len) << " " << (r.x+ r.width/2) / w << " " << (r.y+ r.height/2) / h << " " << r.width / w << " " << r.height / h << "\n"; 62 | for (int j = 0; j < bbox0.size(); j++){ 63 | Rect box(bbox0[j].x, bbox0[j].y, bbox0[j].w, bbox0[j].h); 64 | if ((box&r).area()>0 || box.area() < 50) 65 | bbox0.erase(bbox0.begin() + j--); 66 | } 67 | 68 | } 69 | for (auto i : bbox0) { 70 | txtfile << (i.h > truck_len) << " " << (i.x + i.w / 2) / w << " " << (i.y + i.h / 2) / h << " " << i.w / w << " " << i.h / h << "\n"; 71 | } 72 | } 73 | } -------------------------------------------------------------------------------- /step3_lanedetection/Project1/marker_functions.cpp: -------------------------------------------------------------------------------- 1 | #include "FRAMES.h" 2 | 3 | void FRAMES::get_sub_imgs(GpuMat gmat, vector locs, vector edge, Point2i size, vector &output, vector &vrects) { 4 | Point outerp(size.x + size.y, size.x + size.y); 5 | Mat rotmat; 6 | GpuMat boundmat, bound_rot, gmati; 7 | for (int i = 0; i < locs.size(); i++) { 8 | Point2i p = locs[i]; 9 | int len = 14; 10 | double angle = atan(double(edge[p.y + 60] - edge[p.y]) / double(60)) * 180 / PI; 11 | //Mat rot_mat = getRotationMatrix2D(Point(x0, y0), -angle * 180 / PI, 1); 12 | Rect bound(p - outerp, p + outerp); 13 | boundmat = gmat(bound); 14 | //vrects.push_back(bound); 15 | rotmat = getRotationMatrix2D(outerp, -angle, 1); 16 | cuda::warpAffine(boundmat, bound_rot, rotmat, boundmat.size()); 17 | Rect cut(outerp - Point2i(size.x / 2, size.x / 2), outerp + Point2i(size.x / 2, len - size.x / 2)); 18 | gmati = bound_rot(cut); 19 | output.push_back(gmati.clone()); 20 | //vrects.push_back(bound); 21 | } 22 | } 23 | 24 | 25 | void FRAMES::detect_markers(GpuMat gmat, vector &forcasted_p, vector edge, Point2i size, vector &ori_p, vector &matched_p, vector &match_bool, int &acu, vector &vrects) { 26 | Mat frame; 27 | gmat.download(frame); 28 | 29 | Mat rotmat, vmr; 30 | GpuMat boundmat, bound_rot, gmati, g_res; 31 | 32 | cv::Ptr ptr; 33 | int match_method = CV_TM_CCOEFF_NORMED;// CV_TM_CCOEFF; 34 | ptr = cuda::createTemplateMatching(CV_8UC3, match_method, Size(0, 0)); 35 | for (int j = 0; j < forcasted_p.size(); j++) { 36 | Point outerp(marker_img[j].rows*3+marker_img[j].cols, marker_img[j].rows*3 + marker_img[j].cols); 37 | Point2i orip = forcasted_p[j]; 38 | int bdg = 0; 39 | if (orip.y >= gmat.rows - 50 || orip.x >= gmat.cols - 50 || orip.x <= 50 || orip.y < 50) 40 | bdg = 1; 41 | Vec3b cor; 42 | if (!bdg) 43 | cor = frame.at(orip); 44 | if (bdg || cor[0] == 0) { 45 | matched_p.push_back(forcasted_p[j]); 46 | match_bool.push_back(0); 47 | continue; 48 | } 49 | 50 | Point p = forcasted_p[j]; 51 | double angle = -atan(double(edge[p.y + 60] - edge[p.y]) / double(60)) * 180 / PI; 52 | //Mat rot_mat = getRotationMatrix2D(Point(x0, y0), -angle * 180 / PI, 1); 53 | Rect bound(p - outerp, p + outerp); 54 | bound.x = max(1, bound.x); 55 | bound.y = max(1, bound.y); 56 | boundmat = gmat(bound); 57 | 58 | rotmat = getRotationMatrix2D(outerp, angle, 1); 59 | cuda::warpAffine(boundmat, bound_rot, rotmat, boundmat.size()); 60 | Rect cut(outerp - Point2i(size.x / 2, marker_img[j].rows / 2), outerp + Point2i(size.x / 2, 1.5 * marker_img[j].rows + size.x/2)); 61 | gmati = bound_rot(cut); 62 | ptr->match(gmati, marker_img[j], g_res); 63 | Mat match_mat; 64 | g_res.download(match_mat); 65 | g_res.release(); 66 | 67 | 68 | Mat mker, outr; 69 | gmati.download(outr); 70 | marker_img[j].download(mker); 71 | 72 | //imwrite(temp_frame_path + "0_markeri.jpg", mker); 73 | //(temp_frame_path + "0_mouti.jpg", outr); 74 | 75 | 76 | 77 | float score = 0; 78 | Point2i matchLoc; 79 | if (!match_mat.empty()) { 80 | double minVal; double maxVal; Point minLoc; Point maxLoc; 81 | cv::minMaxLoc(match_mat, &minVal, &maxVal, &minLoc, &maxLoc, Mat()); 82 | maxLoc += Point2i(cut.x, cut.y); 83 | vmr = getRotationMatrix2D(outerp, -angle, 1); 84 | maxLoc += Point2i(marker_img[j].cols / 2, marker_img[j].cols / 2); 85 | matchLoc.x = maxLoc.x*vmr.at(0, 0) + maxLoc.y*vmr.at(0, 1) + vmr.at(0, 2); 86 | matchLoc.y = maxLoc.x*vmr.at(1, 0) + maxLoc.y*vmr.at(1, 1) + vmr.at(1, 2); 87 | score = maxVal; 88 | } 89 | if (score > 0.8) {// 90 | matchLoc += Point(bound.x, bound.y); 91 | matched_p.push_back(matchLoc); 92 | match_bool.push_back(1); 93 | vrects.push_back(Rect(matchLoc - Point(4, 4), matchLoc + Point(4, 4))); 94 | acu++; 95 | } 96 | else { 97 | matched_p.push_back(forcasted_p[j]); 98 | match_bool.push_back(0); 99 | } 100 | } 101 | 102 | 103 | } 104 | 105 | 106 | -------------------------------------------------------------------------------- /step3_lanedetection/Project1/openh264-1.7.0-win64.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATS-Lab/Filed-Experiment-Data-HIGH_Sim/b5f9845bd61591fdcd3e9101a22e458ca3afae18/step3_lanedetection/Project1/openh264-1.7.0-win64.dll -------------------------------------------------------------------------------- /step4_connection/Project1/Angle_test.cpp: -------------------------------------------------------------------------------- 1 | #include "FRAMES.h" 2 | 3 | void FRAMES::test_angle(string path, bool st1) { 4 | vector center; 5 | vector angle; 6 | vector fnum; 7 | load_angle_parameters(gen_path + path, fnum, center, angle); 8 | Mat frame, frame_shifted, frame_rotated; 9 | Point c0 = center[0]; 10 | for (int i = 0; i < center.size(); i+=1000) { 11 | load_frame(fnum[i], frame); 12 | int shiftx = center[i].x - c0.x; 13 | int shifty = center[i].y - c0.y; 14 | //Mat rotate_M = getRotationMatrix2D(c0, (angle[i] - PI / 2)*180.0/PI, 1); 15 | //Mat shift_M = (Mat_(2, 3) << 1, 0, -shiftx, 0, 1, -shifty); 16 | //cv::warpAffine(frame, frame_shifted, shift_M, frame.size()); 17 | Mat rotate_M = getRotationMatrix2D(center[i], (angle[i] - PI / 2)*180.0 / PI, 1); 18 | //for left 1st video, shift 8190-5600=2500; 19 | Mat shift_M = (Mat_(2, 3) << 0, 0, -shiftx, 0, 0, -shifty+2500*st1); 20 | rotate_M = rotate_M + shift_M; 21 | cv::warpAffine(frame, frame_rotated, rotate_M, frame.size()); 22 | string img_path = "N:/LEFT/mid_processing/rotated/" + to_string(fnum[i]) + ".jpg"; 23 | cv::imwrite(img_path, frame_rotated); 24 | 25 | } 26 | 27 | } 28 | void FRAMES::load_angle_parameters(string path, vector &fnum, vector ¢er, vector &angle) { 29 | ifstream ifstr(path.c_str()); 30 | string snum; 31 | while (getline(ifstr, snum, ',')) { 32 | fnum.push_back(stoi(snum)); 33 | int i = 0, x = 0, y = 0; 34 | string xs, ys, as; 35 | float a; 36 | getline(ifstr, xs, ','); 37 | x = stoi(xs); 38 | getline(ifstr, ys, ','); 39 | y = stoi(ys); 40 | getline(ifstr, as); 41 | a = stof(as); 42 | center.push_back(Point(x, y)); 43 | angle.push_back(a); 44 | } 45 | } 46 | 47 | -------------------------------------------------------------------------------- /step4_connection/Project1/Calibration.cpp: -------------------------------------------------------------------------------- 1 | #include "myVideo.h" 2 | #include "Veh.h" 3 | void myVideo::cali(Mat frame) { 4 | string file = parapaths + "/cali.txt"; 5 | Mat frame_rted; 6 | if (exists_file(file)) { 7 | ifstream input(file); 8 | string number; 9 | getline(input, number); 10 | AL = atof(number.c_str()); 11 | Mat frame_rted; 12 | Calibration(frame, frame_rted); 13 | //Showframe("video", frame_rted, 5000); 14 | } 15 | else { 16 | namedWindow("video", WINDOW_NORMAL | WINDOW_KEEPRATIO); 17 | cv::moveWindow("video", WINDOWSXP, 0); 18 | cv::resizeWindow("video", 800, 1800); 19 | 20 | AL = 0; 21 | while (1) { 22 | //Showframe("video", frame); 23 | Calibration(frame, frame_rted); 24 | for (int i = 50; i < frame_rted.cols - 100; i += 100) 25 | cv::line(frame_rted, Point(i, 20), Point(i, frame_rted.rows - 20), Scalar(0, 0, 200), 3); 26 | cout << "click on t to set the angle\n"; 27 | Showframe("video", frame_rted, 400, 0, 1800,800); 28 | char c = (char)waitKey(3000);// 29 | switch (c) { 30 | case 'z': 31 | AL -= 5 * PI / 180; 32 | break; 33 | case 'c': 34 | AL += 5 * PI / 180; 35 | break; 36 | case 'q': 37 | AL -= 2 * PI / 180; 38 | break; 39 | case 'e': 40 | AL += 2 * PI / 180; 41 | break; 42 | case 'a': 43 | AL -= 0.5 * PI / 180; 44 | break; 45 | case 'd': 46 | AL += 0.5 * PI / 180; 47 | break; 48 | } 49 | 50 | if (c == 't') 51 | break; 52 | 53 | } 54 | std::ofstream outfile(file); 55 | outfile << to_string(AL) << std::endl; 56 | outfile.close(); 57 | } 58 | Calibration(frame, frame_rted); 59 | background = frame_rted.clone(); 60 | destroyWindow("video"); 61 | //Showframe("video", edgebackground); 62 | //destroyAllWindows(); 63 | } 64 | 65 | void myVideo::Calibration(Mat frame, Mat &dst) { 66 | getangle(); 67 | 68 | float Zy = (FF*cos(AL) + HS / HI * sin(AL) * (HI - HI / 2)); 69 | float yrmax = HS / HI * (HI / 2) * HH / cos(AL) / Zy; 70 | Zy = (FF*cos(AL) - HS / HI * sin(AL) * (HI / 2)); 71 | float yrmin = -HS / HI * (HI / 2.0) * HH / cos(AL) / Zy; 72 | float Z = -yrmin * sin(AL) + HH / cos(AL); 73 | float xrmin = -Z / FF * WS / WI * WI / 2; 74 | float xrmax = Z / FF * WS / WI * WI / 2; 75 | float xrr = xrmax - xrmin + 1; 76 | float yrr = yrmax - yrmin + 1; 77 | float yr, xr, yri, xri; 78 | int x_, y_; 79 | 80 | cv::Mat_ mapx_32f((int)HI, (int)WI), mapy_32f((int)HI, (int)WI); 81 | 82 | //Mat dst2((int)HI,(int)WI, CV_8UC3, Scalar(0, 0, 0)); 83 | for (int y = 0; y pt(3, 1); 90 | pt(0) = x; 91 | pt(1) = y; 92 | pt(2) = 1; 93 | 94 | yri = y; 95 | xri = x; 96 | yr = yrr / HI * yri + yrmin; 97 | xr = xrr / WI * xri + xrmin; 98 | Zy = (FF*cos(AL) * yr / (-yr*sin(AL) + HH / cos(AL))); 99 | 100 | Z = -yr*sin(AL) + HH / cos(AL); 101 | 102 | pt(1) = y_ = HI / 2 + HI / HS * Zy; 103 | pt(0) = x_ = FF / Z * WI / WS * (xr)+WI / 2; 104 | 105 | pt /= pt(2); 106 | buff_mapx[x] = (int)pt(0); 107 | buff_mapy[x] = (int)pt(1); 108 | //Vec3b color = frame_org.at(Point(x, y)); 109 | //dst2.at(Point(x_, y_)) = color; 110 | } 111 | } 112 | cv::convertMaps(mapx_32f, mapy_32f, map1_16u, map2_16u, CV_32FC2, true); 113 | //frame_r = dst2.clone(); 114 | cv::remap(frame, dst, map1_16u, map2_16u, CV_INTER_LINEAR, BORDER_CONSTANT, Scalar(0, 0, 0)); 115 | } 116 | 117 | void myVideo::Calibrateframe(Mat fgmask_rot, Mat &fgmask_cali) { 118 | cv::remap(fgmask_rot, fgmask_cali, map1_16u, map2_16u, CV_INTER_LINEAR, BORDER_CONSTANT, Scalar(0, 0, 0)); 119 | } 120 | 121 | void myVideo::Calibrateframe(GpuMat fgmask, GpuMat &fgmask_cali) { 122 | Mat m1, m2; 123 | fgmask.download(m1); 124 | cv::remap(m1, m2, map1_16u, map2_16u, CV_INTER_LINEAR, BORDER_CONSTANT, Scalar(0, 0, 0)); 125 | 126 | fgmask_cali.upload(m2); 127 | } 128 | 129 | void myVideo::getangle() { 130 | float H, al, a, pi, x, b, bl, abl, r, focal, hs, ws; 131 | H = 400.0; 132 | pi = 3.14159; 133 | FF = focal = 8.8 / 1000.0*0.3048; 134 | HH = H; 135 | WS = 13.2*0.3048 / 1000.0; 136 | HS = 7.425*0.3048 / 1000.0; 137 | } -------------------------------------------------------------------------------- /step4_connection/Project1/D2lanenum.m: -------------------------------------------------------------------------------- 1 | color = rand(30,3) -------------------------------------------------------------------------------- /step4_connection/Project1/Lanefunctions.cpp: -------------------------------------------------------------------------------- 1 | #include "FRAMES.h" 2 | #include "Poly.h" 3 | #include "Lanes.h" 4 | #include "Public_functions.h" 5 | 6 | void LaneS::loadlanenum() { 7 | string file = path + "/lanenum.txt"; 8 | string lanenum_s; 9 | char c; 10 | Public_func PB; 11 | if (PB.exists_file(file)) { 12 | ifstream input(file); 13 | getline(input, lanenum_s); 14 | lanenum = atoi(lanenum_s.c_str()); 15 | } 16 | else { 17 | cout << "lanenumtxt not exist.\n"; 18 | getline(cin, lanenum_s); 19 | lanenum = atoi(lanenum_s.c_str()); 20 | std::ofstream outfile(file); 21 | outfile << to_string(lanenum) << std::endl; 22 | outfile.close(); 23 | } 24 | for (int i = 0; i < lanenum; i++) { 25 | vector vec; 26 | vehpointx.push_back(vec); 27 | vehpointy.push_back(vec); 28 | } 29 | } 30 | 31 | void LaneS::loadorderandpoints() { 32 | 33 | loadlanenum(); 34 | order = 2; 35 | string file = path + "/lanes.txt"; 36 | Public_func PB; 37 | if (PB.exists_file(file)) {// 38 | ifstream input(file); 39 | for (int j = 0; j < lanenum; j++) { 40 | 41 | vector coefs; 42 | string ys; 43 | float y; 44 | int i = 0; 45 | while (i++ < order + 1) { 46 | getline(input, ys); 47 | y = atof(ys.c_str()); 48 | coefs.push_back(y); 49 | } 50 | fy.push_back(coefs); 51 | } 52 | } 53 | else { 54 | cout << "file not found\n"; 55 | } 56 | 57 | } 58 | 59 | void LaneS::findlanenum(Veh * vehi) { 60 | float x, y; 61 | int cutH = 2000; 62 | if (vehi->pv != NULL && vehi->y > cutH){ 63 | vehi->lnum = vehi->pv->lnum; 64 | return; 65 | } 66 | 67 | x = vehi->cx; 68 | y = vehi->cy; 69 | 70 | 71 | vector xdet;; 72 | for (int i = 0; i < lanenum; i++) { 73 | float xi = fy[i][0] + fy[i][1] * y + fy[i][2] * y*y; 74 | xdet.push_back(abs(x - xi)); 75 | } 76 | int output = 0; 77 | float dis0 = 10; 78 | float dis = dis0; 79 | for (int i = 0; i < lanenum; i++) { 80 | if (xdet[i] < dis) { 81 | output = i; 82 | dis = xdet[i]; 83 | } 84 | } 85 | if(dis < dis0){ 86 | vehi->lnum = output; 87 | if(dis < 3){ 88 | vehpointx[output].push_back(x); 89 | vehpointy[output].push_back(y); 90 | } 91 | } 92 | else { 93 | vehi->lnum = -1; 94 | } 95 | 96 | } 97 | 98 | LaneS::LaneS() { 99 | 100 | } 101 | 102 | LaneS::~LaneS() { 103 | 104 | } 105 | 106 | void LaneS::regresslanes() { 107 | for (int i = 0; i < lanenum; i++) { 108 | if (vehpointx[i].size() < 200 || vehpointy[i].size() != vehpointx[i].size()) 109 | continue; 110 | PolynomialRegression poly; 111 | poly.fitIt(vehpointy[i], vehpointx[i], order, fy[i]); 112 | } 113 | } 114 | 115 | void LaneS::showlane(Mat frame) { 116 | string windowname = "Lanes"; 117 | namedWindow(windowname, WINDOW_NORMAL | WINDOW_KEEPRATIO); 118 | cv::moveWindow(windowname, 5, 5); 119 | cv::resizeWindow(windowname, 384 * 4, 216 * 4); 120 | imshow(windowname, frame); 121 | for(float y = 0; y < 8190; y = y + 1.0) 122 | for (int i = 0; i < lanenum; i++){ 123 | float xi = fy[i][0] + fy[i][1] * y + fy[i][2] * y*y; 124 | cv::circle(frame, Point(xi, y), 2, Scalar(0, 0, 60 * i), 1); 125 | } 126 | imshow(windowname, frame); 127 | waitKey(200); 128 | destroyWindow(windowname); 129 | } -------------------------------------------------------------------------------- /step4_connection/Project1/Lanes.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "FRAMES.h" 3 | #include "Poly.h" 4 | 5 | class LaneS { 6 | public: 7 | vector> vehpointx; 8 | vector> vehpointy; 9 | 10 | int order; 11 | int lanenum; 12 | float time0; 13 | vector> fy; 14 | Mat frame_0; 15 | string path; 16 | 17 | 18 | public: 19 | LaneS(); 20 | ~LaneS(); 21 | 22 | void loadlanenum(); 23 | void loadorderandpoints(); 24 | void regresslanes(); 25 | void findlanenum(Veh * v); 26 | void showlane(Mat frame); 27 | }; 28 | -------------------------------------------------------------------------------- /step4_connection/Project1/Load_circle_parameters.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "FRAMES.h" 3 | 4 | void FRAMES::load_circle_parameters(string path, vector &fnum, vector> ¢ers, int sti, int stj, int detect) { 5 | ifstream ifstr(path.c_str()); 6 | string snum; 7 | getline(ifstr, snum, ','); 8 | while (1) { 9 | if (snum.size() < 1) 10 | break; 11 | fnum.push_back(stoi(snum)); 12 | int i = 0, x = 0, y = 0, r = 0; 13 | string xs, ys, rs; 14 | vector center; 15 | while (1) { 16 | getline(ifstr, xs, ','); 17 | if (xs[0] == '\n') { 18 | centers.push_back(center); 19 | xs.erase(xs.begin()); 20 | snum = xs; 21 | break; 22 | } 23 | x = stoi(xs); 24 | getline(ifstr, ys, ','); 25 | y = stoi(ys); 26 | getline(ifstr, rs, ','); 27 | r = stof(rs); 28 | center.push_back(Point(x, y)); 29 | } 30 | } 31 | 32 | for (auto &ca : centers) 33 | for (auto & ci : ca) 34 | ci = ci + Point(IMGW / 2, IMGH / 2); 35 | if (sti == 2 && stj == 1) 36 | for (auto &ca : centers) { 37 | if (ca.size() == 4) 38 | ca.erase(ca.begin() + 2); 39 | } 40 | if(detect == 1){ 41 | vector fnum2; 42 | vector> center2; 43 | for (int i = 0, ni = fnum[0]; i < fnum.size(); ni++) { 44 | int n = fnum[i]; 45 | if (ni == n) { 46 | fnum2.push_back(ni); 47 | center2.push_back(centers[i]); 48 | i++; 49 | } 50 | else { 51 | fnum2.push_back(ni); 52 | vector pi = centers[i-1]; 53 | double k = ni - fnum[i-1], kn = fnum[i] - fnum[i-1]; 54 | for (int j = 0; j < pi.size(); j++) { 55 | pi[j].x = double(centers[i][j].x - pi[j].x) * k / kn + pi[j].x; 56 | pi[j].y = double(centers[i][j].y - pi[j].y) * k / kn + pi[j].y; 57 | } 58 | center2.push_back(pi); 59 | } 60 | } 61 | fnum = fnum2; 62 | centers = center2; 63 | } 64 | } 65 | 66 | -------------------------------------------------------------------------------- /step4_connection/Project1/Load_cut_edge.cpp: -------------------------------------------------------------------------------- 1 | #include "FRAMES.h" 2 | #include "Poly.h" 3 | 4 | void FRAMES::load_edge_function(vector> center, string edge_path, vector fnum, vector &output) { 5 | Public_func pf; 6 | Mat frame, frame_shifted, frame_rotated, frame_merged; 7 | vector points; 8 | Size2i resized(IMGW*2*ratio_frames, IMGH * 2 * ratio_frames); 9 | Mat affine_M; 10 | if (pf.exists_file(edge_path)) { 11 | load_edge_from_file(edge_path, points); 12 | } 13 | else { 14 | for (int i = 0; i < center.size(); i += 4000) { 15 | load_frame(fnum[i], frame); 16 | Size large_size = frame.size() * 2; 17 | if (center[0].size() > 3) 18 | affine_M = findHomography(center[i], center[0]); 19 | else{ 20 | Point2f a0[3] = {center[0][0], center[0][1], center[0][2]}; 21 | Point2f a1[3] = { center[i][0], center[i][1], center[i][2] }; 22 | affine_M = getAffineTransform(a1, a0); 23 | } 24 | Mat shift_M = (Mat_(2, 3) << 1, 0, large_size.width / 4, 0, 1, large_size.height / 4); 25 | cv::warpAffine(frame, frame_shifted, shift_M, large_size); 26 | if (center[0].size() > 3) 27 | cv::warpPerspective(frame_shifted, frame_rotated, affine_M, large_size); 28 | else 29 | cv::warpAffine(frame_shifted, frame_rotated, affine_M, large_size); 30 | //show_frame(frame_rotated, 10000); 31 | if (!frame_merged.empty()) 32 | cv::addWeighted(frame_merged, 0.5, frame_rotated, 0.5, 0, frame_merged); 33 | else 34 | frame_merged = frame_rotated.clone(); 35 | } 36 | cv::resize(frame_merged, frame_merged, resized); 37 | load_edge_from_frame(frame_merged, points); 38 | ofstream outcsv(edge_path.c_str()); 39 | for (auto p : points) 40 | outcsv << p.x << "," << p.y << "\n"; 41 | } 42 | 43 | PolynomialRegression poly; 44 | vector vy, vx, f; 45 | for (auto p : points) { 46 | vy.push_back(long double(p.y)); 47 | vx.push_back(long double(p.x)); 48 | } 49 | poly.fitIt(vy, vx, EDGE_FIT_ORDER, f); 50 | output.clear(); 51 | for (double y = 0; y < resized.height; y = y + 1.0) { 52 | double xi = 0; 53 | for (int j = 0; j < EDGE_FIT_ORDER + 1; j++) 54 | xi += f[j] * pow(y, j); 55 | output.push_back(xi); 56 | } 57 | } 58 | 59 | void FRAMES::load_edge_from_file(string edge_path, vector &points) { 60 | ifstream input(edge_path.c_str()); 61 | string xs, ys; 62 | float x, y; 63 | while (getline(input, xs, ',')) { 64 | x = stoi(xs); 65 | getline(input, ys); 66 | y = stof(ys); 67 | points.push_back(Point(x, y)); 68 | } 69 | } 70 | 71 | Mat frame_draw; 72 | void Cross(cv::Mat frame, cv::Point point, cv::Scalar s) { 73 | int cl = 10; 74 | cv::Point a(cl, 0), b(-cl, 0), c(0, cl), d(0, -cl); 75 | line(frame, point + a, point + b, s, 4); 76 | line(frame, point + c, point + d, s, 4); 77 | } 78 | 79 | void Mouseclick(int evt, int x, int y, int flags, void* edgepoints) { 80 | cv::Scalar s(0, 0, 255); 81 | std::vector* edgep = (std::vector*)edgepoints; 82 | if (evt == CV_EVENT_LBUTTONDOWN) { 83 | edgep->push_back(Point(x, y)); 84 | if (edgep->size() >= 1) { 85 | Point tem = edgep->back(); 86 | Cross(frame_draw, tem, s); 87 | } 88 | imshow("Merged_frame", frame_draw); 89 | } 90 | } 91 | void FRAMES::load_edge_from_frame(Mat frame_merged, vector &points){ 92 | frame_draw = frame_merged.clone(); 93 | string windowname = "Merged_frame"; 94 | namedWindow(windowname, WINDOW_NORMAL | WINDOW_KEEPRATIO); 95 | cv::moveWindow(windowname, 0, 0); 96 | cv::resizeWindow(windowname, 1820, 1000); 97 | 98 | cv::setMouseCallback(windowname, Mouseclick, &points); 99 | 100 | imshow(windowname, frame_draw); 101 | reload: 102 | points.clear(); 103 | char c = waitKey(0); 104 | if (c == 'c') { 105 | if (points.size() > 3) { 106 | destroyWindow(windowname); 107 | return; 108 | } 109 | else { 110 | cout << "please enter more points"; 111 | goto reload; 112 | } 113 | } 114 | else 115 | goto reload; 116 | 117 | } -------------------------------------------------------------------------------- /step4_connection/Project1/Main.cpp: -------------------------------------------------------------------------------- 1 | #include "FRAMES.h" 2 | #include 3 | 4 | int main() 5 | { 6 | string gen_path[3] = {"N:/LEFT/","L:/MIDDLE/","N:/RIGHT/" }; 7 | 8 | //1. 9 | //FRAMES.get_motion_parameters(); 10 | //2. 11 | //string path = "verify/to_show.csv"; 12 | //FRAMES.test_circles(path); 13 | //3.test angle paramters and generate rotated imgs. 14 | // 15 | 16 | string path[2] = { "temp_data/117700.csv", "temp_data/206700.csv" }; 17 | int order[3] = { 1,9,2 }; 18 | //FRAMES.test_angle(path[1]); 19 | //4. generate learning imgs, with W nad H; 20 | int learn_w = 480, learn_h = 800; 21 | string cnn_path = "C:/Users/dongfangzhao/Documents/OPENCV/darknet-master/build/darknet/x64/backup/yolov3_I75_left_last.weights"; 22 | for(int i = 0, imgnum = 0; i < 3; i ++){ 23 | for(int j = 1; j < 2; j ++){ 24 | FRAMES FRAMES(gen_path[i]); 25 | FRAMES.ratio_frames = FRAMES.frame_ratios[j][i]; 26 | FRAMES.EDGE_FIT_ORDER = order[i]; 27 | FRAMES.iffirst = j; 28 | cout << "\n" << i << j <<":"<< endl; 29 | FRAMES.detect_lane_marker(i, j, path[j]); 30 | return 1; 31 | } 32 | } 33 | 34 | //5. generate learning data 35 | /* 36 | FRAMES FRAMES(gen_path[0]); 37 | string imgpath = "learning_imgs/imgs/"; 38 | string darknet = "C:/Users/dongfangzhao/Documents/OPENCV/darknet-master/build/darknet/x64/"; 39 | string gen_cnn_path = darknet + "backup/I75/all"; 40 | string learning_img_path = darknet + "data/obj/"; 41 | int interval = 1; 42 | string last_cnn_path = gen_cnn_path + "_" + to_string(interval * 10) + "/"; 43 | string cnn_path = gen_cnn_path + "_" + to_string(interval) + "/"; 44 | 45 | FRAMES.generate_learning_data(imgpath, last_cnn_path, cnn_path, learning_img_path, interval); 46 | */ 47 | return 0; 48 | } 49 | -------------------------------------------------------------------------------- /step4_connection/Project1/NOTE.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Documentation: 3 | 0. preprocessing 4 | a. revise rotation algorithm, make it work in 2s frames; 5 | b. revise calibration part... 6 | c. cut the processed image into edge, (save it, optional); 7 | 8 | 1. revise the identification part; 9 | a. resize the image to imagesize*1.2; 10 | b. cut image into 5 pieces and detect them; 11 | c. process results with Rect parameters, coordinate/1.2, and return results; 12 | 13 | 2. tet performance. 14 | 15 | */ 16 | -------------------------------------------------------------------------------- /step4_connection/Project1/OperateOneFrame.cpp: -------------------------------------------------------------------------------- 1 | #include "myVideo.h" 2 | #include "Veh.h" 3 | 4 | void myVideo::OperateOneFrame(Mat fgmask_rot, vector> &Fig, int count, int &n, int cnum) { 5 | vector> contours; 6 | vector hierarchy; 7 | findContours(fgmask_rot, contours, hierarchy, RETR_CCOMP, CHAIN_APPROX_SIMPLE); 8 | vector Fig0; 9 | int hi; 10 | for (int i = 0; i < int(contours.size()); i++) { 11 | if (hierarchy[i][3] < 0) { 12 | if (contours[i].size() < 10) 13 | continue; 14 | float ty = static_cast(contourArea(cv::Mat(contours[i]))); 15 | Moments mu = moments(contours[i], false); 16 | Point2f mc = Point2f(static_cast(mu.m10 / mu.m00), static_cast(mu.m01 / mu.m00)); 17 | mc = Point2f(max(0.0f, mc.x), max(0.0f, mc.y)); 18 | Rect rect = boundingRect(contours[i]); 19 | RotatedRect rt = minAreaRect(contours[i]); 20 | float angle = rt.angle; 21 | Veh* v = new Veh(n++, ty, rt, cnum, count, angle); 22 | Point2f pf[4]; 23 | rt.points(pf); 24 | for (int k = 0; k < 4; k++) { 25 | v->ep[2 * k] = pf[k].x; 26 | v->ep[2 * k + 1] = pf[k].y; 27 | if (cv::norm(pf[k] - pf[(k + 1) % 3]) > 1500) 28 | ty = 10; 29 | } 30 | if (ty < MINCONAREA || ty > MAXCONAREA) 31 | continue; 32 | //if (count > 160) 33 | //rectangle(fgmask_rot, rect, Scalar(255), 10, 8, 0); 34 | Fig0.push_back(v); 35 | } 36 | } 37 | if (count == 0) { 38 | Fig.push_back(Fig0); 39 | } 40 | else { 41 | vector Fb = Fig.back(); 42 | double disu = 50.0; 43 | double d1, d2; 44 | int k; 45 | for (int i = 0; i < int(Fig0.size()); i++) { 46 | d2 = disu; 47 | for (int j = 0; j < int(Fb.size()); j++) { 48 | d1 = Vehdis(Fig0[i], Fb[j]); 49 | if (d1 < d2) { 50 | d2 = d1; 51 | k = j; 52 | } 53 | } 54 | int newv = 1; 55 | if (d2 < disu) { 56 | if (Fb[k]->nv) { 57 | float a1, a2, a3; 58 | a1 = Fb[k]->vtype; 59 | a2 = Fb[k]->nv->vtype; 60 | a3 = Fig0[i]->vtype; 61 | float area1, area2; 62 | area1 = (a1 - a2) * (a1 - a2); 63 | area2 = (a1 - a3) * (a1 - a3); 64 | if (area1 < area2) 65 | newv = 0; 66 | else 67 | newv = 2; 68 | } 69 | if (newv) { 70 | if (newv == 2) { 71 | Fb[k]->nv->pv = NULL; 72 | //Fb[k]->nv = NULL; 73 | } 74 | Fig0[i]->pv = Fb[k]; 75 | Fb[k]->nv = Fig0[i]; 76 | Fig0[i]->num = Fb[k]->num; 77 | Fig0[i]->vtype = (Fb[k]->vtype + Fig0[i]->vtype) / 2; 78 | Fb[k]->vtype = Fig0[i]->vtype; 79 | if (!Fig0[i]->state) { 80 | Veh* b; 81 | b = Fig0[i]; 82 | b->forcast[0] = 2 * b->cx - b->pv->cx; 83 | b->forcast[1] = 2 * b->cy - b->pv->cy; 84 | b->state = 1; 85 | } 86 | } 87 | } 88 | } 89 | Fig.push_back(Fig0); 90 | Fig0.clear(); 91 | } 92 | } 93 | 94 | void myVideo::RefreshOutput(vector> &Fig) { 95 | for (int i = 0; i < Fig.size(); i ++) { 96 | for (int j = 0; j < Fig[i].size(); j++) { 97 | Veh* v; 98 | v = Fig[i][j]; 99 | if (v->time == 0) { 100 | int ti = 1; 101 | while (v->nv) { 102 | v = v->nv; 103 | ti++; 104 | } 105 | while (v) { 106 | v->time = ti; 107 | v = v->pv; 108 | } 109 | } 110 | } 111 | } 112 | for (auto &i : Fig) { 113 | for (int j = 0; j < int(i.size()); j++) { 114 | if (i[j]->nv) 115 | if (i[j]->time != i[j]->nv->time) 116 | cout << "wrong"; 117 | else if (i[j]->nv->pv == NULL) 118 | cout << "nopv"; 119 | else if (i[j]->nv->num != i[j]->num) 120 | cout << "number not equal"; 121 | if (i[j]->time < MINFRAMELEN) 122 | i.erase(i.begin() + j--); 123 | } 124 | } 125 | } -------------------------------------------------------------------------------- /step4_connection/Project1/POINTS.cpp: -------------------------------------------------------------------------------- 1 | #include "myVideo.h" 2 | #include "Veh.h" 3 | 4 | void myVideo::Points(vector > V) { 5 | Mat img(YPIXEL, XPIXEL, CV_8UC3, Scalar(255, 255, 255)); 6 | int num = 0; 7 | std::ofstream mycsv; 8 | mycsv.open("output.csv"); 9 | for (auto &i : V) { 10 | for (auto &j : i) { 11 | mycsv << j->num << "," << j->glotime << "," << j->cx << "," << j->cy << "," << j->lnum << "," << j->vtype << ",\n"; 12 | num++; 13 | //if (num)// % 2 == 0) 14 | circle(img, cv::Point2f(j->cx, j->cy), 2, Scalar(0, 0, 0), 2, 8, 0); 15 | circle(background, cv::Point2f(j->cx, j->cy), 2, Scalar(0, 0, 255), 2, 8, 0); 16 | } 17 | } 18 | mycsv.close(); 19 | namedWindow("Points", WINDOW_NORMAL | WINDOW_KEEPRATIO); 20 | cv::moveWindow("Points", WINDOWSXP, WINDOWSYP); 21 | cv::resizeWindow("Points", WINDOWSW, WINDOWSH); 22 | imshow("Points", img); // Show our image inside it. 23 | imwrite("a.jpg", img); 24 | imwrite("b.jpg", background); 25 | waitKey(3000); 26 | cv::destroyAllWindows(); 27 | } 28 | -------------------------------------------------------------------------------- /step4_connection/Project1/Plot.cpp: -------------------------------------------------------------------------------- 1 | #include "myVideo.h" 2 | #include "Veh.h" 3 | #include 4 | 5 | void Plot(vector > V){ 6 | ofstream out("zline.xls"); 7 | if (!out) { 8 | cout << "open lxs unsuccessful"; 9 | throw; 10 | } 11 | for (auto &i : V){ 12 | for (auto &j : i){ 13 | out << j->num << " \t" << j->glotime << " \t" << j->x << " \t" << j->y\ 14 | << " \t" << j->spd << " \t" << j->acc << " \t" << j->lnum << endl; 15 | } 16 | } 17 | out.close(); 18 | 19 | } 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /step4_connection/Project1/Poly.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #ifndef _POLYNOMIAL_REGRESSION_H 3 | #define _POLYNOMIAL_REGRESSION_H __POLYNOMIAL_REGRESSION_H 4 | 5 | /** 6 | * PURPOSE: 7 | * 8 | * Polynomial Regression aims to fit a non-linear relationship to a set of 9 | * points. It approximates this by solving a series of linear equations using 10 | * a least-squares approach. 11 | * 12 | * We can model the expected value y as an nth degree polynomial, yielding 13 | * the general polynomial regression model: 14 | * 15 | * y = a0 + a1 * x + a2 * x^2 + ... + an * x^n 16 | * @author Chris Engelsma 17 | */ 18 | 19 | #include 20 | #include 21 | 22 | template 23 | class PolynomialRegression { 24 | public: 25 | 26 | PolynomialRegression(); 27 | virtual ~PolynomialRegression() {}; 28 | 29 | bool fitIt( 30 | const std::vector & x, 31 | const std::vector & y, 32 | const int & order, 33 | std::vector & coeffs); 34 | }; 35 | 36 | template 37 | PolynomialRegression::PolynomialRegression() {}; 38 | 39 | template 40 | bool PolynomialRegression::fitIt( 41 | const std::vector & x, 42 | const std::vector & y, 43 | const int & order, 44 | std::vector & coeffs) 45 | { 46 | // The size of xValues and yValues should be same 47 | if (x.size() != y.size()) { 48 | throw std::runtime_error("The size of x & y arrays are different"); 49 | return false; 50 | } 51 | // The size of xValues and yValues cannot be 0, should not happen 52 | if (x.size() == 0 || y.size() == 0) { 53 | throw std::runtime_error("The size of x or y arrays is 0"); 54 | return false; 55 | } 56 | 57 | size_t N = x.size(); 58 | int n = order; 59 | int np1 = n + 1; 60 | int np2 = n + 2; 61 | int tnp1 = 2 * n + 1; 62 | TYPE tmp; 63 | 64 | // X = vector that stores values of sigma(xi^2n) 65 | std::vector X(tnp1); 66 | for (int i = 0; i < tnp1; ++i) { 67 | X[i] = 0; 68 | for (int j = 0; j < N; ++j) 69 | X[i] += (TYPE)pow(x[j], i); 70 | } 71 | 72 | // a = vector to store final coefficients. 73 | std::vector a(np1); 74 | 75 | // B = normal augmented matrix that stores the equations. 76 | std::vector > B(np1, std::vector(np2, 0)); 77 | 78 | for (int i = 0; i <= n; ++i) 79 | for (int j = 0; j <= n; ++j) 80 | B[i][j] = X[i + j]; 81 | 82 | // Y = vector to store values of sigma(xi^n * yi) 83 | std::vector Y(np1); 84 | for (int i = 0; i < np1; ++i) { 85 | Y[i] = (TYPE)0; 86 | for (int j = 0; j < N; ++j) { 87 | Y[i] += (TYPE)pow(x[j], i)*y[j]; 88 | } 89 | } 90 | 91 | // Load values of Y as last column of B 92 | for (int i = 0; i <= n; ++i) 93 | B[i][np1] = Y[i]; 94 | 95 | n += 1; 96 | int nm1 = n - 1; 97 | 98 | // Pivotisation of the B matrix. 99 | for (int i = 0; i < n; ++i) 100 | for (int k = i + 1; k < n; ++k) 101 | if (B[i][i] < B[k][i]) 102 | for (int j = 0; j <= n; ++j) { 103 | tmp = B[i][j]; 104 | B[i][j] = B[k][j]; 105 | B[k][j] = tmp; 106 | } 107 | 108 | // Performs the Gaussian elimination. 109 | // (1) Make all elements below the pivot equals to zero 110 | // or eliminate the variable. 111 | for (int i = 0; i= 0; --i) { 123 | a[i] = B[i][n]; // (1) 124 | for (int j = 0; j> myVideo::extractBackground(int startnum) { 8 | string s = outputpaths; 9 | string s_del = "rm -rf "; 10 | s_del += s + "/*.jpg"; 11 | const char* todel = s_del.c_str(); 12 | cout << s_del << "\n" << todel << endl; 13 | system(todel); 14 | int n = 0, cnum = 0, count0 = 0, count = startnum; 15 | 16 | vector> Fig; 17 | Mat frame_org, fgmask, fgmask_cali_rot, fgmask_cali, frame, fgimg, bgimg, contours, frame_cali, frame_cali_rot; 18 | GpuMat d_fgmask, d_fgimg, d_bgimg, d_bgimg_rot, d_fgmask_rot, d_frameb, d_frame, d_frame_cali, d_bg_cali; 19 | Stream stream; 20 | 21 | 22 | int GPU_work = getCudaEnabledDeviceCount(); 23 | if (GPU_work) 24 | cout << "The GPU is working with OPENCV" << endl; 25 | string fname = "s"; 26 | int Win = 300, Hin = 600; 27 | namedWindow(fname, WINDOW_NORMAL | WINDOW_KEEPRATIO); 28 | cv::moveWindow(fname, WINDOWSXP, WINDOWSYP); 29 | cv::resizeWindow(fname, Win, Hin); 30 | //cv::moveWindow(fname, WINDOWSXP + mov, WINDOWSYP); 31 | 32 | 33 | 34 | while (1) { 35 | cout << count << "--"; 36 | readvehframe(frame_org, count+1); 37 | //cv::flip(frame_org, frame_org, -1); 38 | if (count == startnum) { 39 | cali(frame_org); 40 | CalculateVideoDirection(Fig); 41 | } 42 | Calibrateframe(frame_org, frame_cali); 43 | 44 | Size ms = frame_org.size(); 45 | d_frame_cali.upload(frame_cali, stream); 46 | 47 | 48 | //TODO: load area to match in rotation 49 | //FindRotationParameter(d_frame, d_frame_cali, fgmask_cali_rot, count, cnum); 50 | float rotp[3] = { 0,0,0 }; 51 | 52 | RotateFrame(d_frame_cali, d_fgmask_rot, rotp, count); 53 | d_fgmask_rot.download(frame_cali_rot); 54 | 55 | 56 | if(count%100 == 0){ 57 | imshow(fname, frame_cali_rot); 58 | waitKey(10); 59 | } 60 | saveframe(frame_cali_rot, count, count0); 61 | 62 | //OperateOneFrame(fgmask_rot_cali, Fig, count, n, cnum); 63 | 64 | 65 | if (count == OPFRAMENUM + 200) 66 | break; 67 | count++; 68 | } 69 | //writerotparas(); 70 | 71 | cv::destroyAllWindows(); 72 | return Fig; 73 | } 74 | -------------------------------------------------------------------------------- /step4_connection/Project1/f2r.py: -------------------------------------------------------------------------------- 1 | import xlrd 2 | import matplotlib 3 | import matplotlib.pyplot as plt 4 | import numpy as np 5 | from mpl_toolkits import mplot3d 6 | import csv 7 | 8 | cor = { 9 | 0: 'b', 10 | 1: 'k', 11 | 2: 'r', 12 | 3: 'g', 13 | 4: 'c', 14 | 5: 'sienna', 15 | 6: 'grey', 16 | 7: 'y', 17 | 8: 'violet', 18 | 9: 'coral', 19 | 10:'darkblue', 20 | 11:'darkgreen', 21 | 12:'indigo', 22 | 13:'maroon', 23 | 14:'navy', 24 | 15:'olive', 25 | 16:'orangered', 26 | 17:'sienna', 27 | 18:'violet', 28 | } 29 | font = {'family' : 'serif', 'size' : 10} 30 | matplotlib.rc('font', **font) 31 | fig = plt.figure() 32 | ax = plt.axes(projection='3d') 33 | 34 | cormap = range(19) 35 | cormap = list(cormap) 36 | initi = 0 37 | fx = [] 38 | fy = [] 39 | ft = [] 40 | fl = [] 41 | def turn(x,y): 42 | pi = 3.1415926 43 | zH = 400.0; 44 | zpi = 3.14159; 45 | zal = 52.0; 46 | za = zal*pi / 180.0; 47 | zx = np.arctan(np.tan(za) - 404.49 / 400.0); 48 | zb = za - zx; 49 | zbl = zb * 180.0 /pi; 50 | zabl = zal + zbl; 51 | zr = zH*(np.tan(zb + za) - np.tan(za - zb)); 52 | zfocal = 27.0 / 100.0 / 0.3048 / 10.0; 53 | zhs = zfocal * np.tan(zb) * 2; 54 | zws = zhs * 4096.0 / 2160.0; 55 | zWS = zws; 56 | zHS = zhs; 57 | zXS0 = zWS / 2.0; 58 | zYS0 = zHS / 2.0; 59 | zFF = zfocal; 60 | zHH = zH; 61 | zAL = za; 62 | zBE = zb; 63 | 64 | WI = 4096; HI = 2160; 65 | XS0 = zWS / 2; YS0 = zHS / 2; 66 | xp = x*zWS / WI - XS0; 67 | yp = y*zHS / HI - YS0; 68 | cs = np.cos(zAL) 69 | ss = np.sin(zAL) 70 | t = zHH / cs; fy = zFF / cs; 71 | xm = 1 72 | ym = 1 73 | qy = int(t*yp/(fy + ss * yp) * ym); 74 | qx = int(xp * (t - ss * qy) / zFF * xm); 75 | mult = 2 76 | return qx*1.5,qy*3 77 | 78 | with open('lanenumbered.csv') as csvDataFile: 79 | csvReader = csv.reader(csvDataFile) 80 | tt = [] 81 | xx = [] 82 | yy = [] 83 | ln = -1 84 | cn = -1 85 | for row in csvReader: 86 | cnum = int(row[0]) 87 | time = float(row[1]) 88 | cx = int(row[2]) 89 | cy = int(row[3]) 90 | lane = int(row[4]) 91 | if ln == -1: 92 | ln = lane 93 | cn = cnum 94 | qx,qy = turn(cx,cy) 95 | tt.append(time/25) 96 | xx.append(qx) 97 | yy.append(qy) 98 | if lane != ln or cnum != cn: 99 | fx.append(xx) 100 | fy.append(yy) 101 | ft.append(tt) 102 | fl.append(ln) 103 | tt = [] 104 | xx = [] 105 | yy = [] 106 | ln = lane 107 | cn = cnum 108 | elif cy > 2160*0.45 and cx > 4096*0.4 and (cy-2160*0.45)*0.4 < 700 and (cx-4096*0.4)*0.4 < 800: 109 | tt.append(time/25) 110 | qx,qy = turn(cx,cy) 111 | xx.append(qx) 112 | yy.append(qy) 113 | 114 | for i in range(len(fx)): 115 | bb = 0 116 | for j in range(len(fx[i]) - 1): 117 | if len(fx[i]) < 100 or fl[i] == 0: 118 | bb = 1 119 | break 120 | if bb: 121 | continue 122 | ax.plot(fx[i],fy[i],ft[i],linewidth = 2, linestyle = '-', color = cor[fl[i]],label = 'lane ' + str(fl[i]) if fl[i] in cormap else "") 123 | if fl[i] in cormap: 124 | cormap.remove(fl[i]) 125 | plt.xlim(0,800) 126 | plt.ylim(0,800) 127 | plt.legend(loc=2) 128 | plt.xlabel("X (ft)") 129 | plt.ylabel("Y (ft)") 130 | ax.set_zlabel("T (s)") 131 | #plt.xticks(np.arange(0, 600, step=200)) 132 | #plt.yticks(np.arange(0, 300, step=100)) 133 | #ax.set_zticks(np.arange(0, 200, step=50)) 134 | #plt.zlabel("Y (ft)") 135 | plt.show() 136 | plt.savefig('lanenumberedtra1.png') 137 | -------------------------------------------------------------------------------- /step4_connection/Project1/file_operation_and_inits.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "FRAMES.h" 3 | 4 | FRAMES::FRAMES(string g_path) { 5 | gen_path = g_path; 6 | frame_path = gen_path + "imgs/"; 7 | output_data_path = gen_path + "output/"; 8 | temp_data_path = gen_path + "temp_data/"; 9 | temp_frame_path = gen_path + "temp_frame/"; 10 | 11 | if (!PF.exists_file(output_data_path)) 12 | PF.create_file(output_data_path); 13 | if (!PF.exists_file(temp_data_path)) 14 | PF.create_file(temp_data_path); 15 | if (!PF.exists_file(temp_frame_path)) 16 | PF.create_file(temp_frame_path); 17 | 18 | } 19 | 20 | -------------------------------------------------------------------------------- /step4_connection/Project1/frame_load.cpp: -------------------------------------------------------------------------------- 1 | #include "FRAMES.h" 2 | 3 | bool FRAMES::load_frame(int fnum, Mat &cur_frame) { 4 | string mat_path = frame_path + to_string(fnum) + ".jpg"; 5 | cur_frame = imread(mat_path); 6 | if (cur_frame.empty()) 7 | return false; 8 | else return true; 9 | } 10 | 11 | 12 | bool FRAMES::load_frame(string frame_path, Mat &cur_frame) { 13 | cur_frame = imread(frame_path); 14 | if (cur_frame.empty()) 15 | return false; 16 | else return true; 17 | } -------------------------------------------------------------------------------- /step4_connection/Project1/generate_learning_data.cpp: -------------------------------------------------------------------------------- 1 | #include "FRAMES.h" 2 | 3 | void create_learning_name_file(string imgpath, int freq){ 4 | string name_file_path = "C:/Users/dongfangzhao/Documents/OPENCV/darknet-master/build/darknet/x64/data/"; 5 | 6 | Public_func pf; 7 | string train_file = name_file_path + "train.txt"; 8 | string test_file = name_file_path + "test.txt"; 9 | ofstream trainfile, testfile; 10 | trainfile.open(train_file); 11 | testfile.open(test_file); 12 | string string0 = "data/obj/"; 13 | for (int i = 0; pf.exists_file(imgpath + to_string(i*freq) + ".jpg"); i++) { 14 | if (i % 5) 15 | trainfile << string0 + to_string(i) + ".jpg\n"; 16 | else 17 | testfile << string0 + to_string(i) + ".jpg\n"; 18 | } 19 | } 20 | 21 | std::vector objects_names_from_file(std::string const filename) { 22 | std::ifstream file(filename); 23 | std::vector file_lines; 24 | if (!file.is_open()) return file_lines; 25 | for (std::string line; getline(file, line);) file_lines.push_back(line); 26 | std::cout << "object names loaded \n"; 27 | return file_lines; 28 | } 29 | 30 | void FRAMES::generate_learning_data(string imgpath, string last_cnn_path, string cnn_path, string learning_img_path, int freq) { 31 | imgpath = gen_path + imgpath; 32 | create_learning_name_file(imgpath, freq); 33 | Public_func pf; 34 | string dele = "rm -r " + learning_img_path + "*"; 35 | //system(dele.c_str()); 36 | 37 | 38 | namedWindow("video", WINDOW_NORMAL | WINDOW_KEEPRATIO); 39 | cv::moveWindow("video", 0, 0); 40 | cv::resizeWindow("video", 480, 960); 41 | 42 | std::string genpath = "C:/Users/dongfangzhao/Documents/Opencv/darknet-master/build/darknet/x64"; 43 | std::string names_file = genpath + "/data/obj.names"; 44 | std::string cfg_file = genpath + "/yolov3_I75_left.cfg"; 45 | std::string weights_file = last_cnn_path + "yolov3_I75_left_last.weights"; 46 | 47 | LIB_API::Detector yolo(cfg_file, weights_file, 0); 48 | auto obj_names = objects_names_from_file(names_file); 49 | vector bbox0; 50 | for (int i = 0; pf.exists_file(imgpath + to_string(i*freq) + ".jpg"); i++) { 51 | string img_i_path = imgpath + to_string(i*freq) + ".jpg"; 52 | Mat img; 53 | int imgnum = i*freq; 54 | load_frame(img_i_path, img); 55 | string img_path = learning_img_path + to_string(i) + ".jpg"; 56 | cv::imwrite(img_path, img); 57 | 58 | string txt_path = learning_img_path + to_string(i) + ".txt"; 59 | cout << i << "#"; 60 | bbox0 = yolo.detect(img, 0.5, true); 61 | writedata(txt_path, img, bbox0); 62 | } 63 | } 64 | 65 | 66 | 67 | 68 | -------------------------------------------------------------------------------- /step4_connection/Project1/get_mergedmarker.cpp: -------------------------------------------------------------------------------- 1 | #include "FRAMES.h" 2 | 3 | void FRAMES::get_mergedmarker_loc(vector frames, vector edge) { 4 | Public_func pf; 5 | vector g_path = { "N:/LEFT/","L:/MIDDLE/","N:/RIGHT/" }; 6 | string markers_path = "N:/LEFT/temp_data/" + to_string(iffirst) + "_marker_loc.csv"; 7 | if (!pf.exists_file(markers_path)){ 8 | load_markers_from_file(markers_path); 9 | } 10 | else { 11 | vector markers_p; 12 | vector left_markers, right_markers, middle_markers; 13 | load_left_right_trans_marker(left_markers, right_markers); 14 | load_middle_markers(frames[1], edge, middle_markers); 15 | calculate_load(left_markers, middle_markers, right_markers, frames); 16 | save_initial_markers(); 17 | } 18 | 19 | 20 | } -------------------------------------------------------------------------------- /step4_connection/Project1/get_motion_parameters.cpp: -------------------------------------------------------------------------------- 1 | #include "FRAMES.h" 2 | 3 | void FRAMES::get_motion_parameters() { 4 | //when we want to restart the process, load parameters and find initial fnum is needed; 5 | Mat cur_frame, pre_frame, cur_depth; 6 | //138001 or 0 7 | for (int fnum = 138001; fnum < 216000 ; fnum+=30) { 8 | if (!load_frame(fnum, cur_frame)){ 9 | save_results(pre_frame, fnum); 10 | break; 11 | } 12 | if (circle_mat.empty()){ 13 | load_depth(fnum, cur_depth); 14 | load_circle(cur_frame, cur_depth); 15 | if (pre_frame.empty()) 16 | pre_frame = cur_frame.clone(); 17 | } 18 | else{ 19 | bool matched = match_circles(cur_frame);//cur 20 | if (!matched) { 21 | cout << "circles matched unsuccefully, reload started" << endl; 22 | load_circle(cur_frame, cur_depth); 23 | continue; 24 | } 25 | } 26 | 27 | if ((fnum-1) % (1200) == 0)// * 33 28 | save_results(cur_frame, fnum);//cur 29 | cout << fnum << "-"; 30 | 31 | } 32 | 33 | } 34 | 35 | 36 | 37 | bool FRAMES::load_depth(int fnum, Mat &cur_depth) { 38 | string mat_path = frame_path + to_string(fnum) + ".jpg"; 39 | cur_depth = imread(mat_path, IMREAD_ANYDEPTH); 40 | if (cur_depth.empty()) 41 | return false; 42 | else return true; 43 | } 44 | 45 | //save frame with circles and locations(framenum, position (x,y) and radius) 46 | void FRAMES::save_results(Mat cur_frame, int fnum) { 47 | for(int i = 0; i < vector_c_p.back().size(); i ++) 48 | cv::circle(cur_frame, vector_c_p.back()[i], vector_c_r.back()[i], Scalar(255, 0, 0), 5); 49 | string img_path = temp_frame_path + to_string(fnum) + ".jpg"; 50 | cv::imwrite(img_path, cur_frame); 51 | 52 | ofstream ofstr; 53 | string csv_path_s = temp_data_path + to_string(fnum) + ".csv"; 54 | ofstr.open(csv_path_s.c_str()); 55 | for (int i = 0; i < vector_c_p.size(); i++) { 56 | ofstr << i << ","; 57 | for (int j = 0; j < vector_c_p[i].size(); j++) { 58 | ofstr << vector_c_p[i][j].x << "," << 59 | vector_c_p[i][j].y << "," << vector_c_r[i][j]<<","; 60 | } 61 | ofstr << "\n"; 62 | } 63 | } 64 | 65 | 66 | void FRAMES::push_points() { 67 | vector_c_p.push_back(circle_point); 68 | vector_c_r.push_back(circle_rad); 69 | } -------------------------------------------------------------------------------- /step4_connection/Project1/lanenum.py: -------------------------------------------------------------------------------- 1 | from sklearn.cluster import DBSCAN 2 | import numpy as np 3 | import csv 4 | from sklearn import metrics 5 | from sklearn.datasets.samples_generator import make_blobs 6 | from sklearn.preprocessing import StandardScaler 7 | import matplotlib.pyplot as plt 8 | 9 | data = [] 10 | with open('output.csv') as mycsv: 11 | csvread = csv.reader(mycsv) 12 | for row in csvread: 13 | t = [i for i in row] 14 | data.append(t) 15 | data = np.array(data) 16 | for k, col in zip(unique_labels, colors): 17 | plt.plot(xy[:, 0], xy[:, 1], 'o', markerfacecolor=tuple(col), 18 | markeredgecolor='w', markersize=4) 19 | plt.title('Estimated number of clusters: %d' % n_clusters_) 20 | plt.show() 21 | -------------------------------------------------------------------------------- /step4_connection/Project1/lanenumber1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATS-Lab/Filed-Experiment-Data-HIGH_Sim/b5f9845bd61591fdcd3e9101a22e458ca3afae18/step4_connection/Project1/lanenumber1.jpg -------------------------------------------------------------------------------- /step4_connection/Project1/lanenumberedtra1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATS-Lab/Filed-Experiment-Data-HIGH_Sim/b5f9845bd61591fdcd3e9101a22e458ca3afae18/step4_connection/Project1/lanenumberedtra1.png -------------------------------------------------------------------------------- /step4_connection/Project1/lanenumberedtraroundabouts.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATS-Lab/Filed-Experiment-Data-HIGH_Sim/b5f9845bd61591fdcd3e9101a22e458ca3afae18/step4_connection/Project1/lanenumberedtraroundabouts.jpg -------------------------------------------------------------------------------- /step4_connection/Project1/load_and_store_2.cpp: -------------------------------------------------------------------------------- 1 | #include "myVideo.h" 2 | #include "Veh.h" 3 | 4 | void myVideo::store2(const char* file, vector> V) { 5 | if (edge.empty()) 6 | edge = addedge(); 7 | //_mkdir(file); 8 | string s = file; 9 | string s_del = "rm -r "; 10 | s_del += s + "/*"; 11 | const char* todel = s_del.c_str(); 12 | cout << s_del << "\n" << todel << endl; 13 | //system(todel); 14 | int n = 0, cnum = 0, count = 0; 15 | 16 | ofstream myfile; 17 | string f_csv = s; 18 | const char* f_cs = f_csv.c_str(); 19 | myfile.open(f_cs); 20 | myfile << "i->num" << "," << "i->cx" << "," << "i->cy" << "," 21 | << "i->rect.x" << "," << "i->rect.y" << "," << "i->rect.width" << "," 22 | << "i->rect.height" << "," << "i->state" << ",i->ep[i],,,,,,,," << "time" << "\n"; 23 | while (1) { 24 | if (count == V.size()) 25 | break; 26 | Point2f pf[2]; 27 | for (auto &i : V[count]) { 28 | if (i->state > 0 && i->rect.x >= edge.x) { 29 | myfile << i->num << "," << i->cx << "," << i->cy << "," << i->rect.x << "," << i->rect.y << "," << i->rect.width << "," 30 | << i->rect.height << "," << i->state; 31 | for (int p = 0; p < 4; p++) 32 | myfile << "," << i->ep[2 * p] << "," << i->ep[2 * p + 1]; 33 | myfile << "," << i->ct; 34 | myfile << "\n"; 35 | } 36 | 37 | } 38 | count++; 39 | } 40 | cv::destroyAllWindows(); 41 | } 42 | 43 | void myVideo::load2(const char* file, vector> &V) { 44 | 45 | int num, x, y, w, h, state, count; 46 | float cx, cy, ep[8]; 47 | string snum, sx, sy, sw, sh, sstate, scx, scy, sep[8], scount; 48 | ifstream datafile(file); 49 | getline(datafile, snum); 50 | int count0 = 0, precount = 0; 51 | vector V0; 52 | while (getline(datafile, snum, ',')) { 53 | if (snum.length() < 2) { 54 | getline(datafile, snum); 55 | continue; 56 | } 57 | getline(datafile, scx, ','); 58 | getline(datafile, scy, ','); 59 | getline(datafile, sx, ','); 60 | getline(datafile, sy, ','); 61 | getline(datafile, sw, ','); 62 | getline(datafile, sh, ','); 63 | getline(datafile, sstate, ','); 64 | for (int i = 0; i < 8; i++) { 65 | getline(datafile, sep[i], ','); 66 | ep[i] = stof(sep[i]); 67 | } 68 | getline(datafile, scount); 69 | num = stoi(snum); 70 | cx = stof(scx); 71 | cy = stof(scy); 72 | x = stoi(sx); 73 | y = stoi(sy); 74 | w = stoi(sw); 75 | h = stoi(sh); 76 | state = stoi(sstate); 77 | count = stoi(scount); 78 | 79 | Veh *vehi = new Veh(num, cx, cy, x, y, w, h, state, ep, count); 80 | for (auto vehs : V0) 81 | if (vehs->num == vehi->num && count == precount) 82 | goto cont; 83 | if ((vehi->rect & block).area() > 0 || vehi->x > edge.x+edge.width || vehi->rect.width > 70 || !inEdge(vehi)) 84 | continue; 85 | //copy here new. 86 | if (count > V.size() + 1) { 87 | V.push_back(V0); 88 | V0.clear(); 89 | for (int i = 0; i < count - V.size() - 1; i++) 90 | V.push_back(V0); 91 | } 92 | if (count == precount) { 93 | if (!V.empty()) 94 | for (int i = 0; i < V.back().size(); i++) { 95 | if (V.back()[i]->num == vehi->num) { 96 | vehi->pv = V.back()[i]; 97 | V.back()[i]->nv = vehi; 98 | break; 99 | } 100 | } 101 | V0.push_back(vehi); 102 | } 103 | else { 104 | V.push_back(V0); 105 | V0.clear(); 106 | if (!V.empty()) 107 | for (int i = 0; i < V.back().size(); i++) { 108 | if (V.back()[i]->num == vehi->num) { 109 | vehi->pv = V.back()[i]; 110 | V.back()[i]->nv = vehi; 111 | break; 112 | } 113 | } 114 | V0.push_back(vehi); 115 | precount = count; 116 | } 117 | cont: 118 | continue; 119 | } 120 | V.push_back(V0); 121 | } -------------------------------------------------------------------------------- /step4_connection/Project1/load_block.cpp: -------------------------------------------------------------------------------- 1 | #include "myVideo.h" 2 | #include "Veh.h" 3 | Mat blockground; 4 | string mousewinname; 5 | void Cross_block(cv::Mat frame, cv::Point point, cv::Scalar s) { 6 | int cl = 10; 7 | cv::Point a(cl, 0), b(-cl, 0), c(0, cl), d(0, -cl); 8 | line(frame, point + a, point + b, s, 4); 9 | line(frame, point + c, point + d, s, 4); 10 | } 11 | 12 | void Mouseclick_block(int evt, int x, int y, int flags, void* edgepoints) { 13 | cv::Scalar s(0, 0, 255); 14 | std::vector* edgep = (std::vector*)edgepoints; 15 | if (evt == CV_EVENT_LBUTTONDOWN) { 16 | edgep->push_back(Point(x, y)); 17 | if (edgep->size() >= 1) { 18 | Point tem = edgep->back(); 19 | Cross(blockground, tem, s); 20 | } 21 | imshow(mousewinname, blockground); 22 | } 23 | } 24 | 25 | Rect myVideo::load_block() { 26 | vector blockpoints; 27 | string file = parapaths + "/block.txt"; 28 | Rect output; 29 | blockground = background.clone(); 30 | if (exists_file(file)) { 31 | ifstream input(file); 32 | string number; 33 | int paras[4]; 34 | int i = 0; 35 | while (i < 4) { 36 | getline(input, number); 37 | paras[i] = atoi(number.c_str()); 38 | i++; 39 | } 40 | output = Rect(paras[0], paras[1], paras[2], paras[3]); 41 | } 42 | else { 43 | namedWindow("Background", WINDOW_NORMAL | WINDOW_KEEPRATIO); 44 | cv::moveWindow("Background", 50, 50); 45 | cv::resizeWindow("Background", 1500, 750); 46 | cv::setMouseCallback("Background", Mouseclick_block, &blockpoints); 47 | imshow("Background", blockground); 48 | mousewinname = "Background"; 49 | char c = waitKey(0); 50 | if (c == 't') { 51 | if (blockpoints.size() > 3) { 52 | vector outpoints(blockpoints.end() - 4, blockpoints.end()); 53 | output = boundingRect(outpoints); 54 | } 55 | 56 | else { 57 | int x = 0.15 * blockground.cols, y = 0.05 * blockground.rows; 58 | int w = 0.7 * blockground.cols, h = 0.93 * blockground.rows; 59 | output = Rect(x, y, w, h); 60 | cout << "x" << x << "y" << y << "w" << w << "h" << h; 61 | } 62 | } 63 | output.x = max(50, output.x); 64 | output.height = min(YPIXEL - 50 - output.y, output.height); 65 | rectangle(blockground, output, Scalar(255), 5, 6, 0); 66 | imshow("Background", blockground); 67 | c = waitKey(3000); 68 | destroyAllWindows(); 69 | std::ofstream outfile(file); 70 | outfile << to_string(output.x) << std::endl; 71 | outfile << to_string(output.y) << std::endl; 72 | outfile << to_string(output.width) << std::endl; 73 | outfile << to_string(output.height) << std::endl; 74 | outfile.close(); 75 | } 76 | return output; 77 | } 78 | 79 | 80 | 81 | 82 | Rect myVideo::load_rect_etc(Mat frame_rted) { 83 | cout << "loading rect etc" << endl; 84 | vector blockpoints; 85 | string file = parapaths + "/rect_etc.txt"; 86 | Rect output; 87 | blockground = frame_rted.clone(); 88 | if (exists_file(file)) { 89 | ifstream input(file); 90 | string number; 91 | int paras[4]; 92 | int i = 0; 93 | while (i < 4) { 94 | getline(input, number); 95 | paras[i] = atoi(number.c_str()); 96 | i++; 97 | } 98 | output = Rect(paras[0], paras[1], paras[2], paras[3]); 99 | } 100 | else { 101 | string name = "rect_etc"; 102 | mousewinname = name; 103 | namedWindow(name, WINDOW_NORMAL | WINDOW_KEEPRATIO); 104 | cv::moveWindow(name, WINDOWSXP, 0); 105 | cv::resizeWindow(name, 400, 800); 106 | cv::setMouseCallback(name, Mouseclick_block, &blockpoints); 107 | reshow: 108 | imshow(name, blockground); 109 | char c = waitKey(0); 110 | if (c == 't') { 111 | if (blockpoints.size() > 3) { 112 | output = boundingRect(blockpoints); 113 | } 114 | else { 115 | cout << "rect_etc is not correctly loaded" << endl; 116 | blockground = frame_rted.clone(); 117 | blockpoints.clear(); 118 | goto reshow; 119 | } 120 | } 121 | rectangle(blockground, output, Scalar(255), 5, 6, 0); 122 | imshow(name, blockground); 123 | c = waitKey(2000); 124 | std::ofstream outfile(file); 125 | outfile << to_string(output.x) << std::endl; 126 | outfile << to_string(output.y) << std::endl; 127 | outfile << to_string(output.width) << std::endl; 128 | outfile << to_string(output.height) << std::endl; 129 | outfile.close(); 130 | destroyWindow(name); 131 | } 132 | return output; 133 | } 134 | -------------------------------------------------------------------------------- /step4_connection/Project1/load_cir_edge.cpp: -------------------------------------------------------------------------------- 1 | #include "FRAMES.h" 2 | 3 | void FRAMES::load_cir_edge_paras(vector>> &v_center, vector> &v_fnum, vector> &v_edge, vector &frames, string path) { 4 | vector g_path = { "N:/LEFT/","L:/MIDDLE/","N:/RIGHT/" }; 5 | v_center.resize(3); 6 | v_fnum.resize(3); 7 | v_edge.resize(3); 8 | frames.resize(3); 9 | double ratio[2][3] = { { 0.497326203, 1, 0.486631016 },{ 0.455782313, 1, 0.469387755 } }; 10 | int order[3] = { 1,9,5 }; 11 | for (int j = 0; j < g_path.size(); j++) { 12 | 13 | ratio_frames = ratio[iffirst][j]; 14 | //load_circle_parameters(g_path[j] + path, v_fnum[j], v_center[j], sti, j, 1); 15 | string edge_path = g_path[j] + "temp_data/" + to_string(iffirst) + "_edge.csv"; 16 | load_circle_parameters(g_path[j] + path, v_fnum[j], v_center[j], j, iffirst, 1); 17 | EDGE_FIT_ORDER = order[j]; 18 | load_edge_function(v_center[j], edge_path, v_fnum[j], v_edge[j]); 19 | load_frame(g_path[j] + "imgs/" + to_string(v_fnum[j][0]) + ".jpg", frames[j]); 20 | Size large_size = frames[j].size() * 2; 21 | int frame_W = large_size.width, frame_H = large_size.height; 22 | Mat shift_M = (Mat_(2, 3) << 1, 0, large_size.width / 4, 0, 1, large_size.height / 4); 23 | cv::warpAffine(frames[j], frames[j], shift_M, large_size); 24 | 25 | Size2i resized(frames[j].cols*ratio[iffirst][j], frames[j].rows*ratio[iffirst][j]); 26 | cv::resize(frames[j], frames[j], resized); 27 | } 28 | 29 | } -------------------------------------------------------------------------------- /step4_connection/Project1/manually_add_train_data.cpp: -------------------------------------------------------------------------------- 1 | #include "FRAMES.h" 2 | 3 | Mat theframe; 4 | void Cross2(cv::Mat &frame, cv::Point point, cv::Scalar s) { 5 | int cl = 6; 6 | cv::Point a(cl, 0), b(-cl, 0), c(0, cl), d(0, -cl); 7 | line(frame, point + a, point + b, s, 2); 8 | line(frame, point + c, point + d, s, 2); 9 | } 10 | 11 | void Mouseclick3(int evt, int x, int y, int flags, void* edgepoints) { 12 | cv::Scalar s(0, 0, 255); 13 | std::vector* edgep = (std::vector*)edgepoints; 14 | if (evt == CV_EVENT_LBUTTONDOWN) { 15 | Mat frame = theframe.clone(); 16 | edgep->push_back(Point(x, y)); 17 | if (edgep->size() >= 1) { 18 | for (int i = 0; i * 2 + 1 < edgep->size(); i++) 19 | rectangle(frame, Rect((*edgep)[i * 2], (*edgep)[i * 2 + 1]), Scalar(255, 100, 0), 3); 20 | Cross2(frame, edgep->back(), Scalar(0, 0, 255)); 21 | } 22 | imshow("video", frame); 23 | } 24 | if (evt == CV_EVENT_RBUTTONDOWN) { 25 | Mat frame = theframe.clone(); 26 | edgep->pop_back(); 27 | if (edgep->size() >= 1) { 28 | for (int i = 0; i * 2 + 1 < edgep->size(); i++) 29 | rectangle(frame, Rect((*edgep)[i * 2], (*edgep)[i * 2 + 1]), Scalar(255, 100, 0), 3); 30 | Cross2(frame, edgep->back(), Scalar(0, 0, 255)); 31 | } 32 | imshow("video", frame); 33 | } 34 | } 35 | 36 | void FRAMES::writedata(string txt_path, Mat img, vector bbox0) { 37 | vector contourpoints; 38 | cv::setMouseCallback("video", Mouseclick3, &contourpoints); 39 | double w = img.cols, h = img.rows; 40 | ofstream txtfile; 41 | txtfile.open(txt_path); 42 | 43 | for (auto i : bbox0){ 44 | rectangle(img, Rect(i.x, i.y, i.w, i.h), Scalar(0, 0, 255), 2); 45 | //txtfile<< (i.h > truck_len) << " " << (i.x+i.w/2) / w << " " << (i.y+i.h/2) / h << " " << i.w / w << " " << i.h / h << "\n"; 46 | } 47 | theframe = img.clone(); 48 | imshow("video", img); 49 | reload: 50 | char c = waitKey(0); 51 | if (c == 'c') { 52 | if (contourpoints.size() % 2) { 53 | cout << "points not correct, plz reload. \n"; 54 | contourpoints.clear(); 55 | goto reload; 56 | } 57 | for (int i = 0; i < contourpoints.size(); i += 2) { 58 | Point2i a = contourpoints[i], b = contourpoints[i + 1]; 59 | Rect r(a, b); 60 | rectangle(img, Rect(r.x, r.y, r.width, r.height), Scalar(0, 255, 255), 2); 61 | txtfile << (r.height > truck_len) << " " << (r.x+ r.width/2) / w << " " << (r.y+ r.height/2) / h << " " << r.width / w << " " << r.height / h << "\n"; 62 | for (int j = 0; j < bbox0.size(); j++){ 63 | Rect box(bbox0[j].x, bbox0[j].y, bbox0[j].w, bbox0[j].h); 64 | if ((box&r).area()>0 || box.area() < 50) 65 | bbox0.erase(bbox0.begin() + j--); 66 | } 67 | 68 | } 69 | for (auto i : bbox0) { 70 | txtfile << (i.h > truck_len) << " " << (i.x + i.w / 2) / w << " " << (i.y + i.h / 2) / h << " " << i.w / w << " " << i.h / h << "\n"; 71 | } 72 | } 73 | } -------------------------------------------------------------------------------- /step4_connection/Project1/marker_functions.cpp: -------------------------------------------------------------------------------- 1 | #include "FRAMES.h" 2 | 3 | void FRAMES::get_sub_imgs(GpuMat gmat, vector locs, vector edge, Point2i size, vector &output, vector &vrects) { 4 | Point outerp(size.x + size.y, size.x + size.y); 5 | Mat rotmat; 6 | GpuMat boundmat, bound_rot, gmati; 7 | for (int i = 0; i < locs.size(); i++) { 8 | Point2i p = locs[i]; 9 | int len = 14; 10 | double angle = atan(double(edge[p.y + 60] - edge[p.y]) / double(60)) * 180 / PI; 11 | //Mat rot_mat = getRotationMatrix2D(Point(x0, y0), -angle * 180 / PI, 1); 12 | Rect bound(p - outerp, p + outerp); 13 | boundmat = gmat(bound); 14 | //vrects.push_back(bound); 15 | rotmat = getRotationMatrix2D(outerp, -angle, 1); 16 | cuda::warpAffine(boundmat, bound_rot, rotmat, boundmat.size()); 17 | Rect cut(outerp - Point2i(size.x / 2, size.x / 2), outerp + Point2i(size.x / 2, len - size.x / 2)); 18 | gmati = bound_rot(cut); 19 | output.push_back(gmati.clone()); 20 | //vrects.push_back(bound); 21 | } 22 | } 23 | 24 | 25 | void FRAMES::detect_markers(GpuMat gmat, vector &forcasted_p, vector edge, Point2i size, vector &ori_p, vector &matched_p, vector &match_bool, int &acu, vector &vrects) { 26 | Mat frame; 27 | gmat.download(frame); 28 | 29 | Mat rotmat, vmr; 30 | GpuMat boundmat, bound_rot, gmati, g_res; 31 | 32 | cv::Ptr ptr; 33 | int match_method = CV_TM_CCOEFF_NORMED;// CV_TM_CCOEFF; 34 | ptr = cuda::createTemplateMatching(CV_8UC3, match_method, Size(0, 0)); 35 | for (int j = 0; j < forcasted_p.size(); j++) { 36 | Point outerp(marker_img[j].rows*3+marker_img[j].cols, marker_img[j].rows*3 + marker_img[j].cols); 37 | Point2i orip = forcasted_p[j]; 38 | int bdg = 0; 39 | if (orip.y >= gmat.rows - 50 || orip.x >= gmat.cols - 50 || orip.x <= 50 || orip.y < 50) 40 | bdg = 1; 41 | Vec3b cor; 42 | if (!bdg) 43 | cor = frame.at(orip); 44 | if (bdg || cor[0] == 0) { 45 | matched_p.push_back(forcasted_p[j]); 46 | match_bool.push_back(0); 47 | continue; 48 | } 49 | 50 | Point p = forcasted_p[j]; 51 | double angle = -atan(double(edge[p.y + 60] - edge[p.y]) / double(60)) * 180 / PI; 52 | //Mat rot_mat = getRotationMatrix2D(Point(x0, y0), -angle * 180 / PI, 1); 53 | Rect bound(p - outerp, p + outerp); 54 | bound.x = max(1, bound.x); 55 | bound.y = max(1, bound.y); 56 | boundmat = gmat(bound); 57 | 58 | rotmat = getRotationMatrix2D(outerp, angle, 1); 59 | cuda::warpAffine(boundmat, bound_rot, rotmat, boundmat.size()); 60 | Rect cut(outerp - Point2i(size.x / 2, marker_img[j].rows / 2), outerp + Point2i(size.x / 2, 1.5 * marker_img[j].rows + size.x/2)); 61 | gmati = bound_rot(cut); 62 | ptr->match(gmati, marker_img[j], g_res); 63 | Mat match_mat; 64 | g_res.download(match_mat); 65 | g_res.release(); 66 | 67 | 68 | Mat mker, outr; 69 | gmati.download(outr); 70 | marker_img[j].download(mker); 71 | 72 | //imwrite(temp_frame_path + "0_markeri.jpg", mker); 73 | //(temp_frame_path + "0_mouti.jpg", outr); 74 | 75 | 76 | 77 | float score = 0; 78 | Point2i matchLoc; 79 | if (!match_mat.empty()) { 80 | double minVal; double maxVal; Point minLoc; Point maxLoc; 81 | cv::minMaxLoc(match_mat, &minVal, &maxVal, &minLoc, &maxLoc, Mat()); 82 | maxLoc += Point2i(cut.x, cut.y); 83 | vmr = getRotationMatrix2D(outerp, -angle, 1); 84 | maxLoc += Point2i(marker_img[j].cols / 2, marker_img[j].cols / 2); 85 | matchLoc.x = maxLoc.x*vmr.at(0, 0) + maxLoc.y*vmr.at(0, 1) + vmr.at(0, 2); 86 | matchLoc.y = maxLoc.x*vmr.at(1, 0) + maxLoc.y*vmr.at(1, 1) + vmr.at(1, 2); 87 | score = maxVal; 88 | } 89 | if (score > 0.8) {// 90 | matchLoc += Point(bound.x, bound.y); 91 | matched_p.push_back(matchLoc); 92 | match_bool.push_back(1); 93 | vrects.push_back(Rect(matchLoc - Point(4, 4), matchLoc + Point(4, 4))); 94 | acu++; 95 | } 96 | else { 97 | matched_p.push_back(forcasted_p[j]); 98 | match_bool.push_back(0); 99 | } 100 | } 101 | 102 | 103 | } 104 | 105 | 106 | -------------------------------------------------------------------------------- /step4_connection/Project1/openh264-1.7.0-win64.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CATS-Lab/Filed-Experiment-Data-HIGH_Sim/b5f9845bd61591fdcd3e9101a22e458ca3afae18/step4_connection/Project1/openh264-1.7.0-win64.dll --------------------------------------------------------------------------------