From 17c5b7a818151798dd7d91932733936f9fcf4c6b Mon Sep 17 00:00:00 2001 From: Tomasz Kloda Date: Sat, 27 Apr 2019 08:31:54 +0000 Subject: [PATCH 1/5] readme modified --- tracker_CLASS | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tracker_CLASS b/tracker_CLASS index fad7877..8512a87 160000 --- a/tracker_CLASS +++ b/tracker_CLASS @@ -1 +1 @@ -Subproject commit fad787751156d828d7e26ef17cc67da183c592d1 +Subproject commit 8512a87dc2a936b94d46620dd3c795481d423172 From 978833fd6ed3f58624f85b0fe56f5157d98a70e8 Mon Sep 17 00:00:00 2001 From: Tomasz Kloda Date: Sat, 27 Apr 2019 11:27:51 +0000 Subject: [PATCH 2/5] Revert "visualization via thread" This reverts commit ca9d18c69e2d436f7d221647dc7841060f00d33f. --- demo/demo/demo.cpp | 75 ++++++++++++++++------------------------------ 1 file changed, 25 insertions(+), 50 deletions(-) diff --git a/demo/demo/demo.cpp b/demo/demo/demo.cpp index 83ed14a..6a5d9ef 100644 --- a/demo/demo/demo.cpp +++ b/demo/demo/demo.cpp @@ -3,8 +3,7 @@ #include /* srand, rand */ #include #include -#include -#include +#include #include "utils.h" #include @@ -21,9 +20,6 @@ #define MAX_DETECT_SIZE 100 bool gRun; -cv::Mat frame_v; -cv::Mat frame_top_v; -std::mutex sem; void sig_handler(int signo) { @@ -31,19 +27,6 @@ void sig_handler(int signo) gRun = false; } -void *showImages(void *x_void_ptr) -{ - - while (gRun) - { - sem.lock(); - cv::imshow("detection", frame_v); - cv::imshow("topview", frame_top_v); - sem.unlock(); - cv::waitKey(30); - } -} - int main(int argc, char *argv[]) { @@ -66,7 +49,7 @@ int main(int argc, char *argv[]) int CAM_IDX = 0; if (argc > 5) CAM_IDX = atoi(argv[5]); - bool to_show = false; + bool to_show = true; if (argc > 6) to_show = atoi(argv[6]); @@ -83,16 +66,13 @@ int main(int argc, char *argv[]) std::cout << "camera started\n"; cv::Mat frame; - cv::Mat frame_top; - cv::Mat dnn_input; cv::namedWindow("detection", cv::WINDOW_NORMAL); cv::namedWindow("topview", cv::WINDOW_NORMAL); + cv::Mat frame_top; frame_top = cv::imread("../demo/demo/data/map_b.jpg"); - pthread_t visual; - /*projection matrix*/ int proj_matrix_read = 0; @@ -113,7 +93,7 @@ int main(int argc, char *argv[]) double lat, lon, alt; /*tracker infos*/ - srand(time(NULL)); + srand (time(NULL)); std::vector trackers; std::vector cur_frame; int initial_age = -5; @@ -127,6 +107,7 @@ int main(int argc, char *argv[]) while (gRun) { + cap >> frame; if (!frame.data) { @@ -178,9 +159,10 @@ int main(int argc, char *argv[]) //std::cout<= 0 && pix_y >= 0) - cv::circle(frame_top, cv::Point(pix_x, pix_y), 7.0, cv::Scalar(t.r_, t.g_, t.b_), CV_FILLED, 8, 0); + if(pix_x < frame_top.cols && pix_y < frame_top.rows && pix_x >= 0 && pix_y >= 0) + cv::circle(frame_top, cv::Point(pix_x,pix_y), 7.0, cv::Scalar(t.r_, t.g_, t.b_), CV_FILLED, 8, 0); std::vector map_p, camera_p; map_p.push_back(cv::Point2f(pix_x, pix_y)); @@ -227,25 +211,7 @@ int main(int argc, char *argv[]) //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(t.r_, t.g_, t.b_), CV_FILLED, 8, 0); - } - } - - TIMER_START - sem.lock(); - frame_v = frame.clone(); - frame_top_v = frame_top.clone(); - sem.unlock(); - - TIMER_STOP - - if (frame_nbr == 0) - { - - if (pthread_create(&visual, NULL, showImages, NULL)) - { - fprintf(stderr, "Error creating thread\n"); - return 1; } } @@ -254,6 +220,15 @@ int main(int argc, char *argv[]) send_client_dummy(coords, coord_i, sock, socket_opened, CAM_IDX); + + if (to_show) + { + cv::imshow("detection", frame); + cv::imshow("topview", frame_top); + cv::waitKey(1); + } + + } /* for (size_t i = 0; i < trackers.size(); i++) From 753699104afdcddf1d412763bfe485e932562fdd Mon Sep 17 00:00:00 2001 From: Tomasz Kloda Date: Sat, 27 Apr 2019 11:39:41 +0000 Subject: [PATCH 3/5] submodule fix --- tracker_CLASS | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tracker_CLASS b/tracker_CLASS index 8512a87..fad7877 160000 --- a/tracker_CLASS +++ b/tracker_CLASS @@ -1 +1 @@ -Subproject commit 8512a87dc2a936b94d46620dd3c795481d423172 +Subproject commit fad787751156d828d7e26ef17cc67da183c592d1 From 1b9fe1ea61dc7ac9dd5b4c6dc81d2a7ee9b45409 Mon Sep 17 00:00:00 2001 From: Tomasz Kloda Date: Sat, 27 Apr 2019 15:59:55 +0000 Subject: [PATCH 4/5] added arrows(to fix), deleted old traj in top view --- demo/demo/demo.cpp | 68 ++++++++++++++++++++++++++++++++++------------ 1 file changed, 50 insertions(+), 18 deletions(-) diff --git a/demo/demo/demo.cpp b/demo/demo/demo.cpp index 6a5d9ef..281d035 100644 --- a/demo/demo/demo.cpp +++ b/demo/demo/demo.cpp @@ -27,6 +27,19 @@ void sig_handler(int signo) gRun = false; } + + +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! +} + int main(int argc, char *argv[]) { @@ -71,7 +84,9 @@ int main(int argc, char *argv[]) cv::namedWindow("topview", cv::WINDOW_NORMAL); cv::Mat frame_top; + cv::Mat original_frame_top; frame_top = cv::imread("../demo/demo/data/map_b.jpg"); + original_frame_top = frame_top.clone(); /*projection matrix*/ @@ -187,31 +202,48 @@ int main(int argc, char *argv[]) TIMER_STOP - //std::cout << "There are " << trackers.size() << " trackers" << std::endl; + std::cout << "There are " << trackers.size() << " trackers" << std::endl; - for (auto t : trackers) + if(to_show) { - for(auto pred_pos: t.pred_list_ ) + frame_top = original_frame_top.clone(); + for (auto t : trackers) { - - 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; - if(pix_x < frame_top.cols && pix_y < frame_top.rows && pix_x >= 0 && pix_y >= 0) - cv::circle(frame_top, cv::Point(pix_x,pix_y), 7.0, cv::Scalar(t.r_, t.g_, t.b_), CV_FILLED, 8, 0); + for(size_t p=1; p= 0 && pix_y >= 0) + cv::circle(frame_top, cv::Point(pix_x,pix_y), 7.0, cv::Scalar(t.r_, t.g_, t.b_), CV_FILLED, 8, 0); - std::vector map_p, camera_p; - map_p.push_back(cv::Point2f(pix_x, pix_y)); + 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; + //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(t.r_, t.g_, t.b_), CV_FILLED, 8, 0); + cv::circle(frame, cv::Point(camera_p[0].x, camera_p[0].y), 3.0, cv::Scalar(t.r_, t.g_, t.b_), CV_FILLED, 8, 0); + if(p == t.pred_list_.size()-1) + { + + auto center = cv::Point(camera_p[0].x, camera_p[0].y); + auto color = cv::Scalar(t.r_, t.g_, t.b_); + draw_arrow(t.pred_list_[p].yaw_, t.pred_list_[p].vel_,color, center, frame); + + center = cv::Point(pix_x,pix_y); + draw_arrow(t.pred_list_[p].yaw_, t.pred_list_[p].vel_,color, center, frame_top); + + + } + + } } } From 7d1d31ac450053842d909c4f43dd503de117fc6b Mon Sep 17 00:00:00 2001 From: Tomasz Kloda Date: Sun, 28 Apr 2019 11:46:38 +0000 Subject: [PATCH 5/5] re-added thread for visualisation --- demo/demo/demo.cpp | 92 ++++++++++++++++++++++++---------------------- 1 file changed, 49 insertions(+), 43 deletions(-) diff --git a/demo/demo/demo.cpp b/demo/demo/demo.cpp index 281d035..59ee58c 100644 --- a/demo/demo/demo.cpp +++ b/demo/demo/demo.cpp @@ -4,6 +4,7 @@ #include #include #include +#include #include "utils.h" #include @@ -21,12 +22,28 @@ bool gRun; + +cv::Mat frame_v; +cv::Mat frame_top_v; +std::mutex sem; + void sig_handler(int signo) { std::cout << "request gateway stop\n"; gRun = false; } +void *showImages(void *x_void_ptr) +{ + while (gRun) + { + sem.lock(); + cv::imshow("detection", frame_v); + cv::imshow("topview", frame_top_v); + sem.unlock(); + cv::waitKey(30); + } +} void draw_arrow(float angleRad, float vel, cv::Scalar color, cv::Point center, cv::Mat &frame) @@ -58,14 +75,15 @@ int main(int argc, char *argv[]) char *tiffile = "../demo/demo/data/map_b.tif"; if (argc > 4) tiffile = argv[4]; - /*CAMID*/ int CAM_IDX = 0; if (argc > 5) CAM_IDX = atoi(argv[5]); - bool to_show = true; + bool to_show = false; if (argc > 6) to_show = atoi(argv[6]); + + tk::dnn::Yolo3Detection yolo; yolo.init(net); yolo.thresh = 0.25; @@ -76,26 +94,31 @@ int main(int argc, char *argv[]) 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("topview", cv::WINDOW_NORMAL); - cv::Mat frame_top; + cv::Mat dnn_input; cv::Mat original_frame_top; - frame_top = cv::imread("../demo/demo/data/map_b.jpg"); - original_frame_top = frame_top.clone(); + + pthread_t visual; + + if(to_show) + { + cv::namedWindow("detection", cv::WINDOW_NORMAL); + cv::namedWindow("topview", cv::WINDOW_NORMAL); + frame_top = cv::imread("../demo/demo/data/map_b.jpg"); + original_frame_top = frame_top.clone(); + } /*projection matrix*/ - 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); + struct obj_coords *coords = (struct obj_coords *)malloc(MAX_DETECT_SIZE * sizeof(struct obj_coords)); /*socket*/ int sock; @@ -116,13 +139,9 @@ int main(int argc, char *argv[]) int n_states = 5; float dt = 0.03; - struct obj_coords *coords = (struct obj_coords *)malloc(MAX_DETECT_SIZE * sizeof(struct obj_coords)); - int frame_nbr = 0; while (gRun) { - - cap >> frame; if (!frame.data) { @@ -144,13 +163,7 @@ int main(int argc, char *argv[]) num_detected = MAX_DETECT_SIZE; 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 < num_detected; i++) @@ -179,17 +192,14 @@ int main(int argc, char *argv[]) TIMER_START + //convert from latitude and longitude to meters for ekf cur_frame.clear(); for (int i = 0; i < coord_i; i++) { - //std::cout << "lat orig: " << coords[i].LAT << " lon orig: " << coords[i].LONG << std::endl; gc.geodetic2Enu(coords[i].LAT, coords[i].LONG, 0, &east, &north, &up); - //std::cout << "east: " << east << " north: " << north << std::endl; cur_frame.push_back(Data(east, north, frame_nbr)); } - - if (frame_nbr == 0) { for (auto f : cur_frame) @@ -200,7 +210,6 @@ int main(int argc, char *argv[]) Track(cur_frame, dt, n_states, initial_age, age_threshold, trackers); } - TIMER_STOP std::cout << "There are " << trackers.size() << " trackers" << std::endl; @@ -232,41 +241,38 @@ int main(int argc, char *argv[]) if(p == t.pred_list_.size()-1) { - auto center = cv::Point(camera_p[0].x, camera_p[0].y); auto color = cv::Scalar(t.r_, t.g_, t.b_); draw_arrow(t.pred_list_[p].yaw_, t.pred_list_[p].vel_,color, center, frame); center = cv::Point(pix_x,pix_y); draw_arrow(t.pred_list_[p].yaw_, t.pred_list_[p].vel_,color, center, frame_top); - - } - } } } - frame_nbr++; - - send_client_dummy(coords, coord_i, sock, socket_opened, CAM_IDX); - - - - if (to_show) + if(to_show) { - cv::imshow("detection", frame); - cv::imshow("topview", frame_top); - cv::waitKey(1); + sem.lock(); + frame_v = frame.clone(); + frame_top_v = frame_top.clone(); + sem.unlock(); } + if (frame_nbr == 0 && to_show) + { + if (pthread_create(&visual, NULL, showImages, NULL)) + { + fprintf(stderr, "Error creating thread\n"); + return 1; + } + } + frame_nbr++; + send_client_dummy(coords, coord_i, sock, socket_opened, CAM_IDX); } - /* 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);