mmr/tkDNN
Archived
1
0
Fork 0
Code Issues 46 Pull Requests 12 Releases 4 Activity

Fix visualization thread

Browse Source 
This commit splits some operations into different threads.
Some threads compute the visualization preprocessing for the
live, detection, top view and disparity visualization.
Only one thread has the role to display the different views.
This commit is contained in:
Davide Sapienza
2019-09-09 14:40:43 +02:00
parent 5bbb3f3480
commit b1a3620061
2 changed files with 570 additions and 148 deletions
Download Patch File Download Diff File Expand all files Collapse all files
demo/demo/demo.cpp
+568 -146
View File
@@ -38,17 +38,32 @@
bool gRun;
std::string obj_class[10]{"person", "car", "truck", "bus", "motor", "bike", "rider", "traffic light", "traffic sign", "train"};
std::mutex sem;
// sem for mainthread, detectionthread and topviewthread
// sem_vc for mainthread, videocapturethread, originalthread and disparitythread
std::mutex sem, sem_vc;
// a single mutex for each operation - the show_updates function must get all mutex
std::mutex mutex_o, mutex_de, mutex_t, mutex_di;
struct InfoShow{
struct ModFrame_t{
std::vector<Tracker> trackers;
geodetic_converter::GeodeticConverter gc;
double adfGeoTransform[6];
cv::Mat H;
cv::Mat frame_v;
cv::Mat frame_disp;
cv::Mat original_frame;
tk::dnn::Yolo3Detection yolo;
cv::Mat mask;
};
struct Frame_t{
char *input;
cv::Mat frame;
int frame_nbr;
};
struct Show_t{
cv::Mat original, detection, topview, disparity;
bool update_o, update_de, update_t, update_di;
}updates;
void sig_handler(int signo)
{
@@ -56,25 +71,208 @@ void sig_handler(int signo)
gRun = false;
}
void *showImages(void *x_void_ptr)
void *readVideoCapture(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;
Frame_t *info_f = (Frame_t *) x_void_ptr;
cv::VideoCapture cap(info_f->input);
cv::Mat frame_loc;
int frame_nbr_loc = 0;
// bool to_show = false;
if (!cap.isOpened())
gRun = false;
else
std::cout << "camera started\n";
cap.set(cv::CAP_PROP_BUFFERSIZE,3);
// cap.set(CV_CAP_PROP_FPS, 10);
// // CV_CAP_PROP_BUFFERSIZE
std::cout<<"buf size: "<<cap.get(CV_CAP_PROP_BUFFERSIZE)<<std::endl;
// ModFrame_t *info_show = (ModFrame_t *) x_void_ptr;
auto start_t = std::chrono::steady_clock::now();
auto step_t = std::chrono::steady_clock::now();
auto end_t = std::chrono::steady_clock::now();
// compute fps
// std::cout << "Frames per second using video.get(CV_CAP_PROP_FPS) : " << cap.get(CV_CAP_PROP_FPS) << std::endl;
// //compute frame rate
// int i=0, num_f = 120;
// while (i<21)
// {
// cap >> frame;
// i++;
// }
// i=0;
// start_t = std::chrono::steady_clock::now();
// while (i<num_f)
// {
// step_t = std::chrono::steady_clock::now();
// cap >> frame;
// i++;
// std::cout << " step "<<i<<" : "<<std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now() - step_t).count() << " ms"<<std::endl;
// }
// end_t = std::chrono::steady_clock::now();
// std::cout << "Capturing " << num_f << " frames" << std::endl ;
// std::cout << " Time taken : "<<std::chrono::duration_cast<std::chrono::milliseconds>(end_t - start_t).count() << " ms"<<std::endl;
// return 0;
//------------------------
while (gRun)
{
cap >> frame_loc;
// CAP_PROP_POS_MSEC Current position of the video file in milliseconds or video capture timestamp.
std::cout<<"id: "<<cap.get(cv::CAP_PROP_POS_MSEC)<<std::endl;
// CAP_PROP_FRAME_COUNT Number of frames in the video file.
// std::cout<<"id: "<<cap.get(cv::CAP_PROP_FRAME_COUNT)<<std::endl;
if (!frame_loc.data)
{
usleep(1000000);
cap.open(info_f->input);
printf("cap reinitialize\n");
continue;
}
end_t = std::chrono::steady_clock::now();
std::cout << " VC-TIME 1 : "<<std::chrono::duration_cast<std::chrono::milliseconds>(end_t - start_t).count() << " ms"<<std::endl;
start_t = end_t;
sem_vc.lock();
info_f->frame = frame_loc.clone();
info_f->frame_nbr = frame_nbr_loc;
sem_vc.unlock();
end_t = std::chrono::steady_clock::now();
std::cout << " VC-TIME 2 : "<<std::chrono::duration_cast<std::chrono::milliseconds>(end_t - start_t).count() << " ms"<<std::endl;
start_t = end_t;
frame_nbr_loc ++;
}
}
/* Thread function to show the updated images
**/
void *show_updates(void *x_void_ptr)
{
cv::namedWindow("original", cv::WINDOW_NORMAL);
cv::namedWindow("detection", cv::WINDOW_NORMAL);
cv::namedWindow("topview", cv::WINDOW_NORMAL);
cv::namedWindow("disparity", cv::WINDOW_NORMAL);
cv::Mat original_loc, detection_loc, topview_loc, disparity_loc;
bool update_o_loc, update_de_loc, update_t_loc, update_di_loc;
while (gRun)
{
TIMER_START
// critical section: copy the struct in local variable
// in this way we can unlock the sem for the main thread
if(mutex_o.try_lock())
{
update_o_loc = updates.update_o;
updates.update_o = false;
if(update_o_loc)
original_loc = updates.original.clone();
mutex_o.unlock();
}
// 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;
if(mutex_de.try_lock())
{
update_de_loc = updates.update_de;
updates.update_de = false;
if(update_de_loc)
detection_loc = updates.detection.clone();
mutex_de.unlock();
}
if(mutex_t.try_lock())
{
update_t_loc = updates.update_t;
updates.update_t = false;
if(update_t_loc)
topview_loc = updates.topview.clone();
mutex_t.unlock();
}
if(mutex_di.try_lock())
{
update_di_loc = updates.update_di;
updates.update_di = false;
if(update_di_loc)
disparity_loc = updates.disparity.clone();
mutex_di.unlock();
}
if(update_o_loc)
cv::imshow("original", original_loc);
if(update_de_loc)
cv::imshow("detection", detection_loc);
if(update_t_loc)
cv::imshow("topview", topview_loc);
if(update_di_loc)
cv::imshow("disparity", disparity_loc);
cv::waitKey(1);
// usleep(20000); //sleep 20 msec
std::cout<<"show_updates: ";
TIMER_STOP
}
}
void *originalFrame(void *x_void_ptr)
{
Frame_t *info_show_orig = (Frame_t *) x_void_ptr;
cv::Mat frame_loc;
int frame_nbr_loc = 0;
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_vc.lock();
frame_loc = info_show_orig->frame.clone();
frame_nbr_loc = info_show_orig->frame_nbr;
sem_vc.unlock();
if (frame_nbr_loc == 0)
{
usleep(1000000);
printf("no frame received\n");
continue;
}
mutex_o.lock();
updates.original = frame_loc.clone();
updates.update_o = true;
mutex_o.unlock();
usleep(10000); //sleep 10 msec
std::cout<<"originalFrame: ";
TIMER_STOP
}
}
void *detectionFrame(void *x_void_ptr)
{
ModFrame_t *info_show = (ModFrame_t *) x_void_ptr;
double lat, lon, alt;
int pix_x, pix_y;
cv::Mat original_frame_loc;
std::vector<Tracker> trackers;
geodetic_converter::GeodeticConverter gc;
double adfGeoTransform[6];
cv::Mat H;
cv::Mat frame_disp_loc;
tk::dnn::Yolo3Detection yolo;
int num_detected;
cv::Mat mask;
// box variable
tk::dnn::box b;
int x0, w, x1, y0, h, y1;
int objClass;
std::string det_class;;
float prob;
cv::Scalar intensity;
while (gRun)
{
@@ -82,7 +280,7 @@ void *showImages(void *x_void_ptr)
// critical section: copy the struct in local variable
// in this way we can unlock the sem for the main thread
sem.lock();
frame_v_loc = info_show->frame_v.clone();
original_frame_loc = info_show->original_frame.clone();
// std::vector<Tracker> trackers;
trackers = info_show->trackers;
// geodetic_converter::GeodeticConverter gc;
@@ -91,10 +289,111 @@ void *showImages(void *x_void_ptr)
adfGeoTransform[i] = info_show->adfGeoTransform[i];
// cv::Mat H;
H = info_show->H.clone();
frame_disp_loc = info_show->frame_disp.clone();
yolo = info_show->yolo;
mask = info_show->mask.clone();
sem.unlock();
if (trackers.empty())
{
usleep(1000000);
printf("no data available\n");
continue;
}
num_detected = yolo.detected.size();
for (int i = 0; i < num_detected; i++)
{
b = yolo.detected[i];
x0 = b.x;
w = b.w;
x1 = b.x + w;
y0 = b.y;
h = b.h;
y1 = b.y + h;
objClass = b.cl;
det_class = obj_class[b.cl];
prob = b.prob;
intensity = mask.at<uchar>(cv::Point(int(x0 + b.w / 2), y1));
if (intensity[0] && objClass < 6)
{
//std::cout<<objClass<<" ("<<prob<<"): "<<x0<<" "<<y0<<" "<<x1<<" "<<y1<<"\n";
cv::rectangle(original_frame_loc, cv::Point(x0, y0), cv::Point(x1, y1), yolo.colors[objClass], 2);
// draw label
int baseline = 0;
float fontScale = 0.5;
int thickness = 2;
cv::Size textSize = getTextSize(det_class, cv::FONT_HERSHEY_SIMPLEX, fontScale, thickness, &baseline);
cv::rectangle(original_frame_loc, cv::Point(x0, y0), cv::Point((x0 + textSize.width - 2), (y0 - textSize.height - 2)), yolo.colors[b.cl], -1);
cv::putText(original_frame_loc, det_class, cv::Point(x0, (y0 - (baseline / 2))), cv::FONT_HERSHEY_SIMPLEX, fontScale, cv::Scalar(255, 255, 255), thickness);
}
}
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);
coord2pixel(lat, lon, pix_x, pix_y, adfGeoTransform);
std::vector<cv::Point2f> 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());
cv::circle(original_frame_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);
}
}
mutex_de.lock();
updates.detection = original_frame_loc.clone();
updates.update_de = true;
mutex_de.unlock();
std::cout<<"detectionFrame: ";
TIMER_STOP
}
}
void *topviewFrame(void *x_void_ptr)
{
ModFrame_t *info_show = (ModFrame_t *) x_void_ptr;
double lat, lon, alt;
int pix_x, pix_y;
cv::Mat frame_top;
cv::Mat original_frame_top;
// original_frame_top = cv::imread("../demo/demo/data/map/map_geo.jpg");
// original_frame_top = cv::imread("../demo/demo/data/map/MASA_4670.png");
original_frame_top = cv::imread("../demo/demo/data/map/MASA_4670_V.png");
std::vector<Tracker> 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();
// std::vector<Tracker> 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();
if (trackers.empty())
{
usleep(1000000);
printf("no data available\n");
continue;
}
frame_top = original_frame_top.clone();
for (auto t : trackers)
{
@@ -110,40 +409,118 @@ void *showImages(void *x_void_ptr)
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<cv::Point2f> 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;
//outputVideo<< frame_top;
// ------------------------------------------------
std::cout<<"size: "<<frame_v_loc.rows<<" - "<<frame_v_loc.cols<<std::endl;
std::cout<<"size: "<<frame_top.rows<<" - "<<frame_top.cols<<std::endl;
std::cout<<"size: "<<frame_disp_loc.rows<<" - "<<frame_disp_loc.cols<<std::endl;
cv::imshow("detection", frame_v_loc);
cv::imshow("topview", frame_top);
cv::imshow("disparity", frame_disp_loc);
mutex_t.lock();
updates.topview = frame_top.clone();
updates.update_t = true;
mutex_t.unlock();
cv::waitKey(1);
std::cout<<"visual: ";
std::cout<<"topviewFrame: ";
TIMER_STOP
}
}
void *disparityFrame(void *x_void_ptr)
{
Frame_t *info_show_disparity = (Frame_t *) x_void_ptr;
bool first_iteration = true;
cv::Mat frame_loc;
int frame_nbr_loc = 0, pre_frame_nbr_loc = 0;
auto start_t = std::chrono::steady_clock::now();
auto step_t = std::chrono::steady_clock::now();
auto end_t = std::chrono::steady_clock::now();
// information for the disparity map
cv::Mat canny, pre_canny, canny_RGB, pre_canny_RGB;
cv::Mat canny_img;
cv::Mat disparity_frame;
while (gRun)
{
start_t = std::chrono::steady_clock::now();
step_t = start_t;
// critical section: copy the struct in local variable
// in this way we can unlock the sem for the main thread
sem_vc.lock();
frame_loc = info_show_disparity->frame.clone();
frame_nbr_loc = info_show_disparity->frame_nbr;
sem_vc.unlock();
if (frame_nbr_loc == 0)
{
usleep(1000000);
printf("no frame received\n");
continue;
}
// compute frame disparity only in there is a new frame
if(frame_nbr_loc - pre_frame_nbr_loc > 0)
{
pre_frame_nbr_loc = frame_nbr_loc;
//preprocessing frame
step_t = std::chrono::steady_clock::now();
// src_gray
canny_img = img_laplacian(frame_loc,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_loc, 999);
// cv::imwrite(buf_frame_crop_name, canny);
end_t = std::chrono::steady_clock::now();
std::cout << " TIME END pre canny : "<<std::chrono::duration_cast<std::chrono::milliseconds>(end_t - step_t).count() << " ms"<<std::endl;
step_t = end_t;
// std::cout<<"o: "<<frame_loc.cols<<" - "<<frame_loc.rows<<std::endl;
// std::cout<<"canny: "<<canny.cols<<" - "<<canny.rows<<std::endl;
// std::cout<<"pre: "<<pre_canny.cols<<" - "<<pre_canny.rows<<std::endl;
if(!first_iteration)
{
// backtorgb = cv::cvtColor(pre_canny,cv::COLOR_GRAY2RGB)
cv::cvtColor(pre_canny, pre_canny_RGB, CV_GRAY2RGB);
cv::cvtColor(canny, canny_RGB, CV_GRAY2RGB);
disparity_frame = frame_disparity(pre_canny_RGB, canny_RGB, frame_nbr_loc, 999, 0);
// std::cout<<"size: "<<disparity_frame.rows<<" - "<<disparity_frame.cols<<std::endl;
// if (disparity_frame.rows == 0 || disparity_frame.cols == 0)
// return -1;
// if (disparity_frame.empty())
// { // only fools don't check...
// std::cout << "image not loaded !" << std::endl;
// return -1;
// }
end_t = std::chrono::steady_clock::now();
std::cout << " TIME canny : frame_disparity : "<<std::chrono::duration_cast<std::chrono::milliseconds>(end_t - step_t).count() << " ms"<<std::endl;
step_t = end_t;
// //--------------------------------
// //frame box disparity on the original image
// step_t_segmentation = std::chrono::steady_clock::now();
// frame_box_disparity(pre_frame, frame, pre_rois, frame_nbr_loc);
// // reset pre_rois for the new roi of the current frame
// // pre_rois.erase(pre_rois.begin(), pre_rois.end());
// end_t_segmentation = std::chrono::steady_clock::now();
// std::cout << " TIME Frame disparity : "<<std::chrono::duration_cast<std::chrono::milliseconds>(end_t_segmentation - step_t_segmentation).count() << " ms"<<std::endl;
// step_t_segmentation = end_t_segmentation;
// //frame box disparity on the preprocessed image
// cv::cvtColor(pre_canny, pre_canny_RGB, CV_GRAY2RGB);
// cv::cvtColor(canny, canny_RGB, CV_GRAY2RGB);
// frame_box_disparity(pre_canny_RGB, canny_RGB, pre_rois, frame_nbr_loc);
// // reset pre_rois for the new roi of the current frame
// pre_rois.erase(pre_rois.begin(), pre_rois.end());
// end_t_segmentation = std::chrono::steady_clock::now();
// std::cout << " TIME Canny Frame disparity : "<<std::chrono::duration_cast<std::chrono::milliseconds>(end_t_segmentation - step_t_segmentation).count() << " ms"<<std::endl;
// step_t_segmentation = end_t_segmentation;
// //---------------------------------
mutex_di.lock();
updates.disparity = disparity_frame.clone();
updates.update_di = true;
mutex_di.unlock();
}
pre_canny = canny.clone();
if(first_iteration)
first_iteration = false;
end_t = std::chrono::steady_clock::now();
std::cout<<"disparityFrame : TIME END pre canny : "<<std::chrono::duration_cast<std::chrono::milliseconds>(end_t - start_t).count() << " ms"<<std::endl;
}
}
}
@@ -185,17 +562,21 @@ int main(int argc, char *argv[])
yolo.init(net);
yolo.thresh = 0.25;
gRun = true;
cv::VideoCapture cap(input);
if (!cap.isOpened())
gRun = false;
else
std::cout << "camera started\n";
pthread_t visual;
pthread_t visual, videocap, originalshow, detectionshow, topviewshow, disparityshow;
//create video capture thread
Frame_t info_f;
info_f.input = input;
if (pthread_create(&videocap, NULL, readVideoCapture, (void*)&info_f))
{
fprintf(stderr, "Error creating thread\n");
return 1;
};
cv::Mat frame;
cv::Mat frame_crop;
cv::Mat frame_disp;
char buf_frame_crop_name [200];
cv::Mat dnn_input;
@@ -266,12 +647,13 @@ int main(int argc, char *argv[])
auto step_t_segmentation = std::chrono::steady_clock::now();
auto end_t_segmentation = std::chrono::steady_clock::now();
// information for the disparity map
//TODO: move in a thread
// // information for the disparity map
std::vector <cv::Rect> pre_rois;
cv::Mat pre_frame;
// cv::Mat pre_frame;
cv::Mat orig_frame;
cv::Mat canny, pre_canny, canny_RGB, pre_canny_RGB;
cv::Mat canny_img;
// cv::Mat canny, pre_canny, canny_RGB, pre_canny_RGB;
// cv::Mat canny_img;
// box variable
tk::dnn::box b;
@@ -282,44 +664,79 @@ int main(int argc, char *argv[])
cv::Scalar intensity;
cv::Rect roi;
InfoShow info_show;
ModFrame_t 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))
// initialize updates struct
updates.update_o = false;
updates.update_de = false;
updates.update_t = false;
updates.update_di = false;
if (pthread_create(&visual, NULL, show_updates, (void*)NULL))
{
fprintf(stderr, "Error creating thread\n");
return 1;
}
}
};
if (pthread_create(&originalshow, NULL, originalFrame, (void*)&info_f))
{
fprintf(stderr, "Error creating thread\n");
return 1;
};
if (pthread_create(&disparityshow, NULL, disparityFrame, (void*)&info_f))
{
fprintf(stderr, "Error creating thread\n");
return 1;
};
info_show.H = cv::Mat(cv::Size(3, 3), CV_64FC1);
if (pthread_create(&detectionshow, NULL, detectionFrame, (void*)&info_show))
{
fprintf(stderr, "Error creating thread\n");
return 1;
};
if (pthread_create(&topviewshow, NULL, topviewFrame, (void*)&info_show))
{
fprintf(stderr, "Error creating thread\n");
return 1;
};
// if (pthread_create(&detectionshow, NULL, showImages, (void *)&info_show))
// {
// fprintf(stderr, "Error creating thread\n");
// return 1;
// }
}
bool first_iteration = true;
while (gRun)
{
TIMER_START
start_t = std::chrono::steady_clock::now();
step_t = start_t;
cap >> frame;
orig_frame = frame.clone();
sem_vc.lock();
frame = info_f.frame.clone();
if(info_f.frame_nbr - frame_nbr > 1)
std::cout<<"more than one - f_n\n";
frame_nbr = info_f.frame_nbr;
sem_vc.unlock();
std::cout<<"f_n: "<<frame_nbr<<std::endl;
// if (!frame.data)
if (frame_nbr == 0)
{
usleep(1000000);
printf("no frame received\n");
continue;
}
orig_frame = frame.clone();
if (first_iteration)
cv::initUndistortRectifyMap(cameraMat, distCoeff, cv::Mat(), cameraMat, frame.size(), CV_16SC2, map1, map2);
cv::Mat temp = frame.clone();
cv::remap(temp, frame, map1, map2, 1);
//undistort(temp, frame, cameraMat, distCoeff);
if (!frame.data)
{
usleep(1000000);
cap.open(input);
printf("cap reinitialize\n");
continue;
}
// this will be resized to the net format
dnn_input = frame.clone();
// TODO: async infer
@@ -337,58 +754,59 @@ int main(int argc, char *argv[])
// draw dets
std::cout<<"num detected: "<<num_detected<<std::endl;
//preprocessing frame
step_t_segmentation = std::chrono::steady_clock::now();
// 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);
end_t_segmentation = std::chrono::steady_clock::now();
std::cout << " - TIME END pre canny : "<<std::chrono::duration_cast<std::chrono::milliseconds>(end_t_segmentation - step_t_segmentation).count() << " ms"<<std::endl;
step_t_segmentation = end_t_segmentation;
// std::cout<<"o: "<<orig_frame.cols<<" - "<<orig_frame.rows<<std::endl;
// std::cout<<"canny: "<<canny.cols<<" - "<<canny.rows<<std::endl;
// std::cout<<"pre: "<<pre_canny.cols<<" - "<<pre_canny.rows<<std::endl;
if(frame_nbr!=0)
{
// backtorgb = cv::cvtColor(pre_canny,cv::COLOR_GRAY2RGB)
cv::cvtColor(pre_canny, pre_canny_RGB, CV_GRAY2RGB);
cv::cvtColor(canny, canny_RGB, CV_GRAY2RGB);
frame_disp = frame_disparity(pre_canny_RGB, canny_RGB, frame_nbr, 999, 0);
std::cout<<"size: "<<frame_disp.rows<<" - "<<frame_disp.cols<<std::endl;
if (frame_disp.rows == 0 || frame_disp.cols == 0)
return -1;
if (frame_disp.empty())
{ // only fools don't check...
std::cout << "image not loaded !" << std::endl;
return -1;
}
end_t_segmentation = std::chrono::steady_clock::now();
std::cout << " TIME canny : frame_disparity : "<<std::chrono::duration_cast<std::chrono::milliseconds>(end_t_segmentation - step_t_segmentation).count() << " ms"<<std::endl;
step_t_segmentation = end_t_segmentation;
//TODO: move in a thread
// //preprocessing frame
// step_t_segmentation = std::chrono::steady_clock::now();
// // 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);
// end_t_segmentation = std::chrono::steady_clock::now();
// std::cout << " - TIME END pre canny : "<<std::chrono::duration_cast<std::chrono::milliseconds>(end_t_segmentation - step_t_segmentation).count() << " ms"<<std::endl;
// step_t_segmentation = end_t_segmentation;
// // std::cout<<"o: "<<orig_frame.cols<<" - "<<orig_frame.rows<<std::endl;
// // std::cout<<"canny: "<<canny.cols<<" - "<<canny.rows<<std::endl;
// // std::cout<<"pre: "<<pre_canny.cols<<" - "<<pre_canny.rows<<std::endl;
// if(!first_iteration)
// {
// // backtorgb = cv::cvtColor(pre_canny,cv::COLOR_GRAY2RGB)
// cv::cvtColor(pre_canny, pre_canny_RGB, CV_GRAY2RGB);
// cv::cvtColor(canny, canny_RGB, CV_GRAY2RGB);
// disparity_frame = frame_disparity(pre_canny_RGB, canny_RGB, frame_nbr, 999, 0);
// std::cout<<"size: "<<disparity_frame.rows<<" - "<<disparity_frame.cols<<std::endl;
// if (disparity_frame.rows == 0 || disparity_frame.cols == 0)
// return -1;
// if (disparity_frame.empty())
// { // only fools don't check...
// std::cout << "image not loaded !" << std::endl;
// return -1;
// }
// end_t_segmentation = std::chrono::steady_clock::now();
// std::cout << " TIME canny : frame_disparity : "<<std::chrono::duration_cast<std::chrono::milliseconds>(end_t_segmentation - step_t_segmentation).count() << " ms"<<std::endl;
// step_t_segmentation = end_t_segmentation;
// //--------------------------------
// //frame box disparity on the original image
// step_t_segmentation = std::chrono::steady_clock::now();
// frame_box_disparity(pre_frame, frame, pre_rois, frame_nbr);
// // reset pre_rois for the new roi of the current frame
// // pre_rois.erase(pre_rois.begin(), pre_rois.end());
// end_t_segmentation = std::chrono::steady_clock::now();
// std::cout << " TIME Frame disparity : "<<std::chrono::duration_cast<std::chrono::milliseconds>(end_t_segmentation - step_t_segmentation).count() << " ms"<<std::endl;
// step_t_segmentation = end_t_segmentation;
// // //--------------------------------
// // //frame box disparity on the original image
// // step_t_segmentation = std::chrono::steady_clock::now();
// // frame_box_disparity(pre_frame, frame, pre_rois, frame_nbr);
// // // reset pre_rois for the new roi of the current frame
// // // pre_rois.erase(pre_rois.begin(), pre_rois.end());
// // end_t_segmentation = std::chrono::steady_clock::now();
// // std::cout << " TIME Frame disparity : "<<std::chrono::duration_cast<std::chrono::milliseconds>(end_t_segmentation - step_t_segmentation).count() << " ms"<<std::endl;
// // step_t_segmentation = end_t_segmentation;
// //frame box disparity on the preprocessed image
// cv::cvtColor(pre_canny, pre_canny_RGB, CV_GRAY2RGB);
// cv::cvtColor(canny, canny_RGB, CV_GRAY2RGB);
// frame_box_disparity(pre_canny_RGB, canny_RGB, pre_rois, frame_nbr);
// // reset pre_rois for the new roi of the current frame
// pre_rois.erase(pre_rois.begin(), pre_rois.end());
// end_t_segmentation = std::chrono::steady_clock::now();
// std::cout << " TIME Canny Frame disparity : "<<std::chrono::duration_cast<std::chrono::milliseconds>(end_t_segmentation - step_t_segmentation).count() << " ms"<<std::endl;
// step_t_segmentation = end_t_segmentation;
// //---------------------------------
}
// // //frame box disparity on the preprocessed image
// // cv::cvtColor(pre_canny, pre_canny_RGB, CV_GRAY2RGB);
// // cv::cvtColor(canny, canny_RGB, CV_GRAY2RGB);
// // frame_box_disparity(pre_canny_RGB, canny_RGB, pre_rois, frame_nbr);
// // // reset pre_rois for the new roi of the current frame
// // pre_rois.erase(pre_rois.begin(), pre_rois.end());
// // end_t_segmentation = std::chrono::steady_clock::now();
// // std::cout << " TIME Canny Frame disparity : "<<std::chrono::duration_cast<std::chrono::milliseconds>(end_t_segmentation - step_t_segmentation).count() << " ms"<<std::endl;
// // step_t_segmentation = end_t_segmentation;
// // //---------------------------------
// }
// compute some metrics on the whole frame
// segmentation(pre_frame, frame, frame_nbr, 0, 0);
@@ -422,9 +840,9 @@ int main(int argc, char *argv[])
roi.height = (roi.y+h >= frame.rows)? frame.rows-1-roi.y : h;
// std::cout<<"w "<<roi.width<<" - h "<<roi.height<<std::endl;
// std::cout<<"wf "<<frame.cols<<" - hf "<<frame.rows<<std::endl;
std::cout<<"---"<<std::endl;
std::cout<<"x "<<roi.x<<" to "<<roi.width+roi.x<<" wf "<<frame.cols<<std::endl;
std::cout<<"y "<<roi.y<<" to "<<roi.height+roi.y<<" hf "<<frame.rows<<std::endl;
// std::cout<<"---"<<std::endl;
// std::cout<<"x "<<roi.x<<" to "<<roi.width+roi.x<<" wf "<<frame.cols<<std::endl;
// std::cout<<"y "<<roi.y<<" to "<<roi.height+roi.y<<" hf "<<frame.rows<<std::endl;
//update pre_roi for the next frame
pre_rois.push_back(roi);
@@ -433,22 +851,22 @@ int main(int argc, char *argv[])
/////
convert_coords(coords, x0 + b.w / 2, y1, objClass, H, adfGeoTransform, frame_nbr);
//std::cout<<objClass<<" ("<<prob<<"): "<<x0<<" "<<y0<<" "<<x1<<" "<<y1<<"\n";
cv::rectangle(frame, cv::Point(x0, y0), cv::Point(x1, y1), yolo.colors[objClass], 2);
// draw label
int baseline = 0;
float fontScale = 0.5;
int thickness = 2;
cv::Size textSize = getTextSize(det_class, cv::FONT_HERSHEY_SIMPLEX, fontScale, thickness, &baseline);
cv::rectangle(frame, cv::Point(x0, y0), cv::Point((x0 + textSize.width - 2), (y0 - textSize.height - 2)), yolo.colors[b.cl], -1);
cv::putText(frame, det_class, cv::Point(x0, (y0 - (baseline / 2))), cv::FONT_HERSHEY_SIMPLEX, fontScale, cv::Scalar(255, 255, 255), thickness);
// //std::cout<<objClass<<" ("<<prob<<"): "<<x0<<" "<<y0<<" "<<x1<<" "<<y1<<"\n";
// cv::rectangle(frame, cv::Point(x0, y0), cv::Point(x1, y1), yolo.colors[objClass], 2);
// // draw label
// int baseline = 0;
// float fontScale = 0.5;
// int thickness = 2;
// cv::Size textSize = getTextSize(det_class, cv::FONT_HERSHEY_SIMPLEX, fontScale, thickness, &baseline);
// cv::rectangle(frame, cv::Point(x0, y0), cv::Point((x0 + textSize.width - 2), (y0 - textSize.height - 2)), yolo.colors[b.cl], -1);
// cv::putText(frame, det_class, cv::Point(x0, (y0 - (baseline / 2))), cv::FONT_HERSHEY_SIMPLEX, fontScale, cv::Scalar(255, 255, 255), thickness);
}
}
}
end_t = std::chrono::steady_clock::now();
std::cout << " TIME 2 : "<<std::chrono::duration_cast<std::chrono::milliseconds>(end_t - step_t).count() << " ms"<<std::endl;
step_t = end_t;
// end_t = std::chrono::steady_clock::now();
// std::cout << " TIME 2 : "<<std::chrono::duration_cast<std::chrono::milliseconds>(end_t - step_t).count() << " ms"<<std::endl;
// step_t = end_t;
//convert from latitude and longitude to meters for ekf
cur_frame.clear();
for (size_t i = 0; i < coords.size(); i++)
@@ -456,7 +874,7 @@ int main(int argc, char *argv[])
gc.geodetic2Enu(coords[i].lat_, coords[i].long_, 0, &east, &north, &up);
cur_frame.push_back(Data(east, north, frame_nbr, coords[i].class_));
}
if (frame_nbr == 0)
if (first_iteration)
{
for (auto f : cur_frame)
trackers.push_back(Tracker(f, initial_age, dt, n_states));
@@ -477,9 +895,9 @@ int main(int argc, char *argv[])
if (to_show)
{
//populate the InfoShow
//populate the ModFrame_t
sem.lock();
info_show.frame_v = frame.clone();
info_show.original_frame = frame.clone();
// std::vector<Tracker> trackers;
info_show.trackers = trackers;
// geodetic_converter::GeodeticConverter gc;
@@ -488,15 +906,19 @@ int main(int argc, char *argv[])
info_show.adfGeoTransform[i] = adfGeoTransform[i];
// cv::Mat H;
info_show.H = H.clone();
info_show.frame_disp = frame_disp.clone();
info_show.yolo = yolo;
info_show.mask = mask.clone();
sem.unlock();
}
// update pre_frame for the disparity map
pre_frame = orig_frame.clone();
pre_canny = canny.clone();
// pre_frame = orig_frame.clone();
// pre_canny = canny.clone();
if(first_iteration)
first_iteration = false;
frame_nbr++;
std::cout<<"MAIN thread: ";
TIMER_STOP
}
include/classutils.h
+2 -2
View File
@@ -215,14 +215,14 @@ void addRoadUserfromTracker(const std::vector<Tracker> &trackers, Message *m, ge
int pix_x, pix_y;
coord2pixel(lat, lon, pix_x, pix_y, adfGeoTransform);
std::cout<<maskOrient.at<cv::Vec3b>(pix_y,pix_x)<<std::endl;
// std::cout<<maskOrient.at<cv::Vec3b>(pix_y,pix_x)<<std::endl;
uint8_t maskOrientPixel = maskOrient.at<cv::Vec3b>(pix_y,pix_x)[0];
uint8_t orientation;
if(maskOrientPixel != 0)
{
orientation = maskOrientPixel;
std::cout<<"orientation given by the mask "<< int(orientation)<<std::endl;
// std::cout<<"orientation given by the mask "<< int(orientation)<<std::endl;
}
else
{