From 2d62d2524c8a93078a4037e6e54a480a04b72a8b Mon Sep 17 00:00:00 2001 From: Davide Sapienza Date: Fri, 30 Aug 2019 17:13:29 +0200 Subject: [PATCH] Fix the visualization thread This commit moves the computation of the visualization into the 'showImages' function (display thread). The main thread workload and the time consuming for each frame are reduced. Please enter the commit message for your changes. Lines starting --- demo/demo/demo.cpp | 229 ++++++++++++++++++++++++++------------------- 1 file changed, 131 insertions(+), 98 deletions(-) diff --git a/demo/demo/demo.cpp b/demo/demo/demo.cpp index f6431f9..72ebcb3 100644 --- a/demo/demo/demo.cpp +++ b/demo/demo/demo.cpp @@ -38,25 +38,102 @@ bool gRun; std::string obj_class[10]{"person", "car", "truck", "bus", "motor", "bike", "rider", "traffic light", "traffic sign", "train"}; -cv::Mat frame_v; -cv::Mat frame_top_v; std::mutex sem; +struct InfoShow{ + std::vector trackers; + geodetic_converter::GeodeticConverter gc; + double adfGeoTransform[6]; + cv::Mat H; + cv::Mat frame_v; +}; + void sig_handler(int signo) { std::cout << "request gateway stop\n"; gRun = false; } + + void *showImages(void *x_void_ptr) { + InfoShow *info_show = (InfoShow *) x_void_ptr; + double lat, lon, alt; + int pix_x, pix_y; + cv::Mat frame_top; + cv::Mat original_frame_top; + cv::namedWindow("detection", cv::WINDOW_NORMAL); + cv::namedWindow("topview", cv::WINDOW_NORMAL); + // original_frame_top = cv::imread("../demo/demo/data/map/map_geo.jpg"); + original_frame_top = cv::imread("../demo/demo/data/map/MASA_4670.png"); + cv::Mat frame_v_loc; + std::vector trackers; + geodetic_converter::GeodeticConverter gc; + double adfGeoTransform[6]; + cv::Mat H; while (gRun) { + TIMER_START + // critical section: copy the struct in local variable + // in this way we can unlock the sem for the main thread sem.lock(); - cv::imshow("detection", frame_v); - cv::imshow("topview", frame_top_v); + frame_v_loc = info_show->frame_v.clone(); + // std::vector trackers; + trackers = info_show->trackers; + // geodetic_converter::GeodeticConverter gc; + gc = info_show->gc; + for(int i = 0; i < 6; i++ ) + adfGeoTransform[i] = info_show->adfGeoTransform[i]; + // cv::Mat H; + H = info_show->H.clone(); sem.unlock(); - cv::waitKey(30); + + frame_top = original_frame_top.clone(); + for (auto t : trackers) + { + for (size_t p = 1; p < t.pred_list_.size(); p++) + { + + gc.enu2Geodetic(t.pred_list_[p].x_, t.pred_list_[p].y_, 0, &lat, &lon, &alt); + //std::cout << "lat: " << lat << " lon: " << lon << std::endl; + + 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); + + 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_v_loc, 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); + }*/ + } + } + + //outputVideo<< frame_top; + // ------------------------------------------------ + + cv::imshow("detection", frame_v_loc); + cv::imshow("topview", frame_top); + cv::waitKey(1); + std::cout<<"visual: "; + TIMER_STOP + } } @@ -99,7 +176,6 @@ int main(int argc, char *argv[]) yolo.thresh = 0.25; gRun = true; - cv::VideoCapture cap(input); if (!cap.isOpened()) gRun = false; @@ -109,21 +185,10 @@ int main(int argc, char *argv[]) cv::Mat frame; cv::Mat frame_crop; char buf_frame_crop_name [200]; - cv::Mat frame_top; cv::Mat dnn_input; - cv::Mat original_frame_top; 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/map_geo.jpg"); - frame_top = cv::imread("../demo/demo/data/map/MASA_4670.png"); - original_frame_top = frame_top.clone(); - } - /*projection matrix from camera to map*/ cv::Mat H(cv::Size(3, 3), CV_64FC1); read_projection_matrix(H, pmatrix); @@ -150,7 +215,7 @@ int main(int argc, char *argv[]) geodetic_converter::GeodeticConverter gc; gc.initialiseReference(44.655540, 10.934315, 0); double east, north, up; - double lat, lon, alt; + // double lat, lon, alt; /*Mask info*/ cv::Mat mask = cv::imread(maskfile, cv::IMREAD_GRAYSCALE); @@ -207,6 +272,17 @@ int main(int argc, char *argv[]) cv::Scalar intensity; cv::Rect roi; + InfoShow info_show; + if (to_show) + { + info_show.H = cv::Mat(cv::Size(3, 3), CV_64FC1); + if (pthread_create(&visual, NULL, showImages, (void *)&info_show)) + { + fprintf(stderr, "Error creating thread\n"); + return 1; + } + } + while (gRun) { TIMER_START @@ -254,8 +330,8 @@ int main(int argc, char *argv[]) // src_gray canny_img = img_laplacian(orig_frame,0); cv::Canny(canny_img, canny, 100, 100*2 ); - sprintf(buf_frame_crop_name,"../demo/demo/data/img_disparity/%d_%d_canny.jpg",frame_nbr, 999); - cv::imwrite(buf_frame_crop_name, canny); + // sprintf(buf_frame_crop_name,"../demo/demo/data/img_disparity/%d_%d_canny.jpg",frame_nbr, 999); + // cv::imwrite(buf_frame_crop_name, canny); end_t_segmentation = std::chrono::steady_clock::now(); std::cout << " - TIME END pre canny : "<(end_t_segmentation - step_t_segmentation).count() << " ms"<(end_t_segmentation - step_t_segmentation).count() << " ms"<(end_t_segmentation - step_t_segmentation).count() << " ms"<(end_t_segmentation - step_t_segmentation).count() << " ms"<(end_t_segmentation - step_t_segmentation).count() << " ms"<(end_t_segmentation - step_t_segmentation).count() << " ms"< 0)? x0 : 0; + roi.y = (y0 > 0)? y0 : 0; // std::cout<<"x "<= frame.cols)? frame.cols-1-x0 : w; - roi.height = (y0+h >= frame.rows)? frame.rows-1-y0 : h; + roi.width = (roi.x+w >= frame.cols)? frame.cols-1-roi.x : w; + roi.height = (roi.y+h >= frame.rows)? frame.rows-1-roi.y : h; // std::cout<<"w "<= 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)); - - //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); - - /*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); - }*/ - } - } - } - - if (to_show) - { + //populate the InfoShow sem.lock(); - frame_v = frame.clone(); - frame_top_v = frame_top.clone(); + info_show.frame_v = frame.clone(); + // std::vector trackers; + info_show.trackers = trackers; + // geodetic_converter::GeodeticConverter gc; + info_show.gc = gc; + for(int i = 0; i < 6; i++ ) + info_show.adfGeoTransform[i] = adfGeoTransform[i]; + // cv::Mat H; + info_show.H = H.clone(); sem.unlock(); - - //outputVideo<< frame_top; - } - - if (frame_nbr == 0 && to_show) - { - if (pthread_create(&visual, NULL, showImages, NULL)) - { - fprintf(stderr, "Error creating thread\n"); - return 1; - } } // update pre_frame for the disparity map