diff --git a/CMakeLists.txt b/CMakeLists.txt index 64b44ad..e488a04 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -34,7 +34,7 @@ cuda_include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include ${CUDA_INCLUDE_DIRS cuda_add_library(kernels SHARED ${tkdnn_CUSRC}) file(GLOB tkdnn_SRC "src/*.cpp") -set(tkdnn_LIBS kernels ${CUDA_LIBRARIES} ${CUDA_CUBLAS_LIBRARIES} -lcudnn -lnvinfer ${OpenCV_LIBS}) +set(tkdnn_LIBS kernels ${CUDA_LIBRARIES} ${CUDA_CUBLAS_LIBRARIES} -lcudnn -lnvinfer ${OpenCV_LIBS} -lgdal) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -std=c++11") include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include ${CUDA_INCLUDE_DIRS} ${OPENCV_INCLUDE_DIRS} ${NVINFER_INCLUDES}) diff --git a/demo/demo/demo.cpp b/demo/demo/demo.cpp index 98ebc49..c7206c8 100644 --- a/demo/demo/demo.cpp +++ b/demo/demo/demo.cpp @@ -12,6 +12,7 @@ #include "Yolo3Detection.h" #include "send.h" + #define MAX_DETECT_SIZE 100 @@ -38,13 +39,16 @@ int main(int argc, char *argv[]) { char *pmatrix = "/home/classfog1/repos/MASA_server/pmatrix/proj_matrix_20937.txt"; if(argc > 3) pmatrix = argv[3]; + char *tiffile = "/home/davide/repos/projection_tool/img/map_b.tif"; + if(argc > 4) + tiffile = argv[4]; /*CAMID*/ int CAM_IDX = 0; - if(argc > 4) - CAM_IDX = atoi(argv[4]); - bool to_show = true; if(argc > 5) - to_show = atoi(argv[5]); + CAM_IDX = atoi(argv[5]); + bool to_show = true; + if(argc > 6) + to_show = atoi(argv[6]); tk::dnn::Yolo3Detection yolo; yolo.init(net); @@ -63,8 +67,14 @@ int main(int argc, char *argv[]) { cv::namedWindow("detection", cv::WINDOW_NORMAL); /*projection matrix*/ - float* proj_matrix = (float*) malloc(9*sizeof(float)); + int proj_matrix_read = 0; + cv::Mat H(cv::Size(3,3),CV_64FC1); + + /*GPS information*/ + double *adfGeoTransform = (double*)malloc(6*sizeof(double)); + readTiff(tiffile, adfGeoTransform); + /*socket*/ int sock; @@ -72,6 +82,7 @@ int main(int argc, char *argv[]) { struct obj_coords *coords = (struct obj_coords*)malloc(MAX_DETECT_SIZE*sizeof(struct obj_coords)); + int frame_nbr = 0; while(gRun) { @@ -86,12 +97,13 @@ int main(int argc, char *argv[]) { 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) - read_projection_matrix(proj_matrix, proj_matrix_read, pmatrix); + 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], @@ -110,7 +122,7 @@ int main(int argc, char *argv[]) { if(obj_class == 0 /*person*/ || obj_class == 1/*bicycle*/ || obj_class == 2/*car*/ || obj_class == 3/*motorbike*/ || obj_class == 5/*bus*/) { - convert_coords(coords, coord_i,x0+b.w/2, y1,obj_class, proj_matrix); + convert_coords(coords, coord_i,x0+b.w/2, y1,obj_class, H, adfGeoTransform, frame_nbr); coord_i++; } @@ -120,6 +132,8 @@ int main(int argc, char *argv[]) { cv::rectangle(frame, cv::Point(x0, y0), cv::Point(x1, y1), yolo.colors[obj_class], 2); } + frame_nbr++; + send_client_dummy(coords, coord_i, sock, socket_opened, CAM_IDX); if (to_show) @@ -130,7 +144,7 @@ int main(int argc, char *argv[]) { } free(coords); - free(proj_matrix); + free(adfGeoTransform); std::cout<<"detection end\n"; return 0; diff --git a/include/send.h b/include/send.h index 2ecf7aa..d7e2169 100644 --- a/include/send.h +++ b/include/send.h @@ -9,6 +9,16 @@ #include //inet_addr #include //write +#include +#include +#include "gdal.h" +#include +#include +#include "gdal/gdal_priv.h" +#include "gdal/cpl_conv.h" + + + struct obj_coords { float LAT; @@ -17,6 +27,47 @@ struct obj_coords }; +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 ); + } +} + +void pixel2coord(int x, int y, double &lat, double &lon, double *adfGeoTransform) +{ + + 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 ); + + lon = a * x + b * y + xoff; + lat = d * x + e * y + yoff; + +} + +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< x_y, ll; + x_y.push_back(cv::Point2f(x, y)); - - - float obj_x = x, obj_y = y, obj_z = 1; - float tmp_z = 0; - - coords[i].LAT = proj_matrix[0] * obj_x + proj_matrix[1] * obj_y + proj_matrix[2] * obj_z; - coords[i].LONG = proj_matrix[3] * obj_x + proj_matrix[4] * obj_y + proj_matrix[5] * obj_z; - tmp_z = proj_matrix[6] * obj_x + proj_matrix[7] * obj_y + proj_matrix[8] * obj_z; - - if(tmp_z != 0.0) - { - coords[i].LAT = coords[i].LAT / tmp_z; - coords[i].LONG = coords[i].LONG / tmp_z; - //printf("lat: %f, long %f\n", coords[i].LAT, coords[i].LONG); - } - else - printf("Division by 0 (tmp_z)\n"); + //transform camera pixel to map pixel + cv::perspectiveTransform( x_y, ll, H); + //tranform to map pixel to map gps + 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) + { + + 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); + + } + } -void read_projection_matrix(float * proj_matrix, 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)); + fp = fopen(path, "r"); if (fp == NULL) exit(EXIT_FAILURE); @@ -128,6 +189,9 @@ void read_projection_matrix(float * proj_matrix, int &proj_matrix_read, char* pa } free(line); fclose(fp); + fillMatrix(H, proj_matrix); + + free(proj_matrix); } int open_socket(char *ip, int &sock, int &socket_opened)