From a6d19d3698a918ddd7fda1310f7c096f2ebd37ad Mon Sep 17 00:00:00 2001 From: mive93 Date: Wed, 8 May 2019 20:08:20 +0200 Subject: [PATCH] order --- demo/demo/demo.cpp | 106 +++++---------------------------------- include/classutils.h | 115 ++++++++++++++++++++++++++++++++++++------- 2 files changed, 108 insertions(+), 113 deletions(-) diff --git a/demo/demo/demo.cpp b/demo/demo/demo.cpp index a361b20..fbfc6b3 100644 --- a/demo/demo/demo.cpp +++ b/demo/demo/demo.cpp @@ -6,7 +6,6 @@ #include #include -#include #include "utils.h" #include @@ -50,63 +49,6 @@ void *showImages(void *x_void_ptr) } } -void draw_arrow(float angleRad, float vel, cv::Scalar color, cv::Point center, cv::Mat &frame) -{ - int angle = angleRad * 180.0 / CV_PI; - auto length = 10 * vel; - auto direction = cv::Point(length * cos(angleRad), length * sin(angleRad)); // calculate direction - double tipLength = .2 + 0.4 * (angle % 180) / 360; - int lineType = 8; - int thickness = 2; - cv::arrowedLine(frame, center, center + direction, color, thickness, lineType, 0, tipLength); // draw arrow! -} - -unsigned long long time_in_ms() -{ - 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; - return t_stamp_ms; -} - -void prepare_message(Message *m, struct obj_coords *coords, int n_coords, int idx) -{ - m->cam_idx = idx; - m->t_stamp_ms = time_in_ms(); - m->num_objects = n_coords; - - m->objects.clear(); - for (int i = 0; i < n_coords ; i++) - { - Categories cat; - switch (static_cast(coords[i].cl)) - { - case 0: - cat = Categories::C_person; - break; - case 1: - cat = Categories::C_car; - break; - case 2: - cat = Categories::C_car; - break; - case 3: - cat = Categories::C_bus; - break; - case 4: - cat = Categories::C_motorbike; - break; - case 5: - cat = Categories::C_bycicle; - break; - } - RoadUser r{coords[i].LAT, coords[i].LONG, 0, 1, C_car}; - m->objects.push_back(r); - } - - m->lights.clear(); -} - int main(int argc, char *argv[]) { @@ -166,33 +108,21 @@ int main(int argc, char *argv[]) } /*projection matrix from camera to map*/ - int proj_matrix_read = 0; cv::Mat H(cv::Size(3, 3), CV_64FC1); + read_projection_matrix(H, pmatrix); /*Camera calibration*/ - YAML::Node config = YAML::LoadFile(cameraCalib); - const YAML::Node &node_test1 = config["camera_matrix"]; - - float data_cm[9]; - for (std::size_t i = 0; i < node_test1["data"].size(); i++) - data_cm[i] = node_test1["data"][i].as(); - cv::Mat cameraMat = cv::Mat(3, 3, CV_32F, data_cm); + cv::Mat cameraMat, distCoeff; + readCameraCalibrationYaml(cameraCalib, cameraMat, distCoeff); std::cout << cameraMat << std::endl; - const YAML::Node &node_test2 = config["distortion_coefficients"]; - - float data_dc[5]; - for (std::size_t i = 0; i < node_test2["data"].size(); i++) - data_dc[i] = node_test2["data"][i].as(); - cv::Mat distCoeff = cv::Mat(5, 1, CV_32F, data_dc); std::cout << distCoeff << std::endl; /*GPS information*/ double *adfGeoTransform = (double *)malloc(6 * sizeof(double)); readTiff(tiffile, adfGeoTransform); - struct obj_coords *coords = (struct obj_coords *)malloc(MAX_DETECT_SIZE * sizeof(struct obj_coords)); + std::vector coords; /*socket*/ - Communicator Comm(SOCK_DGRAM); Comm.open_client_socket("127.0.0.1", 8888); Message *m = new Message; @@ -204,8 +134,7 @@ int main(int argc, char *argv[]) double lat, lon, alt; /*Mask info*/ - cv::Mat mask; - mask = cv::imread(maskfile, cv::IMREAD_GRAYSCALE); + cv::Mat mask = cv::imread(maskfile, cv::IMREAD_GRAYSCALE); /*tracker infos*/ srand(time(NULL)); @@ -217,6 +146,7 @@ int main(int argc, char *argv[]) float dt = 0.03; int frame_nbr = 0; + while (gRun) { @@ -239,14 +169,11 @@ int main(int argc, char *argv[]) // 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) - read_projection_matrix(H, proj_matrix_read, pmatrix); + coords.clear(); // draw dets for (int i = 0; i < num_detected; i++) @@ -268,8 +195,7 @@ int main(int argc, char *argv[]) if (objClass < 6) { - convert_coords(coords, coord_i, x0 + b.w / 2, y1, objClass, H, adfGeoTransform, frame_nbr); - coord_i++; + convert_coords(coords, x0 + b.w / 2, y1, objClass, H, adfGeoTransform, frame_nbr); //std::cout< -struct obj_coords +#include "../masa_protocol/include/send.hpp" +#include "../masa_protocol/include/serialize.hpp" + +struct ObjCoords { - double LAT; - double LONG; - float cl; + double lat_; + double long_; + int class_; }; void readTiff(char *filename, double *adfGeoTransform) @@ -32,12 +36,31 @@ void readTiff(char *filename, double *adfGeoTransform) poDataset = (GDALDataset *)GDALOpen(filename, GA_ReadOnly); if (poDataset != NULL) { - //int colms = poDataset->GetRasterXSize(); - //int rows = poDataset->GetRasterYSize(); poDataset->GetGeoTransform(adfGeoTransform); } } +void readCameraCalibrationYaml(const std::string &cameraCalib, cv::Mat &cameraMat, cv::Mat &distCoeff) +{ + YAML::Node config = YAML::LoadFile(cameraCalib); + const YAML::Node &node_test1 = config["camera_matrix"]; + + float data_cm[9]; + for (std::size_t i = 0; i < node_test1["data"].size(); i++) + data_cm[i] = node_test1["data"][i].as(); + cv::Mat cameraMat_ = cv::Mat(3, 3, CV_32F, data_cm); + cameraMat = cameraMat_.clone(); + std::cout << cameraMat << std::endl; + const YAML::Node &node_test2 = config["distortion_coefficients"]; + + float data_dc[5]; + for (std::size_t i = 0; i < node_test2["data"].size(); i++) + data_dc[i] = node_test2["data"][i].as(); + cv::Mat distCoeff_ = cv::Mat(5, 1, CV_32F, data_dc); + distCoeff = distCoeff_.clone(); + std::cout << distCoeff << std::endl; +} + void pixel2coord(int x, int y, double &lat, double &lon, double *adfGeoTransform) { //Returns global coordinates from pixel x, y coordinates @@ -71,12 +94,9 @@ void fillMatrix(cv::Mat &H, float *matrix, bool show = false) std::cout << H << "\n"; } - - - //FILE *out_file = fopen("prova_pixel.txt", "w"); -void convert_coords(struct obj_coords *coords, int i, int x, int y, int detected_class, cv::Mat H, double *adfGeoTransform, int frame_nbr) +void convert_coords(std::vector &coords, int x, int y, int detected_class, cv::Mat H, double *adfGeoTransform, int frame_nbr) { double latitude, longitude; std::vector x_y, ll; @@ -87,9 +107,12 @@ void convert_coords(struct obj_coords *coords, int i, int x, int y, int detected //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 = detected_class; + + ObjCoords coord; + coord.lat_ = latitude; + coord.long_ = longitude; + coord.class_ = detected_class; + coords.push_back(coord); /*if (detected_class == 0) { @@ -99,12 +122,12 @@ void convert_coords(struct obj_coords *coords, int i, int x, int y, int detected unsigned long long t_stamp_ms = (unsigned long long)(tv.tv_sec) * 1000 + (unsigned long long)(tv.tv_usec) / 1000; //printf(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); - //printf( "%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, coord.LAT, coord.LONG); + //printf( "%d %lld %f %f\n", frame_nbr, t_stamp_ms, coord.LAT, coord.LONG); }*/ } -void read_projection_matrix(cv::Mat &H, int &proj_matrix_read, char *path) +void read_projection_matrix(cv::Mat &H, char *path) { FILE *fp; char *line = NULL; @@ -123,16 +146,70 @@ void read_projection_matrix(cv::Mat &H, int &proj_matrix_read, char *path) 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); fillMatrix(H, proj_matrix); - - free(proj_matrix); } +void draw_arrow(float angleRad, float vel, cv::Scalar color, cv::Point center, cv::Mat &frame) +{ + int angle = angleRad * 180.0 / CV_PI; + auto length = 10 * vel; + auto direction = cv::Point(length * cos(angleRad), length * sin(angleRad)); // calculate direction + double tipLength = .2 + 0.4 * (angle % 180) / 360; + int lineType = 8; + int thickness = 2; + cv::arrowedLine(frame, center, center + direction, color, thickness, lineType, 0, tipLength); // draw arrow! +} + +unsigned long long time_in_ms() +{ + 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; + return t_stamp_ms; +} + +void prepare_message(Message *m, const std::vector& coords, int idx) +{ + m->cam_idx = idx; + m->t_stamp_ms = time_in_ms(); + m->num_objects = coords.size(); + + m->objects.clear(); + for (int i = 0; i < coords.size(); i++) + { + Categories cat; + switch (coords[i].class_) + { + case 0: + cat = Categories::C_person; + break; + case 1: + cat = Categories::C_car; + break; + case 2: + cat = Categories::C_car; + break; + case 3: + cat = Categories::C_bus; + break; + case 4: + cat = Categories::C_motorbike; + break; + case 5: + cat = Categories::C_bycicle; + break; + } + RoadUser r{coords[i].lat_, coords[i].long_, 0, 1, C_car}; + m->objects.push_back(r); + } + + m->lights.clear(); +} + #endif /*CLASSUTILS_H*/ \ No newline at end of file