Davide Sapienza
1090 lines
40 KiB
C++
1090 lines
40 KiB
C++
#include <iostream>
|
|
#include <signal.h>
|
|
#include <stdlib.h> /* srand, rand */
|
|
#include <unistd.h>
|
|
#include <mutex>
|
|
#include <ctime>
|
|
#include <pthread.h>
|
|
|
|
#include <time.h>
|
|
#include <chrono>
|
|
#include <math.h>
|
|
#include <typeinfo>
|
|
|
|
#include "utils.h"
|
|
#include "BoxDetection.h"
|
|
|
|
#include <opencv2/core/core.hpp>
|
|
#include <opencv2/highgui/highgui.hpp>
|
|
#include <opencv2/imgproc/imgproc.hpp>
|
|
|
|
//saliency
|
|
#include <opencv2/core/utility.hpp>
|
|
#include <opencv2/saliency.hpp>
|
|
#include <opencv2/highgui.hpp>
|
|
|
|
#include "Yolo3Detection.h"
|
|
#include "classutils.h"
|
|
#include "../masa_protocol/include/send.hpp"
|
|
#include "../masa_protocol/include/serialize.hpp"
|
|
|
|
#include "ekf.h"
|
|
#include "trackutils.h"
|
|
#include "plot.h"
|
|
#include "tracker.h"
|
|
#include <assert.h>
|
|
|
|
#define MAX_DETECT_SIZE 100
|
|
|
|
std::chrono::steady_clock::time_point local_clock_start;
|
|
|
|
std::mutex mutexgRun;
|
|
|
|
bool gRun;
|
|
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;
|
|
|
|
struct ModFrame_t{
|
|
std::vector<Tracker> trackers;
|
|
geodetic_converter::GeodeticConverter gc;
|
|
double adfGeoTransform[6];
|
|
cv::Mat H;
|
|
cv::Mat original_frame;
|
|
tk::dnn::Yolo3Detection yolo;
|
|
cv::Mat mask;
|
|
// sem for mainthread, detectionthread and topviewthread
|
|
std::mutex sem;
|
|
};
|
|
|
|
struct Frame_t{
|
|
char *input;
|
|
cv::Mat frame;
|
|
int frame_nbr;
|
|
// sem_vc for mainthread, videocapturethread, originalthread and disparitythread
|
|
std::mutex sem_vc;
|
|
};
|
|
|
|
struct Camera_t{
|
|
int CAM_IDX;
|
|
char *input;
|
|
char *pmatrix;
|
|
char *maskfile;
|
|
char *cameraCalib;
|
|
char *maskFileOrient;
|
|
bool to_show;
|
|
tk::dnn::Yolo3Detection yolo;
|
|
double adfGeoTransform[6];
|
|
};
|
|
|
|
struct Show_t{
|
|
cv::Mat original, detection, topview, disparity;
|
|
bool update_o, update_de, update_t, update_di;
|
|
// a single mutex for each operation - the show_updates function must get all mutex
|
|
std::mutex mutex_o, mutex_de, mutex_t, mutex_di;
|
|
}updates;
|
|
|
|
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;
|
|
}
|
|
|
|
/* 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(updates.mutex_o.try_lock())
|
|
{
|
|
update_o_loc = updates.update_o;
|
|
updates.update_o = false;
|
|
if(update_o_loc)
|
|
original_loc = updates.original.clone();
|
|
updates.mutex_o.unlock();
|
|
}
|
|
|
|
if(updates.mutex_de.try_lock())
|
|
{
|
|
update_de_loc = updates.update_de;
|
|
updates.update_de = false;
|
|
if(update_de_loc)
|
|
detection_loc = updates.detection.clone();
|
|
updates.mutex_de.unlock();
|
|
}
|
|
|
|
if(updates.mutex_t.try_lock())
|
|
{
|
|
update_t_loc = updates.update_t;
|
|
updates.update_t = false;
|
|
if(update_t_loc)
|
|
topview_loc = updates.topview.clone();
|
|
updates.mutex_t.unlock();
|
|
}
|
|
|
|
if(updates.mutex_di.try_lock())
|
|
{
|
|
update_di_loc = updates.update_di;
|
|
updates.update_di = false;
|
|
if(update_di_loc)
|
|
disparity_loc = updates.disparity.clone();
|
|
updates.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
|
|
}
|
|
return (void *)0;
|
|
}
|
|
|
|
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
|
|
info_show_orig->sem_vc.lock();
|
|
frame_loc = info_show_orig->frame.clone();
|
|
frame_nbr_loc = info_show_orig->frame_nbr;
|
|
info_show_orig->sem_vc.unlock();
|
|
if (frame_nbr_loc == 0)
|
|
{
|
|
usleep(1000000);
|
|
printf("no frame received\n");
|
|
continue;
|
|
}
|
|
updates.mutex_o.lock();
|
|
updates.original = frame_loc.clone();
|
|
updates.update_o = true;
|
|
updates.mutex_o.unlock();
|
|
usleep(10000); //sleep 10 msec
|
|
std::cout<<"originalFrame: ";
|
|
TIMER_STOP
|
|
}
|
|
return (void *)0;
|
|
}
|
|
|
|
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;
|
|
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;
|
|
std::vector<cv::Point2f> map_p, camera_p;
|
|
int baseline = 0;
|
|
float fontScale = 0.5;
|
|
int thickness = 2;
|
|
|
|
while (gRun)
|
|
{
|
|
TIMER_START
|
|
// critical section: copy the struct in local variable
|
|
// in this way we can unlock the sem for the main thread
|
|
info_show->sem.lock();
|
|
original_frame_loc = info_show->original_frame.clone();
|
|
// 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();
|
|
yolo = info_show->yolo;
|
|
mask = info_show->mask.clone();
|
|
info_show->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
|
|
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);
|
|
|
|
map_p.clear();
|
|
camera_p.clear();
|
|
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<<"x,y: "<<pix_x<<", "<<pix_y<<std::endl;
|
|
// std::cout<<"map_p: "<<map_p<<std::endl;
|
|
// std::cout<<"camera_p: "<<camera_p<<std::endl;
|
|
// std::cout<<"size original_frame_loc: "<<original_frame_loc.cols<<", "<<original_frame_loc.rows<<std::endl;
|
|
// assert (camera_p[0].x < original_frame_loc.cols);
|
|
// assert (camera_p[0].y < original_frame_loc.rows);
|
|
if (camera_p[0].x < original_frame_loc.cols && camera_p[0].y < original_frame_loc.rows && camera_p[0].x >= 0 && camera_p[0].y >= 0)
|
|
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);
|
|
}
|
|
}
|
|
|
|
updates.mutex_de.lock();
|
|
updates.detection = original_frame_loc.clone();
|
|
updates.update_de = true;
|
|
updates.mutex_de.unlock();
|
|
|
|
std::cout<<"detectionFrame: ";
|
|
TIMER_STOP
|
|
|
|
}
|
|
return (void *)0;
|
|
}
|
|
|
|
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
|
|
info_show->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();
|
|
info_show->sem.unlock();
|
|
if (trackers.empty())
|
|
{
|
|
usleep(1000000);
|
|
printf("no data available\n");
|
|
continue;
|
|
}
|
|
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);
|
|
coord2pixel(lat, lon, pix_x, pix_y, adfGeoTransform);
|
|
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);
|
|
|
|
}
|
|
}
|
|
//outputVideo<< frame_top;
|
|
// ------------------------------------------------
|
|
updates.mutex_t.lock();
|
|
updates.topview = frame_top.clone();
|
|
updates.update_t = true;
|
|
updates.mutex_t.unlock();
|
|
|
|
std::cout<<"topviewFrame: ";
|
|
TIMER_STOP
|
|
|
|
}
|
|
return (void *)0;
|
|
}
|
|
|
|
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
|
|
info_show_disparity->sem_vc.lock();
|
|
frame_loc = info_show_disparity->frame.clone();
|
|
frame_nbr_loc = info_show_disparity->frame_nbr;
|
|
info_show_disparity->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;
|
|
// //---------------------------------
|
|
|
|
updates.mutex_di.lock();
|
|
updates.disparity = disparity_frame.clone();
|
|
updates.update_di = true;
|
|
updates.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;
|
|
}
|
|
}
|
|
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;
|
|
//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::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
|
|
camera->yolo.update(dnn_input);
|
|
int num_detected = camera->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_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 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);
|
|
|
|
for (int i = 0; i < num_detected; i++)
|
|
{
|
|
b = camera->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, frame_nbr);
|
|
|
|
|
|
// //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 = camera->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));
|
|
bool no_params = false; //flag to indicate if there are camera parameters or we use mp4 test video
|
|
|
|
char *net = (char *)"yolo3_coco4.rt";
|
|
if (argc > 1)
|
|
net = argv[1];
|
|
char *tiffile = (char *)"../demo/demo/data/map_b.tif";
|
|
if (argc > 2)
|
|
tiffile = argv[2];
|
|
char *n;
|
|
if (argc > 3)
|
|
{
|
|
n = argv[3];
|
|
if(strcmp(n, "-n"))
|
|
return -1;
|
|
};
|
|
int n_cameras = 0;
|
|
if (argc > 4)
|
|
{
|
|
n_cameras = atoi(argv[4]);
|
|
if(argc < 5+ 7 * n_cameras)
|
|
{
|
|
std::cout<<"too few parameters\n";
|
|
return -1;
|
|
}
|
|
if(!n_cameras)
|
|
{
|
|
n_cameras = 1;
|
|
no_params = true;
|
|
}
|
|
}
|
|
Camera_t cameras[n_cameras];
|
|
bool *to_show = (bool *) malloc(n_cameras * sizeof(bool));
|
|
if(no_params)
|
|
{
|
|
cameras[0].CAM_IDX = 20936;
|
|
cameras[0].input = (char *)"../demo/demo/data/single_ped_2.mp4";
|
|
cameras[0].pmatrix = (char *)"../demo/demo/data/pmundist.txt";
|
|
cameras[0].maskfile = (char *)"../demo/demo/data/mask36.jpg";
|
|
cameras[0].cameraCalib = (char *)"../demo/demo/data/calib36.params";
|
|
cameras[0].maskFileOrient = (char *)"../demo/demo/data/mask_orient/6315_mask_orient.jpg";
|
|
cameras[0].to_show = true;
|
|
to_show[0] = true;
|
|
}
|
|
else
|
|
{
|
|
for(int i = 0; i<n_cameras; i++)
|
|
cameras[i].CAM_IDX = atoi(argv[5+i]);
|
|
for(int i = 0; i<n_cameras; i++)
|
|
cameras[i].input = argv[5+n_cameras+i];
|
|
for(int i = 0; i<n_cameras; i++)
|
|
cameras[i].pmatrix = argv[5+2*n_cameras+i];
|
|
for(int i = 0; i<n_cameras; i++)
|
|
cameras[i].maskfile = argv[5+3*n_cameras+i];
|
|
for(int i = 0; i<n_cameras; i++)
|
|
cameras[i].cameraCalib = argv[5+4*n_cameras+i];
|
|
for(int i = 0; i<n_cameras; i++)
|
|
cameras[i].maskFileOrient = argv[5+5*n_cameras+i];
|
|
for(int i = 0; i<n_cameras; i++)
|
|
{
|
|
cameras[i].to_show = atoi(argv[5+6*n_cameras+i]); //only one camera can be shown
|
|
to_show[i] = atoi(argv[5+6*n_cameras+i]);
|
|
}
|
|
}
|
|
//TODO now only one camera can be visualized
|
|
int check_visualization=0;
|
|
for(int i = 0; i<n_cameras; i++)
|
|
check_visualization += to_show[i];
|
|
if(check_visualization > 1)
|
|
return -1;
|
|
|
|
|
|
tk::dnn::Yolo3Detection yolo[n_cameras];
|
|
for(int i=0; i<n_cameras; i++)
|
|
{
|
|
yolo[i].init(net);
|
|
yolo[i].thresh = 0.25;
|
|
}
|
|
// 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(tiffile, adfGeoTransform);
|
|
|
|
for(int i=0; i<n_cameras; i++)
|
|
{
|
|
for(int j = 0; j < 6; j++ )
|
|
cameras[i].adfGeoTransform[j] = adfGeoTransform[j];
|
|
cameras[i].yolo = yolo[i];
|
|
// cameras[i].yolo = yolo;
|
|
|
|
}
|
|
std::cout<<"INIZIA:\n";
|
|
pthread_t camera_task[n_cameras];
|
|
for(int i=0; i<n_cameras; i++)
|
|
{
|
|
std::cout<<"creating thread\n";
|
|
if(pthread_create(&camera_task[i], NULL, computationTask, (void*)&cameras[i]))
|
|
{
|
|
fprintf(stderr, "error creating thread\n");
|
|
return 1;
|
|
}
|
|
std::cout<<"WHAT2\n";
|
|
}
|
|
for(int i=0; i<n_cameras; i++)
|
|
{
|
|
pthread_join(camera_task[i], NULL);
|
|
}
|
|
std::cout <<" free adfGeoT \n";
|
|
free(adfGeoTransform);
|
|
std::cout << "detection end\n";
|
|
return 0;
|
|
}
|