From 9b413b77ab68949ff93708567c7b1a07656ae576 Mon Sep 17 00:00:00 2001 From: Micaela Verucchi Date: Sat, 20 Apr 2019 15:48:52 +0200 Subject: [PATCH] tracking integrated --- demo/demo/data/proj_matrix_map_b.txt | 6 +- demo/demo/demo.cpp | 158 ++++++++++++++------------- demo/server/server_less_dummy.cpp | 4 +- include/send.h | 111 +++++++++---------- tracker_CLASS | 2 +- 5 files changed, 139 insertions(+), 142 deletions(-) diff --git a/demo/demo/data/proj_matrix_map_b.txt b/demo/demo/data/proj_matrix_map_b.txt index 9304877..fdbe397 100644 --- a/demo/demo/data/proj_matrix_map_b.txt +++ b/demo/demo/data/proj_matrix_map_b.txt @@ -1,3 +1,3 @@ -0.1472053693584627 -13.052205853952705 439.9459667398692 --0.6469057430349019 -6.339418443938649 578.7156423035825 --0.00025681337819676855 -0.007446975429175878 1.0 +-0.4122368700442484 -10.479982981650977 812.1821012303 +-0.6536846365005352 -4.951476039607947 512.0831604767536 +-0.00042630219831388434 -0.006003223898658603 1.0 diff --git a/demo/demo/demo.cpp b/demo/demo/demo.cpp index 173ed5e..3dc9c84 100644 --- a/demo/demo/demo.cpp +++ b/demo/demo/demo.cpp @@ -1,6 +1,6 @@ #include #include -#include /* srand, rand */ +#include /* srand, rand */ #include #include #include "utils.h" @@ -16,42 +16,40 @@ #include "plot.h" #include "tracker.h" - #define MAX_DETECT_SIZE 100 - - bool gRun; -void sig_handler(int signo) { - std::cout<<"request gateway stop\n"; +void sig_handler(int signo) +{ + std::cout << "request gateway stop\n"; gRun = false; } -int main(int argc, char *argv[]) { +int main(int argc, char *argv[]) +{ - std::cout<<"detection\n"; + std::cout << "detection\n"; signal(SIGINT, sig_handler); - char *net = "yolo3_coco4.rt"; - if(argc > 1) - net = argv[1]; + if (argc > 1) + net = argv[1]; char *input = "../demo/demo/data/single_ped_2.mp4"; - if(argc > 2) - input = argv[2]; + if (argc > 2) + input = argv[2]; char *pmatrix = "../demo/demo/data/proj_matrix_map_b.txt"; - if(argc > 3) + if (argc > 3) pmatrix = argv[3]; char *tiffile = "../demo/demo/data/map_b.tif"; - if(argc > 4) + if (argc > 4) tiffile = argv[4]; /*CAMID*/ int CAM_IDX = 0; - if(argc > 5) + if (argc > 5) CAM_IDX = atoi(argv[5]); bool to_show = true; - if(argc > 6) + if (argc > 6) to_show = atoi(argv[6]); tk::dnn::Yolo3Detection yolo; @@ -61,24 +59,23 @@ int main(int argc, char *argv[]) { gRun = true; cv::VideoCapture cap(input); - if(!cap.isOpened()) - gRun = false; + if (!cap.isOpened()) + gRun = false; else - std::cout<<"camera started\n"; + std::cout << "camera started\n"; cv::Mat frame; cv::Mat dnn_input; - cv::namedWindow("detection", cv::WINDOW_NORMAL); + cv::namedWindow("detection", cv::WINDOW_NORMAL); /*projection matrix*/ - + int proj_matrix_read = 0; - cv::Mat H(cv::Size(3,3),CV_64FC1); + cv::Mat H(cv::Size(3, 3), CV_64FC1); /*GPS information*/ - double *adfGeoTransform = (double*)malloc(6*sizeof(double)); + double *adfGeoTransform = (double *)malloc(6 * sizeof(double)); readTiff(tiffile, adfGeoTransform); - /*socket*/ int sock; @@ -86,7 +83,7 @@ int main(int argc, char *argv[]) { /*Conversion for tracker, from gps to meters and viceversa*/ geodetic_converter::GeodeticConverter gc; - gc.initialiseReference(44.655540,10.934315, 0); + gc.initialiseReference(44.655540, 10.934315, 0); double east, north, up; double lat, lon, alt; @@ -94,83 +91,83 @@ int main(int argc, char *argv[]) { std::vector trackers; std::vector cur_frame; int initial_age = -5; - int age_threshold = -10; + int age_threshold = -20; int n_states = 5; float dt = 0.03; - struct obj_coords *coords = (struct obj_coords*)malloc(MAX_DETECT_SIZE*sizeof(struct obj_coords)); - + struct obj_coords *coords = (struct obj_coords *)malloc(MAX_DETECT_SIZE * sizeof(struct obj_coords)); int frame_nbr = 0; - while(gRun) { + while (gRun) + { - - cap >> frame; - if(!frame.data) { + cap >> frame; + if (!frame.data) + { usleep(1000000); cap.open(input); printf("cap reinitialize\n"); continue; + } - } - // this will be resized to the net format dnn_input = frame.clone(); // TODO: async infer yolo.update(dnn_input); int coord_i = 0; - + int num_detected = yolo.detected.size(); if (num_detected > MAX_DETECT_SIZE) num_detected = MAX_DETECT_SIZE; - if(proj_matrix_read == 0) + if (proj_matrix_read == 0) + { read_projection_matrix(H, proj_matrix_read, pmatrix); - + } + /*printf("%f %f %f \n%f %f %f\n %f %f %f\n\n", proj_matrix[0],proj_matrix[1], proj_matrix[2],proj_matrix[3],proj_matrix[4],proj_matrix[5], proj_matrix[6],proj_matrix[7],proj_matrix[8]);*/ // draw dets - for(int i=0; i 0) - cv::circle( frame, cv::Point( pix_x, pix_y ), 10.0, cv::Scalar( 255, 0, 0 ), CV_FILLED, 8, 0); + for(auto pred_pos: t.pred_list_ ) + { + + gc.enu2Geodetic(pred_pos.x_, pred_pos.y_, 0, &lat, &lon, &alt); + //std::cout << "lat: " << lat << " lon: " << lon << std::endl; + int pix_x, pix_y; + coord2pixel(lat, lon, pix_x, pix_y, adfGeoTransform); + //std::cout << "pix_x: " << pix_x << " pix_y: " << pix_y << std::endl; + + std::vector map_p, camera_p; + map_p.push_back(cv::Point2f(pix_x, pix_y)); + + //transform camera pixel to map pixel + cv::perspectiveTransform(map_p, camera_p, H.inv()); + //std::cout << "pix_x: " << camera_p[0].x << " pix_y: " << camera_p[0].y << std::endl; + + cv::circle(frame, cv::Point(camera_p[0].x, camera_p[0].y), 3.0, cv::Scalar(255, 0, 0), CV_FILLED, 8, 0); + + } } frame_nbr++; send_client_dummy(coords, coord_i, sock, socket_opened, CAM_IDX); - + if (to_show) - { - cv::imshow("detection", frame); - cv::waitKey(1); - } + { + cv::imshow("detection", frame); + cv::waitKey(1); + } } - -/* for (size_t i = 0; i < trackers.size(); i++) + /* for (size_t i = 0; i < trackers.size(); i++) if (trackers[i].z_list_.size() > 10) plotTruthvsPred(trackers[i].z_list_, trackers[i].pred_list_); */ free(coords); free(adfGeoTransform); - std::cout<<"detection end\n"; + std::cout << "detection end\n"; return 0; } - diff --git a/demo/server/server_less_dummy.cpp b/demo/server/server_less_dummy.cpp index 42add83..70ff7d0 100644 --- a/demo/server/server_less_dummy.cpp +++ b/demo/server/server_less_dummy.cpp @@ -211,8 +211,8 @@ void *connection_handler(void *socket_desc) //Get the socket descriptor int sock = *(int *)socket_desc; int read_size; - - void *client_message = (void*)malloc(message_size); + + void *client_message = (void *)malloc(message_size); /* //Send some messages to the client message = "Greetings! I am your connection handler\n"; diff --git a/include/send.h b/include/send.h index b1a2f51..294572a 100644 --- a/include/send.h +++ b/include/send.h @@ -17,8 +17,6 @@ #include "gdal/gdal_priv.h" #include "gdal/cpl_conv.h" - - #include "serialize.hpp" struct obj_coords @@ -28,78 +26,74 @@ struct obj_coords float cl; }; - -void readTiff(char*filename, double *adfGeoTransform) +void readTiff(char *filename, double *adfGeoTransform) { - GDALDataset *poDataset; - GDALAllRegister(); - poDataset = (GDALDataset *) GDALOpen( filename, GA_ReadOnly ); - if( poDataset != NULL ) - { - //int colms = poDataset->GetRasterXSize(); - //int rows = poDataset->GetRasterYSize(); - poDataset->GetGeoTransform( adfGeoTransform ); - } + GDALDataset *poDataset; + GDALAllRegister(); + poDataset = (GDALDataset *)GDALOpen(filename, GA_ReadOnly); + if (poDataset != NULL) + { + //int colms = poDataset->GetRasterXSize(); + //int rows = poDataset->GetRasterYSize(); + poDataset->GetGeoTransform(adfGeoTransform); + } } void pixel2coord(int x, int y, double &lat, double &lon, double *adfGeoTransform) { - //Returns global coordinates from pixel x, y coordinates - double xoff, a, b, yoff, d, e; - xoff = adfGeoTransform[0]; - a = adfGeoTransform[1]; - b = adfGeoTransform[2]; - yoff = adfGeoTransform[3]; - d = adfGeoTransform[4]; - e = adfGeoTransform[5]; + //Returns global coordinates from pixel x, y coordinates + double xoff, a, b, yoff, d, e; + xoff = adfGeoTransform[0]; + a = adfGeoTransform[1]; + b = adfGeoTransform[2]; + yoff = adfGeoTransform[3]; + d = adfGeoTransform[4]; + e = adfGeoTransform[5]; - //printf("%f %f %f %f %f %f\n",xoff, a, b, yoff, d, e ); + //printf("%f %f %f %f %f %f\n",xoff, a, b, yoff, d, e ); lon = a * x + b * y + xoff; lat = d * x + e * y + yoff; - } -void coord2pixel(double lat, double lon, int &x, int &y, double *adfGeoTransform) +void coord2pixel(double lat, double lon, int &x, int &y, double *adfGeoTransform) { - x = int(round((lon-adfGeoTransform[0])/adfGeoTransform[1])); - y = int(round((lat-adfGeoTransform[3])/adfGeoTransform[5])); + x = int(round((lon - adfGeoTransform[0]) / adfGeoTransform[1])); + y = int(round((lat - adfGeoTransform[3]) / adfGeoTransform[5])); } - -void fillMatrix(cv::Mat &H, float *matrix, bool show=false) +void fillMatrix(cv::Mat &H, float *matrix, bool show = false) { - double *vals = (double*) H.data; - for(int i=0; i<9; i++) { - vals[i] = matrix[i]; - } - if(show) - std::cout< ruv; int i; for (i = 0; i < obj_n; i++) { - Road_User r{c[i].LAT,c[i].LONG,0,0,(int)c[i].cl}; + Road_User r{c[i].LAT, c[i].LONG, 0, 0, (int)c[i].cl}; ruv.push_back(r); } - Message m{CAM_IDX,t_stamp_ms,ruv.size(),ruv}; + Message m{CAM_IDX, t_stamp_ms, ruv.size(), ruv}; archive(m); - //std::cout<str()< x_y, ll; + std::vector x_y, ll; x_y.push_back(cv::Point2f(x, y)); //transform camera pixel to map pixel - cv::perspectiveTransform( x_y, ll, H); + cv::perspectiveTransform(x_y, ll, H); //tranform to map pixel to map gps - pixel2coord(ll[0].x, ll[0].y, latitude,longitude, adfGeoTransform); + pixel2coord(ll[0].x, ll[0].y, latitude, longitude, adfGeoTransform); //printf("lat: %f, long:%f \n", latitude, longitude); coords[i].LAT = latitude; coords[i].LONG = longitude; coords[i].cl = map_class_coco_to_voc(detected_class); - if(detected_class == 0) + if (detected_class == 0) { - + struct timeval tv; gettimeofday(&tv, NULL); unsigned long long t_stamp_ms = (unsigned long long)(tv.tv_sec) * 1000 + (unsigned long long)(tv.tv_usec) / 1000; //fprintf(out_file, "%d %lld %d %d\n",frame_nbr, t_stamp_ms, int(ll[0].x), int(ll[0].y)); - fprintf(out_file, "%d %lld %f %f\n",frame_nbr, t_stamp_ms, coords[i].LAT, coords[i].LONG); - + fprintf(out_file, "%d %lld %f %f\n", frame_nbr, t_stamp_ms, coords[i].LAT, coords[i].LONG); } - } -void read_projection_matrix(cv::Mat &H, int &proj_matrix_read, char* path) +void read_projection_matrix(cv::Mat &H, int &proj_matrix_read, char *path) { FILE *fp; char *line = NULL; size_t len = 0; ssize_t read; - float* proj_matrix = (float*) malloc(9*sizeof(float)); + float *proj_matrix = (float *)malloc(9 * sizeof(float)); fp = fopen(path, "r"); if (fp == NULL) @@ -198,7 +190,7 @@ void read_projection_matrix(cv::Mat &H, int &proj_matrix_read, char* path) int i = 0; while ((read = getline(&line, &len, fp)) != -1) { - if (3 == sscanf(line, "%f %f %f", &proj_matrix[i*3+0], &proj_matrix[i*3+1], &proj_matrix[i*3+2])) + if (3 == sscanf(line, "%f %f %f", &proj_matrix[i * 3 + 0], &proj_matrix[i * 3 + 1], &proj_matrix[i * 3 + 2])) { i++; proj_matrix_read = 1; @@ -206,7 +198,9 @@ void read_projection_matrix(cv::Mat &H, int &proj_matrix_read, char* path) } free(line); fclose(fp); - fillMatrix(H, proj_matrix); + fillMatrix(H, proj_matrix); + + free(proj_matrix); } @@ -219,7 +213,7 @@ int open_socket(char *ip, int &sock, int &socket_opened) { printf("Could not create socket"); } - puts("Socket created"); + puts("Socket created"); server.sin_addr.s_addr = inet_addr(ip); server.sin_family = AF_INET; @@ -243,14 +237,13 @@ int send_client_dummy(struct obj_coords *coords, int n_coords, int &sock, int &s std::stringbuf *message = new std::stringbuf(); serialize_coords(coords, n_coords, CAM_IDX, message); - //std::cout<str().length()<str().data(), message->str().length(), 0) < 0) { diff --git a/tracker_CLASS b/tracker_CLASS index 135e47b..29d9cdf 160000 --- a/tracker_CLASS +++ b/tracker_CLASS @@ -1 +1 @@ -Subproject commit 135e47ba2e2202ad02fd95f2d16bb9ea9abfc29b +Subproject commit 29d9cdf1bd718cb4a0d34e31c0a65e62dafdd673