diff --git a/demo/demo/demo.cpp b/demo/demo/demo.cpp index 5bed784..bca9c16 100644 --- a/demo/demo/demo.cpp +++ b/demo/demo/demo.cpp @@ -10,6 +10,7 @@ #include #include "Yolo3Detection.h" +#include "send.h" bool gRun; @@ -33,6 +34,7 @@ int main(int argc, char *argv[]) { tk::dnn::Yolo3Detection yolo; yolo.init(net); + yolo.thresh = 0.25; gRun = true; @@ -45,8 +47,21 @@ int main(int argc, char *argv[]) { cv::Mat frame; cv::Mat dnn_input; cv::namedWindow("detection", cv::WINDOW_NORMAL); + + /*CAMID*/ + const int CAM_IDX = 0; + + /*projection matrix*/ + float* proj_matrix = (float*) malloc(9*sizeof(float)); + int proj_matrix_read = 0; + + /*socket*/ + int sock; + int socket_opened = 0; while(gRun) { + + cap >> frame; if(!frame.data) { continue; @@ -57,6 +72,15 @@ int main(int argc, char *argv[]) { // TODO: async infer yolo.update(dnn_input); + int coord_i = 0; + struct obj_coords *coords = (struct obj_coords*)malloc(yolo.detected.size()*sizeof(struct obj_coords)); + if(proj_matrix_read == 0) + read_projection_matrix(proj_matrix, proj_matrix_read); + + /*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 +#include +#include +#include +#include //socket +#include //inet_addr +#include //write + +struct obj_coords +{ + float LAT; + float LONG; + float cl; +}; + + +char *serialize_coords(struct obj_coords *c, int obj_n, int CAM_IDX) +{ + + char *buffer = (char *)malloc(100000 * sizeof(char)); + + 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; + + sprintf(buffer, "m %lld %d %d ", t_stamp_ms, CAM_IDX, obj_n); + int i; + for (i = 0; i < obj_n; i++) + sprintf(buffer + strlen(buffer), "%.9f %.9f %.0f ", c[i].LAT, c[i].LONG, c[i].cl); + + //printf("%s\n", buffer); + + return buffer; +} + +struct obj_coords *deserialize_coords(char *buffer, int *obj_n) +{ + + int cam_id; + unsigned long long t_stamp_ms; + char type_of_m; + + int consumed_chars = 0; + char shifted_chars[100000]; + + sscanf(buffer, "%c %lld %d %d ", &type_of_m, &t_stamp_ms, &cam_id, obj_n); + sprintf(shifted_chars, "%c %lld %d %d ", type_of_m, t_stamp_ms, cam_id, *obj_n); + consumed_chars += strlen(shifted_chars); + //printf("%c %lld %d %d\n", type_of_m, t_stamp_ms, cam_id, *obj_n); + + struct obj_coords *c = (struct obj_coords *)malloc(*obj_n * sizeof(struct obj_coords)); + + int i; + for (i = 0; i < *obj_n; i++) + { + sscanf(buffer + consumed_chars, "%f %f %f ", &c[i].LAT, &c[i].LONG, &c[i].cl); + sprintf(shifted_chars, "%.9f %.9f %.0f ", c[i].LAT, c[i].LONG, c[i].cl); + consumed_chars += strlen(shifted_chars); + //printf("%Lf %f %f \n", c[i].LAT, c[i].LONG, c[i].cl); + } + + return c; +} + +int map_class_coco_to_voc(int coco_class) +{ + switch (coco_class) + { + case 0: + return 14; //person + case 1: + return 1; //bicycle + case 2: + return 6; //car + case 3: + return 13; //motorkbike + case 5: + return 5; //bus + } + return -1; +} + +void convert_coords(struct obj_coords *coords, int i, int x, int y, int detected_class,float * proj_matrix) +{ + + + + 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"); + coords[i].cl = map_class_coco_to_voc(detected_class); +} + +void read_projection_matrix(float * proj_matrix, int &proj_matrix_read) +{ + FILE *fp; + char *line = NULL; + size_t len = 0; + ssize_t read; + + fp = fopen("/home/classfog1/repos/MASA_server/pmatrix/proj_matrix_20937.txt", "r"); + if (fp == NULL) + exit(EXIT_FAILURE); + + 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])) + { + i++; + proj_matrix_read = 1; + } + } + free(line); + fclose(fp); +} + +int open_socket(char *ip, int &sock, int &socket_opened) +{ + struct sockaddr_in server; + sock = socket(AF_INET, SOCK_STREAM, 0); + if (sock == -1) + { + printf("Could not create socket"); + } + puts("Socket created"); + + server.sin_addr.s_addr = inet_addr(ip); + server.sin_family = AF_INET; + server.sin_port = htons(8888); + + /*connect to remote server*/ + if (connect(sock, (struct sockaddr *)&server, sizeof(server)) < 0) + { + perror("connect failed. Error"); + socket_opened = 0; + return 0; + } + puts("Connected\n"); + socket_opened = 1; + return 1; +} + +int send_client_dummy(struct obj_coords *coords, int n_coords, int &sock, int &socket_opened, int CAM_IDX) +{ + /*serialize coords*/ + char *message = serialize_coords(coords, n_coords, CAM_IDX); + + /*open socket if not already opened*/ + if (socket_opened == 0) + { + int res = open_socket("127.0.0.1",sock, socket_opened); + if(res) + printf("Socket opened!\n"); + else + { + printf("Problem: socket NOT opened!\n"); + return 0; + } + } + + /*send message to server*/ + if (send(sock, message, strlen(message), 0) < 0) + { + puts("Send failed"); + socket_opened = 0; + } + + free(message); + //close(sock); + return 1; +} + +#endif /*SEND_H*/ \ No newline at end of file diff --git a/src/Yolo3Detection.cpp b/src/Yolo3Detection.cpp index e121503..1e63f0d 100644 --- a/src/Yolo3Detection.cpp +++ b/src/Yolo3Detection.cpp @@ -57,6 +57,60 @@ bool Yolo3Detection::init(std::string tensor_path) { return true; } +void Yolo3Detection::addBorders(cv::Mat &imageORIG, cv::Mat &imageWBorders, int &top, int &left) +{ + float net_ratio = float(netRT->input_dim.w)/float(netRT->input_dim.h); + float img_ratio = float(imageORIG.cols)/float(imageORIG.rows); + int bottom=0, right=0, diff= 0; + top=0, left=0; + + //printf("%f %f\n", net_ratio, img_ratio); + + if(net_ratio != img_ratio) + { + if(netRT->input_dim.w> netRT->input_dim.h) + { + if(img_ratio > net_ratio) + { + diff = std::abs((imageORIG.cols - net_ratio*imageORIG.rows)/net_ratio); + top = diff/2; + bottom = diff/2 + diff%2; + } + else + { + diff = std::abs(net_ratio*float(imageORIG.rows) - float(imageORIG.cols)); + left = diff/2; + right = diff/2 + diff%2; + } + } + else + { + if(img_ratio < net_ratio) + { + diff = std::abs((imageORIG.cols - net_ratio*imageORIG.rows)/net_ratio); + left = diff/2; + right = diff/2 + diff%2; + } + else + { + diff = std::abs(net_ratio*float(imageORIG.rows) - float(imageORIG.cols)); + top = diff/2; + bottom = diff/2 + diff%2; + } + } + + } + + //printf("%d %d %d %d %d \n", diff, top, bottom, left, right); + imageWBorders = imageORIG; + copyMakeBorder( imageORIG, imageWBorders, top, bottom, left, right, cv::BORDER_CONSTANT, (0,0,0) ); + //printf("%d %d\n", imageWBorders.cols, imageWBorders.rows); + //const char* window_name = "borders"; + //cv::namedWindow( window_name, cv::WINDOW_AUTOSIZE ); + //imshow( window_name, imageWBorders ); + //cv::waitKey(0); +} + void Yolo3Detection::update(cv::Mat &imageORIG) { @@ -64,12 +118,21 @@ void Yolo3Detection::update(cv::Mat &imageORIG) { std::cout<<"YOLO: NO IMAGE DATA\n"; return; } - float xRatio = float(imageORIG.cols) / float(netRT->input_dim.w); - float yRatio = float(imageORIG.rows) / float(netRT->input_dim.h); + + int top, left; + cv::Mat imageWBorders; + addBorders(imageORIG, imageWBorders, top, left); - resize(imageORIG, imageORIG, cv::Size(netRT->input_dim.w, netRT->input_dim.h)); + float xRatio = float(imageWBorders.cols) / float(netRT->input_dim.w); + float yRatio = float(imageWBorders.rows) / float(netRT->input_dim.h); + + resize(imageWBorders, imageORIG, cv::Size(netRT->input_dim.w, netRT->input_dim.h)); imageORIG.convertTo(imageF, CV_32FC3, 1/255.0); + //const char* window_name = "resize"; + //cv::namedWindow( window_name, cv::WINDOW_AUTOSIZE ); + ///imshow( window_name, imageORIG ); + //split channels cv::split(imageF,bgr);//split source @@ -127,10 +190,10 @@ void Yolo3Detection::update(cv::Mat &imageORIG) { //cv::rectangle(image, cv::Point(x0, y0), cv::Point(x1, y1), colors[obj_class], 2); // convert to image coords - x0 = xRatio*x0; - x1 = xRatio*x1; - y0 = yRatio*y0; - y1 = yRatio*y1; + x0 = xRatio*x0 - left; + x1 = xRatio*x1 - left; + y0 = yRatio*y0 - top; + y1 = yRatio*y1 - top; tk::dnn::box res; res.cl = obj_class;