Micaela Verucchi
603 lines
23 KiB
C++
603 lines
23 KiB
C++
#include <time.h>
|
|
|
|
#include "utils.h"
|
|
#include "Yolo3Detection.h"
|
|
#include "message.h"
|
|
#include "visualization.h"
|
|
#include "configuration.h"
|
|
|
|
#include "tracker.h"
|
|
#include "../masa_protocol/include/send.hpp"
|
|
#include "../masa_protocol/include/serialize.hpp"
|
|
|
|
// #include <assert.h>
|
|
// #include <unistd.h>
|
|
// #include <mutex>
|
|
// #include <ctime>
|
|
// #include <pthread.h>
|
|
// #include <signal.h>
|
|
// #include <chrono>
|
|
// #include <math.h>
|
|
// #include <typeinfo>
|
|
// #include <iostream>
|
|
|
|
#define MAX_DETECT_SIZE 100
|
|
|
|
bool gRun;
|
|
std::chrono::steady_clock::time_point local_clock_start;
|
|
std::mutex mutexgRun;
|
|
std::string obj_class[10]{"person", "car", "truck", "bus", "motor", "bike", "rider", "traffic light", "traffic sign", "train"};
|
|
//mutex for some opencv operations
|
|
std::mutex mutex_cv;
|
|
Show_t updates;
|
|
bool SAVE_RESULT = false;
|
|
|
|
void sig_handler(int signo)
|
|
{
|
|
std::cout << "request gateway stop\n";
|
|
mutexgRun.lock();
|
|
gRun = false;
|
|
mutexgRun.unlock();
|
|
}
|
|
|
|
void *readVideoCapture(void *x_void_ptr)
|
|
{
|
|
std::cout << "readVideoCapture start...\n";
|
|
|
|
Frame_t *info_f = (Frame_t *)x_void_ptr;
|
|
mutex_cv.lock();
|
|
cv::VideoCapture cap(info_f->input, cv::CAP_FFMPEG);
|
|
mutex_cv.unlock();
|
|
cv::Mat frame_loc, frame0;
|
|
int frame_nbr_loc = 0;
|
|
// bool to_show = false;
|
|
if (!cap.isOpened())
|
|
{
|
|
mutexgRun.lock();
|
|
gRun = false;
|
|
mutexgRun.unlock();
|
|
}
|
|
else
|
|
std::cout << "camera started\n";
|
|
|
|
// cap.set(cv::CAP_PROP_BUFFERSIZE,3);
|
|
// std::cout<<"buf size: "<<cap.get(CV_CAP_PROP_BUFFERSIZE)<<std::endl;
|
|
auto start_t = std::chrono::steady_clock::now();
|
|
auto step_t = std::chrono::steady_clock::now();
|
|
auto end_t = std::chrono::steady_clock::now();
|
|
auto current_timestamp = std::chrono::steady_clock::now();
|
|
|
|
// compute fps and find camera's clock
|
|
double shift, mean_time = 0;
|
|
std::cout << "Frames per second using video.get(cv::CAP_PROP_FPS) : " << cap.get(cv::CAP_PROP_FPS) << std::endl;
|
|
std::cout << "readVideoCapture computes frame rate...\n";
|
|
// //compute frame rate
|
|
int i = 0;
|
|
int num_f = 120;
|
|
// the first 20 frames are null
|
|
while (i < 21)
|
|
{
|
|
cap >> frame_loc;
|
|
i++;
|
|
}
|
|
|
|
i = 0;
|
|
start_t = std::chrono::steady_clock::now();
|
|
while (i < num_f)
|
|
{
|
|
step_t = std::chrono::steady_clock::now();
|
|
cap >> frame_loc;
|
|
mean_time = mean_time + std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now() - step_t).count();
|
|
std::cout << " step " << i << " : " << std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now() - step_t).count() << " ms" << std::endl;
|
|
i++;
|
|
}
|
|
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;
|
|
|
|
/*
|
|
mean_time indicates the milliseconds from a frame and the next. (frame rate)
|
|
local_clock_sync is the camera clock.
|
|
shift is the difference from local camera clock and local process clock.
|
|
a frame is allowed if its local timestamp minus its local clock is less then a tollerance,
|
|
otherwise it will be considered old.
|
|
*/
|
|
auto local_clock_sync = std::chrono::steady_clock::now();
|
|
mean_time = mean_time / num_f;
|
|
shift = ((double)std::chrono::duration_cast<std::chrono::milliseconds>(local_clock_sync - local_clock_start).count()) / mean_time;
|
|
shift = (shift - (int)shift) * mean_time;
|
|
std::cout << ".-------------------------------\n";
|
|
std::cout << " mean time: " << mean_time << std::endl;
|
|
std::cout << " shift: " << shift << std::endl;
|
|
std::cout << " TIMEDIFFERENCE: " << std::chrono::duration_cast<std::chrono::milliseconds>(local_clock_sync - local_clock_start).count() << std::endl;
|
|
std::cout << "\n\n\n\n";
|
|
std::cout << "readVideoCapture start to capture...\n";
|
|
while (gRun)
|
|
{
|
|
// mutex_cv.lock();
|
|
cap >> frame_loc;
|
|
// mutex_cv.unlock();
|
|
current_timestamp = std::chrono::steady_clock::now();
|
|
shift = std::chrono::duration_cast<std::chrono::milliseconds>(current_timestamp - local_clock_sync).count();
|
|
std::cout << " RELATIVE TIMESTAMP FRAME : " << shift << " ms" << std::endl;
|
|
shift = shift / mean_time;
|
|
shift = (shift - (int)shift) * mean_time;
|
|
shift = (shift - mean_time / 2 >= 0) ? -(mean_time - shift) : shift;
|
|
std::cout << "DELAY frame_" << frame_nbr_loc << " : " << shift << " ms" << std::endl;
|
|
// TODO: here introduce a tollerance to discard old frame
|
|
|
|
// std::cout<< "CV_CAP_PROP_POS_MSEC: "<< cap.get( cv::CAP_PROP_POS_MSEC) <<std::endl;
|
|
// std::cout<< "CV_CAP_PROP_POS_FRAMES: "<< cap.get( cv::CAP_PROP_POS_FRAMES) <<std::endl; // <-- the v4l2 'sequence' field
|
|
// std::cout<< "cv::CAP_PROP_FPS: "<< cap.get( cv::CAP_PROP_FPS)<<std::endl;
|
|
// std::cout << "Format: " << cap.get(CV_CAP_PROP_FORMAT) << "\n";
|
|
// 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);
|
|
mutex_cv.lock();
|
|
cap.open(info_f->input);
|
|
printf("cap reinitialize\n");
|
|
mutex_cv.unlock();
|
|
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;
|
|
|
|
info_f->sem_vc.lock();
|
|
info_f->frame = frame_loc.clone();
|
|
info_f->frame_nbr = frame_nbr_loc;
|
|
info_f->sem_vc.unlock();
|
|
// usleep(50000);
|
|
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++;
|
|
}
|
|
return (void *)0;
|
|
}
|
|
|
|
void *computationTask(void *x_void_ptr)
|
|
{
|
|
Camera_t *camera = (Camera_t *)x_void_ptr;
|
|
pthread_t visual, originalshow, detectionshow, topviewshow, disparityshow;
|
|
pthread_t videocap;
|
|
tk::dnn::Yolo3Detection yolo = *(camera->yolo);
|
|
//create video capture thread
|
|
Frame_t info_f;
|
|
info_f.input = camera->input;
|
|
if (pthread_create(&videocap, NULL, readVideoCapture, (void *)&info_f))
|
|
{
|
|
fprintf(stderr, "Error creating thread\n");
|
|
return (void *)1;
|
|
};
|
|
|
|
bool to_show = camera->to_show;
|
|
double adfGeoTransform[6];
|
|
for (int i = 0; i < 6; i++)
|
|
adfGeoTransform[i] = camera->adfGeoTransform[i];
|
|
|
|
ModFrame_t info_show;
|
|
if (to_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 (void *)1;
|
|
};
|
|
if (pthread_create(&originalshow, NULL, originalFrame, (void *)&info_f))
|
|
{
|
|
fprintf(stderr, "Error creating thread\n");
|
|
return (void *)1;
|
|
};
|
|
if (pthread_create(&disparityshow, NULL, disparityFrame, (void *)&info_f))
|
|
{
|
|
fprintf(stderr, "Error creating thread\n");
|
|
return (void *)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 (void *)1;
|
|
};
|
|
if (pthread_create(&topviewshow, NULL, topviewFrame, (void *)&info_show))
|
|
{
|
|
fprintf(stderr, "Error creating thread\n");
|
|
return (void *)1;
|
|
};
|
|
}
|
|
char *pmatrix = camera->pmatrix;
|
|
/*projection matrix from camera to map*/
|
|
cv::Mat H(cv::Size(3, 3), CV_64FC1);
|
|
read_projection_matrix(H, pmatrix);
|
|
assert(cv::countNonZero(H) > 0);
|
|
// std::cout<<H<<std::endl;
|
|
// return (void*)0;
|
|
/*Camera calibration*/
|
|
cv::Mat cameraMat, distCoeff;
|
|
readCameraCalibrationYaml(camera->cameraCalib, cameraMat, distCoeff);
|
|
std::cout << cameraMat << std::endl;
|
|
std::cout << distCoeff << std::endl;
|
|
|
|
/*GPS information*/
|
|
std::vector<ObjCoords> coords;
|
|
|
|
/*socket*/
|
|
Communicator Comm(SOCK_DGRAM);
|
|
Comm.open_client_socket((char *)"127.0.0.1", 8888);
|
|
|
|
Message *m = new Message;
|
|
m->cam_idx = camera->CAM_IDX;
|
|
m->lights.clear();
|
|
/*Conversion for tracker, from gps to meters and viceversa*/
|
|
// mutex_cv.lock();
|
|
geodetic_converter::GeodeticConverter gc;
|
|
gc.initialiseReference(44.655540, 10.934315, 0);
|
|
// mutex_cv.unlock();
|
|
double east, north, up;
|
|
// double lat, lon, alt;
|
|
/*Mask info*/
|
|
cv::Mat mask = cv::imread(camera->maskfile, cv::IMREAD_GRAYSCALE);
|
|
cv::Mat maskOrient = cv::imread(camera->maskFileOrient);
|
|
// cv::Mat maskOrient = cv::imread(camera->maskFileOrient, 0);
|
|
|
|
/*for(int i=0; i< mask.cols; i++)
|
|
{
|
|
for(int j=0; j< mask.rows; j++)
|
|
{
|
|
std::cout<<maskOrient.at<cv::Vec3b>(i,j) <<std::endl;
|
|
}
|
|
}
|
|
|
|
|
|
return 0;*/
|
|
/*tracker infos*/
|
|
std::vector<Tracker> trackers;
|
|
std::vector<Data> cur_frame;
|
|
int initial_age = -5;
|
|
int age_threshold = -8;
|
|
int n_states = 5;
|
|
float dt = 0.03;
|
|
|
|
int frame_nbr = 0;
|
|
|
|
//save video
|
|
/*cv::VideoWriter outputVideo;
|
|
cv::Size S = cv::Size((int)cap.get(cv::CAP_PROP_FRAME_WIDTH), //Acquire input size
|
|
(int)cap.get(cv::CAP_PROP_FRAME_HEIGHT));
|
|
outputVideo.open("test.avi", static_cast<int>(cap.get(cv::CAP_PROP_FOURCC)), cap.get(cv::CAP_PROP_FPS), S, true);*/
|
|
|
|
cv::Mat map1, map2;
|
|
auto start_t = std::chrono::steady_clock::now();
|
|
auto step_t = std::chrono::steady_clock::now();
|
|
auto end_t = std::chrono::steady_clock::now();
|
|
// auto step_t_segmentation = std::chrono::steady_clock::now();
|
|
// auto end_t_segmentation = std::chrono::steady_clock::now();
|
|
|
|
//TODO: move in a thread
|
|
// // information for the disparity map
|
|
// std::vector <cv::Rect> pre_rois;
|
|
// cv::Mat pre_frame;
|
|
cv::Mat orig_frame;
|
|
// cv::Mat canny, pre_canny, canny_RGB, pre_canny_RGB;
|
|
// cv::Mat canny_img;
|
|
|
|
// box variable
|
|
tk::dnn::box b;
|
|
int x0, h, y1; //w, x1, y0;
|
|
int objClass;
|
|
std::string det_class;
|
|
;
|
|
// float prob;
|
|
cv::Scalar intensity;
|
|
|
|
|
|
// cv::VideoWriter resultVideo;
|
|
// if(SAVE_RESULT) {
|
|
// int w = cap.get(cv::CAP_PROP_FRAME_WIDTH);
|
|
// int h = cap.get(cv::CAP_PROP_FRAME_HEIGHT);
|
|
// resultVideo.open("result.mp4", cv::VideoWriter::fourcc('M','P','4','V'), 30, cv::Size(w, h));
|
|
// }
|
|
|
|
cv::Mat frame;
|
|
cv::Mat frame_crop;
|
|
cv::Mat dnn_input;
|
|
bool first_iteration = true;
|
|
|
|
while (gRun)
|
|
{
|
|
TIMER_START
|
|
start_t = std::chrono::steady_clock::now();
|
|
step_t = start_t;
|
|
|
|
info_f.sem_vc.lock();
|
|
frame = info_f.frame.clone();
|
|
if (info_f.frame_nbr - frame_nbr > 1)
|
|
std::cout << "more than one - f_n (diff " << info_f.frame_nbr - frame_nbr << ")\n";
|
|
frame_nbr = info_f.frame_nbr;
|
|
info_f.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();
|
|
// mutex_cv.lock();
|
|
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);
|
|
// mutex_cv.unlock();
|
|
|
|
// this will be resized to the net format
|
|
dnn_input = frame.clone();
|
|
// TODO: async infer
|
|
yolo.update(dnn_input);
|
|
int num_detected = yolo.detected.size();
|
|
if (num_detected > MAX_DETECT_SIZE)
|
|
num_detected = MAX_DETECT_SIZE;
|
|
|
|
coords.clear();
|
|
end_t = std::chrono::steady_clock::now();
|
|
std::cout << " TIME 1 : " << std::chrono::duration_cast<std::chrono::milliseconds>(end_t - step_t).count() << " ms" << std::endl;
|
|
step_t = end_t;
|
|
// draw dets
|
|
std::cout << "camera: " << camera->CAM_IDX << " - num detected: " << num_detected << std::endl;
|
|
|
|
//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::COLOR_GRAY2RGB);
|
|
// cv::cvtColor(canny, canny_RGB, cv::COLOR_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 preprocessed image
|
|
// // cv::cvtColor(pre_canny, pre_canny_RGB, cv::COLOR_GRAY2RGB);
|
|
// // cv::cvtColor(canny, canny_RGB, cv::COLOR_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);
|
|
|
|
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])
|
|
{
|
|
|
|
if (objClass < 6)
|
|
{
|
|
|
|
// find the rectangular on the frame (sub-figure)
|
|
// roi.x = (x0 > 0)? x0 : 0;
|
|
// roi.y = (y0 > 0)? y0 : 0;
|
|
// // std::cout<<"x "<<roi.x<<" - y "<<roi.y<<std::endl;
|
|
// 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 "<<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;
|
|
//update pre_roi for the next frame
|
|
// pre_rois.push_back(roi);
|
|
|
|
// segmentation(frame(roi), frame(roi), frame_nbr, i, 1);
|
|
|
|
/////
|
|
convert_coords(coords, x0 + b.w / 2, y1, objClass, H, adfGeoTransform);
|
|
|
|
// //std::cout<<objClass<<" ("<<prob<<"): "<<x0<<" "<<y0<<" "<<x1<<" "<<y1<<"\n";
|
|
// cv::rectangle(frame, cv::Point(x0, y0), cv::Point(x1, y1), camera->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)), camera->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;
|
|
//convert from latitude and longitude to meters for ekf
|
|
cur_frame.clear();
|
|
for (size_t i = 0; i < coords.size(); i++)
|
|
{
|
|
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 (first_iteration)
|
|
{
|
|
// if there aren't detections and it is the first iteration, we can't initialize the tracker, so continue
|
|
if (cur_frame.empty())
|
|
continue;
|
|
for (auto f : cur_frame)
|
|
trackers.push_back(Tracker(f, initial_age, dt, n_states));
|
|
}
|
|
else
|
|
{
|
|
Track(cur_frame, dt, n_states, initial_age, age_threshold, trackers);
|
|
}
|
|
std::cout << "There are " << trackers.size() << " trackers" << std::endl;
|
|
//prepare message with tracker info
|
|
if (trackers.size() != 0)
|
|
{
|
|
// mutex_cv.lock();
|
|
addRoadUserfromTracker(trackers, m, gc, maskOrient, adfGeoTransform, H);
|
|
// mutex_cv.unlock();
|
|
//prepare the message with detection info
|
|
//prepare_message(m, coords, CAM_IDX);
|
|
//send message
|
|
if (!m->objects.empty())
|
|
Comm.send_message(m);
|
|
}
|
|
|
|
if (to_show)
|
|
{
|
|
//populate the ModFrame_t
|
|
info_show.sem.lock();
|
|
info_show.original_frame = frame.clone();
|
|
// std::vector<Tracker> 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();
|
|
info_show.yolo = yolo;
|
|
// std::copy(camera->yolo.begin(), camera->yolo.end(), info_show.yolo.begin());
|
|
info_show.mask = mask.clone();
|
|
info_show.sem.unlock();
|
|
}
|
|
|
|
// update pre_frame for the disparity map
|
|
// pre_frame = orig_frame.clone();
|
|
// pre_canny = canny.clone();
|
|
if (first_iteration)
|
|
first_iteration = false;
|
|
|
|
frame_nbr++;
|
|
std::cout << camera->CAM_IDX << " camera thread: ";
|
|
TIMER_STOP
|
|
}
|
|
return (void *)0;
|
|
}
|
|
|
|
int main(int argc, char *argv[])
|
|
{
|
|
|
|
std::cout << "detection\n";
|
|
signal(SIGINT, sig_handler);
|
|
srand(time(NULL));
|
|
|
|
Parameters_t par;
|
|
|
|
if(!read_parameters(argc, argv, &par))
|
|
return -1;
|
|
|
|
tk::dnn::Yolo3Detection yolo[par.n_cameras];
|
|
for(int i=0; i<par.n_cameras; i++)
|
|
{
|
|
yolo[i].init(par.net);
|
|
yolo[i].thresh = 0.25;
|
|
|
|
// if(SAVE_RESULT)
|
|
// resultVideo << frame;
|
|
}
|
|
// tk::dnn::Yolo3Detection yolo;
|
|
// yolo.init(net);
|
|
// yolo.thresh = 0.25;
|
|
|
|
gRun = true;
|
|
// start the local clock. It is used to check the incoming frames (by different cameras)
|
|
local_clock_start = std::chrono::steady_clock::now();
|
|
|
|
/*GPS information*/
|
|
double *adfGeoTransform = (double *)malloc(6 * sizeof(double));
|
|
readTiff(par.tiffile, adfGeoTransform);
|
|
// Camera_t cameras[par.n_cameras];
|
|
for(int i=0; i<par.n_cameras; i++)
|
|
{
|
|
for(int j = 0; j < 6; j++ )
|
|
par.cameras[i].adfGeoTransform[j] = adfGeoTransform[j];
|
|
par.cameras[i].yolo = &yolo[i];
|
|
// par.cameras[i].yolo.init(par.net);
|
|
// par.cameras[i].yolo.thresh = 0.25;
|
|
// cameras[i].yolo = yolo[i];
|
|
// cameras[i].yolo = yolo;
|
|
|
|
}
|
|
pthread_t camera_task[par.n_cameras];
|
|
for(int i=0; i<par.n_cameras; i++)
|
|
{
|
|
std::cout<<"creating thread\n";
|
|
if(pthread_create(&camera_task[i], NULL, computationTask, (void*)&(par.cameras[i])))
|
|
{
|
|
fprintf(stderr, "error creating thread\n");
|
|
return 1;
|
|
}
|
|
}
|
|
for(int i=0; i<par.n_cameras; i++)
|
|
{
|
|
pthread_join(camera_task[i], NULL);
|
|
}
|
|
std::cout <<" free adfGeoT \n";
|
|
free(adfGeoTransform);
|
|
std::cout << "detection end\n";
|
|
return 0;
|
|
} |