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

tracking integrated

Browse Source
This commit is contained in:
Micaela Verucchi
2019-04-20 15:48:52 +02:00
parent 9a4a65a3c3
commit 9b413b77ab
5 changed files with 139 additions and 142 deletions
Download Patch File Download Diff File Expand all files Collapse all files
demo/demo/data/proj_matrix_map_b.txt
+3 -3
View File
@@ -1,3 +1,3 @@
0.1472053693584627 -13.052205853952705 439.9459667398692 -0.4122368700442484 -10.479982981650977 812.1821012303
-0.6469057430349019 -6.339418443938649 578.7156423035825 -0.6536846365005352 -4.951476039607947 512.0831604767536
-0.00025681337819676855 -0.007446975429175878 1.0 -0.00042630219831388434 -0.006003223898658603 1.0
demo/demo/demo.cpp
+60 -54
View File
@@ -16,42 +16,40 @@
#include "plot.h" #include "plot.h"
#include "tracker.h" #include "tracker.h"
#define MAX_DETECT_SIZE 100 #define MAX_DETECT_SIZE 100
bool gRun; bool gRun;
void sig_handler(int signo) { void sig_handler(int signo)
std::cout<<"request gateway stop\n"; {
std::cout << "request gateway stop\n";
gRun = false; gRun = false;
} }
int main(int argc, char *argv[]) { int main(int argc, char *argv[])
{
std::cout<<"detection\n"; std::cout << "detection\n";
signal(SIGINT, sig_handler); signal(SIGINT, sig_handler);
char *net = "yolo3_coco4.rt"; char *net = "yolo3_coco4.rt";
if(argc > 1) if (argc > 1)
net = argv[1]; net = argv[1];
char *input = "../demo/demo/data/single_ped_2.mp4"; char *input = "../demo/demo/data/single_ped_2.mp4";
if(argc > 2) if (argc > 2)
input = argv[2]; input = argv[2];
char *pmatrix = "../demo/demo/data/proj_matrix_map_b.txt"; char *pmatrix = "../demo/demo/data/proj_matrix_map_b.txt";
if(argc > 3) if (argc > 3)
pmatrix = argv[3]; pmatrix = argv[3];
char *tiffile = "../demo/demo/data/map_b.tif"; char *tiffile = "../demo/demo/data/map_b.tif";
if(argc > 4) if (argc > 4)
tiffile = argv[4]; tiffile = argv[4];
/*CAMID*/ /*CAMID*/
int CAM_IDX = 0; int CAM_IDX = 0;
if(argc > 5) if (argc > 5)
CAM_IDX = atoi(argv[5]); CAM_IDX = atoi(argv[5]);
bool to_show = true; bool to_show = true;
if(argc > 6) if (argc > 6)
to_show = atoi(argv[6]); to_show = atoi(argv[6]);
tk::dnn::Yolo3Detection yolo; tk::dnn::Yolo3Detection yolo;
@@ -61,10 +59,10 @@ int main(int argc, char *argv[]) {
gRun = true; gRun = true;
cv::VideoCapture cap(input); cv::VideoCapture cap(input);
if(!cap.isOpened()) if (!cap.isOpened())
gRun = false; gRun = false;
else else
std::cout<<"camera started\n"; std::cout << "camera started\n";
cv::Mat frame; cv::Mat frame;
cv::Mat dnn_input; cv::Mat dnn_input;
@@ -73,20 +71,19 @@ int main(int argc, char *argv[]) {
/*projection matrix*/ /*projection matrix*/
int proj_matrix_read = 0; int proj_matrix_read = 0;
cv::Mat H(cv::Size(3,3),CV_64FC1); cv::Mat H(cv::Size(3, 3), CV_64FC1);
/*GPS information*/ /*GPS information*/
double *adfGeoTransform = (double*)malloc(6*sizeof(double)); double *adfGeoTransform = (double *)malloc(6 * sizeof(double));
readTiff(tiffile, adfGeoTransform); readTiff(tiffile, adfGeoTransform);
/*socket*/ /*socket*/
int sock; int sock;
int socket_opened = 0; int socket_opened = 0;
/*Conversion for tracker, from gps to meters and viceversa*/ /*Conversion for tracker, from gps to meters and viceversa*/
geodetic_converter::GeodeticConverter gc; geodetic_converter::GeodeticConverter gc;
gc.initialiseReference(44.655540,10.934315, 0); gc.initialiseReference(44.655540, 10.934315, 0);
double east, north, up; double east, north, up;
double lat, lon, alt; double lat, lon, alt;
@@ -94,24 +91,23 @@ int main(int argc, char *argv[]) {
std::vector<Tracker> trackers; std::vector<Tracker> trackers;
std::vector<Data> cur_frame; std::vector<Data> cur_frame;
int initial_age = -5; int initial_age = -5;
int age_threshold = -10; int age_threshold = -20;
int n_states = 5; int n_states = 5;
float dt = 0.03; float dt = 0.03;
struct obj_coords *coords = (struct obj_coords*)malloc(MAX_DETECT_SIZE*sizeof(struct obj_coords)); struct obj_coords *coords = (struct obj_coords *)malloc(MAX_DETECT_SIZE * sizeof(struct obj_coords));
int frame_nbr = 0; int frame_nbr = 0;
while(gRun) { while (gRun)
{
cap >> frame; cap >> frame;
if(!frame.data) { if (!frame.data)
{
usleep(1000000); usleep(1000000);
cap.open(input); cap.open(input);
printf("cap reinitialize\n"); printf("cap reinitialize\n");
continue; continue;
} }
// this will be resized to the net format // this will be resized to the net format
@@ -125,17 +121,18 @@ int main(int argc, char *argv[]) {
if (num_detected > MAX_DETECT_SIZE) if (num_detected > MAX_DETECT_SIZE)
num_detected = MAX_DETECT_SIZE; num_detected = MAX_DETECT_SIZE;
if(proj_matrix_read == 0) if (proj_matrix_read == 0)
{
read_projection_matrix(H, proj_matrix_read, pmatrix); read_projection_matrix(H, proj_matrix_read, pmatrix);
}
/*printf("%f %f %f \n%f %f %f\n %f %f %f\n\n", proj_matrix[0],proj_matrix[1], /*printf("%f %f %f \n%f %f %f\n %f %f %f\n\n", proj_matrix[0],proj_matrix[1],
proj_matrix[2],proj_matrix[3],proj_matrix[4],proj_matrix[5], proj_matrix[2],proj_matrix[3],proj_matrix[4],proj_matrix[5],
proj_matrix[6],proj_matrix[7],proj_matrix[8]);*/ proj_matrix[6],proj_matrix[7],proj_matrix[8]);*/
// draw dets // draw dets
for(int i=0; i<num_detected; i++) { for (int i = 0; i < num_detected; i++)
{
tk::dnn::box b = yolo.detected[i]; tk::dnn::box b = yolo.detected[i];
int x0 = b.x; int x0 = b.x;
@@ -144,11 +141,10 @@ int main(int argc, char *argv[]) {
int y1 = b.y + b.h; int y1 = b.y + b.h;
int obj_class = b.cl; int obj_class = b.cl;
if (obj_class == 0 /*person*/ || obj_class == 1 /*bicycle*/ || obj_class == 2 /*car*/
if(obj_class == 0 /*person*/ || obj_class == 1/*bicycle*/ || obj_class == 2/*car*/ || obj_class == 3 /*motorbike*/ || obj_class == 5 /*bus*/)
|| obj_class == 3/*motorbike*/ || obj_class == 5/*bus*/)
{ {
convert_coords(coords, coord_i,x0+b.w/2, y1,obj_class, H, adfGeoTransform, frame_nbr); convert_coords(coords, coord_i, x0 + b.w / 2, y1, obj_class, H, adfGeoTransform, frame_nbr);
coord_i++; coord_i++;
} }
@@ -158,19 +154,20 @@ int main(int argc, char *argv[]) {
cv::rectangle(frame, cv::Point(x0, y0), cv::Point(x1, y1), yolo.colors[obj_class], 2); cv::rectangle(frame, cv::Point(x0, y0), cv::Point(x1, y1), yolo.colors[obj_class], 2);
} }
cur_frame.clear(); cur_frame.clear();
for(int i=0; i<coord_i; i++) for (int i = 0; i < coord_i; i++)
{ {
std::cout<<"lat orig: "<<coords[i].LAT<<" lon orig: "<<coords[i].LONG<<std::endl; //std::cout << "lat orig: " << coords[i].LAT << " lon orig: " << coords[i].LONG << std::endl;
gc.geodetic2Enu(coords[i].LAT, coords[i].LONG, 0, &east, &north, &up); gc.geodetic2Enu(coords[i].LAT, coords[i].LONG, 0, &east, &north, &up);
std::cout<<"east: "<<east<<" north: "<<north<<std::endl; //std::cout << "east: " << east << " north: " << north << std::endl;
cur_frame.push_back(Data(east,north, frame_nbr)); cur_frame.push_back(Data(east, north, frame_nbr));
} }
if(frame_nbr == 0)
if (frame_nbr == 0)
{ {
for(auto f:cur_frame) for (auto f : cur_frame)
trackers.push_back(Tracker(f, initial_age, dt, n_states)); trackers.push_back(Tracker(f, initial_age, dt, n_states));
} }
else else
@@ -178,18 +175,29 @@ int main(int argc, char *argv[]) {
Track(cur_frame, dt, n_states, initial_age, age_threshold, trackers); Track(cur_frame, dt, n_states, initial_age, age_threshold, trackers);
} }
std::cout<<"There are "<<trackers.size()<<" trackers"<<std::endl; //std::cout << "There are " << trackers.size() << " trackers" << std::endl;
for(auto t:trackers) for (auto t : trackers)
{ {
State s = t.ekf_.getEstimatedState(); for(auto pred_pos: t.pred_list_ )
gc.enu2Geodetic(s.x_, s.y_, 0, &lat, &lon, &alt); {
std::cout<<"lat: "<<lat<<" lon: "<<lon<<std::endl;
gc.enu2Geodetic(pred_pos.x_, pred_pos.y_, 0, &lat, &lon, &alt);
//std::cout << "lat: " << lat << " lon: " << lon << std::endl;
int pix_x, pix_y; int pix_x, pix_y;
coord2pixel(lat, lon, pix_x, pix_y, adfGeoTransform); coord2pixel(lat, lon, pix_x, pix_y, adfGeoTransform);
std::cout<<"pix_x: "<<pix_x<<" pix_y: "<<pix_y<<std::endl; //std::cout << "pix_x: " << pix_x << " pix_y: " << pix_y << std::endl;
//if(t.age_ > 0)
cv::circle( frame, cv::Point( pix_x, pix_y ), 10.0, cv::Scalar( 255, 0, 0 ), 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, cv::Point(camera_p[0].x, camera_p[0].y), 3.0, cv::Scalar(255, 0, 0), CV_FILLED, 8, 0);
}
} }
frame_nbr++; frame_nbr++;
@@ -203,15 +211,13 @@ int main(int argc, char *argv[]) {
} }
} }
/* for (size_t i = 0; i < trackers.size(); i++)
/* for (size_t i = 0; i < trackers.size(); i++)
if (trackers[i].z_list_.size() > 10) if (trackers[i].z_list_.size() > 10)
plotTruthvsPred(trackers[i].z_list_, trackers[i].pred_list_); */ plotTruthvsPred(trackers[i].z_list_, trackers[i].pred_list_); */
free(coords); free(coords);
free(adfGeoTransform); free(adfGeoTransform);
std::cout<<"detection end\n"; std::cout << "detection end\n";
return 0; return 0;
} }
demo/server/server_less_dummy.cpp
+1 -1
View File
@@ -212,7 +212,7 @@ void *connection_handler(void *socket_desc)
int sock = *(int *)socket_desc; int sock = *(int *)socket_desc;
int read_size; int read_size;
void *client_message = (void*)malloc(message_size); void *client_message = (void *)malloc(message_size);
/* //Send some messages to the client /* //Send some messages to the client
message = "Greetings! I am your connection handler\n"; message = "Greetings! I am your connection handler\n";
include/send.h
+27 -36
View File
@@ -17,8 +17,6 @@
#include "gdal/gdal_priv.h" #include "gdal/gdal_priv.h"
#include "gdal/cpl_conv.h" #include "gdal/cpl_conv.h"
#include "serialize.hpp" #include "serialize.hpp"
struct obj_coords struct obj_coords
@@ -28,17 +26,16 @@ struct obj_coords
float cl; float cl;
}; };
void readTiff(char *filename, double *adfGeoTransform)
void readTiff(char*filename, double *adfGeoTransform)
{ {
GDALDataset *poDataset; GDALDataset *poDataset;
GDALAllRegister(); GDALAllRegister();
poDataset = (GDALDataset *) GDALOpen( filename, GA_ReadOnly ); poDataset = (GDALDataset *)GDALOpen(filename, GA_ReadOnly);
if( poDataset != NULL ) if (poDataset != NULL)
{ {
//int colms = poDataset->GetRasterXSize(); //int colms = poDataset->GetRasterXSize();
//int rows = poDataset->GetRasterYSize(); //int rows = poDataset->GetRasterYSize();
poDataset->GetGeoTransform( adfGeoTransform ); poDataset->GetGeoTransform(adfGeoTransform);
} }
} }
@@ -57,27 +54,25 @@ void pixel2coord(int x, int y, double &lat, double &lon, double *adfGeoTransform
lon = a * x + b * y + xoff; lon = a * x + b * y + xoff;
lat = d * x + e * y + yoff; lat = d * x + e * y + yoff;
} }
void coord2pixel(double lat, double lon, int &x, int &y, double *adfGeoTransform) void coord2pixel(double lat, double lon, int &x, int &y, double *adfGeoTransform)
{ {
x = int(round((lon-adfGeoTransform[0])/adfGeoTransform[1])); x = int(round((lon - adfGeoTransform[0]) / adfGeoTransform[1]));
y = int(round((lat-adfGeoTransform[3])/adfGeoTransform[5])); y = int(round((lat - adfGeoTransform[3]) / adfGeoTransform[5]));
} }
void fillMatrix(cv::Mat &H, float *matrix, bool show = false)
void fillMatrix(cv::Mat &H, float *matrix, bool show=false)
{ {
double *vals = (double*) H.data; double *vals = (double *)H.data;
for(int i=0; i<9; i++) { for (int i = 0; i < 9; i++)
{
vals[i] = matrix[i]; vals[i] = matrix[i];
} }
if(show) if (show)
std::cout<<H<<"\n"; std::cout << H << "\n";
} }
void serialize_coords(struct obj_coords *c, int obj_n, int CAM_IDX, std::stringbuf *buf)
void serialize_coords(struct obj_coords *c, int obj_n, int CAM_IDX, std::stringbuf* buf)
{ {
std::ostream os(buf); std::ostream os(buf);
@@ -92,14 +87,13 @@ void serialize_coords(struct obj_coords *c, int obj_n, int CAM_IDX, std::stringb
int i; int i;
for (i = 0; i < obj_n; i++) for (i = 0; i < obj_n; i++)
{ {
Road_User r{c[i].LAT,c[i].LONG,0,0,(int)c[i].cl}; Road_User r{c[i].LAT, c[i].LONG, 0, 0, (int)c[i].cl};
ruv.push_back(r); ruv.push_back(r);
} }
Message m{CAM_IDX,t_stamp_ms,ruv.size(),ruv}; Message m{CAM_IDX, t_stamp_ms, ruv.size(), ruv};
archive(m); archive(m);
//std::cout<<buf->str()<<std::endl; //std::cout<<buf->str()<<std::endl;
//return buf; //return buf;
@@ -153,22 +147,22 @@ int map_class_coco_to_voc(int coco_class)
FILE *out_file = fopen("prova_pixel.txt", "w"); FILE *out_file = fopen("prova_pixel.txt", "w");
void convert_coords(struct obj_coords *coords, int i, int x, int y, int detected_class,cv::Mat H, double *adfGeoTransform, int frame_nbr) void convert_coords(struct obj_coords *coords, int i, int x, int y, int detected_class, cv::Mat H, double *adfGeoTransform, int frame_nbr)
{ {
double latitude, longitude; double latitude, longitude;
std::vector<cv::Point2f> x_y, ll; std::vector<cv::Point2f> x_y, ll;
x_y.push_back(cv::Point2f(x, y)); x_y.push_back(cv::Point2f(x, y));
//transform camera pixel to map pixel //transform camera pixel to map pixel
cv::perspectiveTransform( x_y, ll, H); cv::perspectiveTransform(x_y, ll, H);
//tranform to map pixel to map gps //tranform to map pixel to map gps
pixel2coord(ll[0].x, ll[0].y, latitude,longitude, adfGeoTransform); pixel2coord(ll[0].x, ll[0].y, latitude, longitude, adfGeoTransform);
//printf("lat: %f, long:%f \n", latitude, longitude); //printf("lat: %f, long:%f \n", latitude, longitude);
coords[i].LAT = latitude; coords[i].LAT = latitude;
coords[i].LONG = longitude; coords[i].LONG = longitude;
coords[i].cl = map_class_coco_to_voc(detected_class); coords[i].cl = map_class_coco_to_voc(detected_class);
if(detected_class == 0) if (detected_class == 0)
{ {
struct timeval tv; struct timeval tv;
@@ -176,20 +170,18 @@ void convert_coords(struct obj_coords *coords, int i, int x, int y, int detected
unsigned long long t_stamp_ms = (unsigned long long)(tv.tv_sec) * 1000 + (unsigned long long)(tv.tv_usec) / 1000; unsigned long long t_stamp_ms = (unsigned long long)(tv.tv_sec) * 1000 + (unsigned long long)(tv.tv_usec) / 1000;
//fprintf(out_file, "%d %lld %d %d\n",frame_nbr, t_stamp_ms, int(ll[0].x), int(ll[0].y)); //fprintf(out_file, "%d %lld %d %d\n",frame_nbr, t_stamp_ms, int(ll[0].x), int(ll[0].y));
fprintf(out_file, "%d %lld %f %f\n",frame_nbr, t_stamp_ms, coords[i].LAT, coords[i].LONG); fprintf(out_file, "%d %lld %f %f\n", frame_nbr, t_stamp_ms, coords[i].LAT, coords[i].LONG);
} }
} }
void read_projection_matrix(cv::Mat &H, int &proj_matrix_read, char* path) void read_projection_matrix(cv::Mat &H, int &proj_matrix_read, char *path)
{ {
FILE *fp; FILE *fp;
char *line = NULL; char *line = NULL;
size_t len = 0; size_t len = 0;
ssize_t read; ssize_t read;
float* proj_matrix = (float*) malloc(9*sizeof(float)); float *proj_matrix = (float *)malloc(9 * sizeof(float));
fp = fopen(path, "r"); fp = fopen(path, "r");
if (fp == NULL) if (fp == NULL)
@@ -198,7 +190,7 @@ void read_projection_matrix(cv::Mat &H, int &proj_matrix_read, char* path)
int i = 0; int i = 0;
while ((read = getline(&line, &len, fp)) != -1) while ((read = getline(&line, &len, fp)) != -1)
{ {
if (3 == sscanf(line, "%f %f %f", &proj_matrix[i*3+0], &proj_matrix[i*3+1], &proj_matrix[i*3+2])) if (3 == sscanf(line, "%f %f %f", &proj_matrix[i * 3 + 0], &proj_matrix[i * 3 + 1], &proj_matrix[i * 3 + 2]))
{ {
i++; i++;
proj_matrix_read = 1; proj_matrix_read = 1;
@@ -208,6 +200,8 @@ void read_projection_matrix(cv::Mat &H, int &proj_matrix_read, char* path)
fclose(fp); fclose(fp);
fillMatrix(H, proj_matrix); fillMatrix(H, proj_matrix);
free(proj_matrix); free(proj_matrix);
} }
@@ -243,14 +237,13 @@ int send_client_dummy(struct obj_coords *coords, int n_coords, int &sock, int &s
std::stringbuf *message = new std::stringbuf(); std::stringbuf *message = new std::stringbuf();
serialize_coords(coords, n_coords, CAM_IDX, message); serialize_coords(coords, n_coords, CAM_IDX, message);
//std::cout<<message->str().length()<<std::endl; //std::cout<<message->str().length()<<std::endl;
/*open socket if not already opened*/ /*open socket if not already opened*/
if (socket_opened == 0) if (socket_opened == 0)
{ {
int res = open_socket("127.0.0.1",sock, socket_opened); int res = open_socket("127.0.0.1", sock, socket_opened);
if(res) if (res)
printf("Socket opened!\n"); printf("Socket opened!\n");
else else
{ {
@@ -259,8 +252,6 @@ int send_client_dummy(struct obj_coords *coords, int n_coords, int &sock, int &s
} }
} }
/*send message to server*/ /*send message to server*/
if (send(sock, message->str().data(), message->str().length(), 0) < 0) if (send(sock, message->str().data(), message->str().length(), 0) < 0)
{ {
tracker_CLASS
+1 -1
Submodule tracker_CLASS updated: 135e47ba2e...29d9cdf1bd