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

order

Browse Source
This commit is contained in:
mive93
2019-05-08 20:08:20 +02:00
parent e38d8e82ca
commit a6d19d3698
2 changed files with 108 additions and 113 deletions
Download Patch File Download Diff File Expand all files Collapse all files
demo/demo/demo.cpp
+12 -94
View File
@@ -6,7 +6,6 @@
#include <ctime> #include <ctime>
#include <pthread.h> #include <pthread.h>
#include <yaml-cpp/yaml.h>
#include "utils.h" #include "utils.h"
#include <opencv2/core/core.hpp> #include <opencv2/core/core.hpp>
@@ -50,63 +49,6 @@ void *showImages(void *x_void_ptr)
} }
} }
void draw_arrow(float angleRad, float vel, cv::Scalar color, cv::Point center, cv::Mat &frame)
{
int angle = angleRad * 180.0 / CV_PI;
auto length = 10 * vel;
auto direction = cv::Point(length * cos(angleRad), length * sin(angleRad)); // calculate direction
double tipLength = .2 + 0.4 * (angle % 180) / 360;
int lineType = 8;
int thickness = 2;
cv::arrowedLine(frame, center, center + direction, color, thickness, lineType, 0, tipLength); // draw arrow!
}
unsigned long long time_in_ms()
{
struct timeval tv;
gettimeofday(&tv, NULL);
unsigned long long t_stamp_ms = (unsigned long long)(tv.tv_sec) * 1000 + (unsigned long long)(tv.tv_usec) / 1000;
return t_stamp_ms;
}
void prepare_message(Message *m, struct obj_coords *coords, int n_coords, int idx)
{
m->cam_idx = idx;
m->t_stamp_ms = time_in_ms();
m->num_objects = n_coords;
m->objects.clear();
for (int i = 0; i < n_coords ; i++)
{
Categories cat;
switch (static_cast<int>(coords[i].cl))
{
case 0:
cat = Categories::C_person;
break;
case 1:
cat = Categories::C_car;
break;
case 2:
cat = Categories::C_car;
break;
case 3:
cat = Categories::C_bus;
break;
case 4:
cat = Categories::C_motorbike;
break;
case 5:
cat = Categories::C_bycicle;
break;
}
RoadUser r{coords[i].LAT, coords[i].LONG, 0, 1, C_car};
m->objects.push_back(r);
}
m->lights.clear();
}
int main(int argc, char *argv[]) int main(int argc, char *argv[])
{ {
@@ -166,33 +108,21 @@ int main(int argc, char *argv[])
} }
/*projection matrix from camera to map*/ /*projection matrix from camera to map*/
int proj_matrix_read = 0;
cv::Mat H(cv::Size(3, 3), CV_64FC1); cv::Mat H(cv::Size(3, 3), CV_64FC1);
read_projection_matrix(H, pmatrix);
/*Camera calibration*/ /*Camera calibration*/
YAML::Node config = YAML::LoadFile(cameraCalib); cv::Mat cameraMat, distCoeff;
const YAML::Node &node_test1 = config["camera_matrix"]; readCameraCalibrationYaml(cameraCalib, cameraMat, distCoeff);
float data_cm[9];
for (std::size_t i = 0; i < node_test1["data"].size(); i++)
data_cm[i] = node_test1["data"][i].as<float>();
cv::Mat cameraMat = cv::Mat(3, 3, CV_32F, data_cm);
std::cout << cameraMat << std::endl; std::cout << cameraMat << std::endl;
const YAML::Node &node_test2 = config["distortion_coefficients"];
float data_dc[5];
for (std::size_t i = 0; i < node_test2["data"].size(); i++)
data_dc[i] = node_test2["data"][i].as<float>();
cv::Mat distCoeff = cv::Mat(5, 1, CV_32F, data_dc);
std::cout << distCoeff << std::endl; std::cout << distCoeff << std::endl;
/*GPS information*/ /*GPS information*/
double *adfGeoTransform = (double *)malloc(6 * sizeof(double)); double *adfGeoTransform = (double *)malloc(6 * sizeof(double));
readTiff(tiffile, adfGeoTransform); readTiff(tiffile, adfGeoTransform);
struct obj_coords *coords = (struct obj_coords *)malloc(MAX_DETECT_SIZE * sizeof(struct obj_coords)); std::vector<ObjCoords> coords;
/*socket*/ /*socket*/
Communicator Comm(SOCK_DGRAM); Communicator Comm(SOCK_DGRAM);
Comm.open_client_socket("127.0.0.1", 8888); Comm.open_client_socket("127.0.0.1", 8888);
Message *m = new Message; Message *m = new Message;
@@ -204,8 +134,7 @@ int main(int argc, char *argv[])
double lat, lon, alt; double lat, lon, alt;
/*Mask info*/ /*Mask info*/
cv::Mat mask; cv::Mat mask = cv::imread(maskfile, cv::IMREAD_GRAYSCALE);
mask = cv::imread(maskfile, cv::IMREAD_GRAYSCALE);
/*tracker infos*/ /*tracker infos*/
srand(time(NULL)); srand(time(NULL));
@@ -217,6 +146,7 @@ int main(int argc, char *argv[])
float dt = 0.03; float dt = 0.03;
int frame_nbr = 0; int frame_nbr = 0;
while (gRun) while (gRun)
{ {
@@ -239,14 +169,11 @@ int main(int argc, char *argv[])
// TODO: async infer // TODO: async infer
yolo.update(dnn_input); yolo.update(dnn_input);
int coord_i = 0;
int num_detected = yolo.detected.size(); int num_detected = yolo.detected.size();
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) coords.clear();
read_projection_matrix(H, proj_matrix_read, pmatrix);
// draw dets // draw dets
for (int i = 0; i < num_detected; i++) for (int i = 0; i < num_detected; i++)
@@ -268,8 +195,7 @@ int main(int argc, char *argv[])
if (objClass < 6) if (objClass < 6)
{ {
convert_coords(coords, coord_i, x0 + b.w / 2, y1, objClass, H, adfGeoTransform, frame_nbr); convert_coords(coords, x0 + b.w / 2, y1, objClass, H, adfGeoTransform, frame_nbr);
coord_i++;
//std::cout<<objClass<<" ("<<prob<<"): "<<x0<<" "<<y0<<" "<<x1<<" "<<y1<<"\n"; //std::cout<<objClass<<" ("<<prob<<"): "<<x0<<" "<<y0<<" "<<x1<<" "<<y1<<"\n";
cv::rectangle(frame, cv::Point(x0, y0), cv::Point(x1, y1), yolo.colors[objClass], 2); cv::rectangle(frame, cv::Point(x0, y0), cv::Point(x1, y1), yolo.colors[objClass], 2);
@@ -284,16 +210,13 @@ int main(int argc, char *argv[])
} }
} }
TIMER_START
//convert from latitude and longitude to meters for ekf //convert from latitude and longitude to meters for ekf
cur_frame.clear(); cur_frame.clear();
for (int i = 0; i < coord_i; i++) for (size_t i = 0; i < coords.size(); i++)
{ {
gc.geodetic2Enu(coords[i].LAT, coords[i].LONG, 0, &east, &north, &up); gc.geodetic2Enu(coords[i].lat_, coords[i].long_, 0, &east, &north, &up);
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)
@@ -304,7 +227,6 @@ 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);
} }
TIMER_STOP
std::cout << "There are " << trackers.size() << " trackers" << std::endl; std::cout << "There are " << trackers.size() << " trackers" << std::endl;
if (to_show) if (to_show)
@@ -364,18 +286,14 @@ int main(int argc, char *argv[])
} }
frame_nbr++; frame_nbr++;
prepare_message(m,coords, coord_i,CAM_IDX); prepare_message(m, coords, CAM_IDX);
std::stringbuf s; std::stringbuf s;
Comm.serialize_coords(m,&s); Comm.serialize_coords(m, &s);
//std::cout<<s.str()<<std::endl; //std::cout<<s.str()<<std::endl;
//std::cout<<s.str().length()<<std::endl; //std::cout<<s.str().length()<<std::endl;
Comm.send_message(m); Comm.send_message(m);
} }
free(coords);
free(adfGeoTransform); free(adfGeoTransform);
std::cout << "detection end\n"; std::cout << "detection end\n";
include/classutils.h
+96 -19
View File
@@ -17,12 +17,16 @@
#include "gdal/gdal_priv.h" #include "gdal/gdal_priv.h"
#include "gdal/cpl_conv.h" #include "gdal/cpl_conv.h"
#include <yaml-cpp/yaml.h>
struct obj_coords #include "../masa_protocol/include/send.hpp"
#include "../masa_protocol/include/serialize.hpp"
struct ObjCoords
{ {
double LAT; double lat_;
double LONG; double long_;
float cl; int class_;
}; };
void readTiff(char *filename, double *adfGeoTransform) void readTiff(char *filename, double *adfGeoTransform)
@@ -32,12 +36,31 @@ void readTiff(char *filename, double *adfGeoTransform)
poDataset = (GDALDataset *)GDALOpen(filename, GA_ReadOnly); poDataset = (GDALDataset *)GDALOpen(filename, GA_ReadOnly);
if (poDataset != NULL) if (poDataset != NULL)
{ {
//int colms = poDataset->GetRasterXSize();
//int rows = poDataset->GetRasterYSize();
poDataset->GetGeoTransform(adfGeoTransform); poDataset->GetGeoTransform(adfGeoTransform);
} }
} }
void readCameraCalibrationYaml(const std::string &cameraCalib, cv::Mat &cameraMat, cv::Mat &distCoeff)
{
YAML::Node config = YAML::LoadFile(cameraCalib);
const YAML::Node &node_test1 = config["camera_matrix"];
float data_cm[9];
for (std::size_t i = 0; i < node_test1["data"].size(); i++)
data_cm[i] = node_test1["data"][i].as<float>();
cv::Mat cameraMat_ = cv::Mat(3, 3, CV_32F, data_cm);
cameraMat = cameraMat_.clone();
std::cout << cameraMat << std::endl;
const YAML::Node &node_test2 = config["distortion_coefficients"];
float data_dc[5];
for (std::size_t i = 0; i < node_test2["data"].size(); i++)
data_dc[i] = node_test2["data"][i].as<float>();
cv::Mat distCoeff_ = cv::Mat(5, 1, CV_32F, data_dc);
distCoeff = distCoeff_.clone();
std::cout << distCoeff << std::endl;
}
void pixel2coord(int x, int y, double &lat, double &lon, double *adfGeoTransform) void pixel2coord(int x, int y, double &lat, double &lon, double *adfGeoTransform)
{ {
//Returns global coordinates from pixel x, y coordinates //Returns global coordinates from pixel x, y coordinates
@@ -71,12 +94,9 @@ void fillMatrix(cv::Mat &H, float *matrix, bool show = false)
std::cout << H << "\n"; std::cout << H << "\n";
} }
//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(std::vector<ObjCoords> &coords, 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;
@@ -87,9 +107,12 @@ void convert_coords(struct obj_coords *coords, int i, int x, int y, int detected
//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].LONG = longitude; ObjCoords coord;
coords[i].cl = detected_class; coord.lat_ = latitude;
coord.long_ = longitude;
coord.class_ = detected_class;
coords.push_back(coord);
/*if (detected_class == 0) /*if (detected_class == 0)
{ {
@@ -99,12 +122,12 @@ 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;
//printf(out_file, "%d %lld %d %d\n",frame_nbr, t_stamp_ms, int(ll[0].x), int(ll[0].y)); //printf(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, coord.LAT, coord.LONG);
//printf( "%d %lld %f %f\n", frame_nbr, t_stamp_ms, coords[i].LAT, coords[i].LONG); //printf( "%d %lld %f %f\n", frame_nbr, t_stamp_ms, coord.LAT, coord.LONG);
}*/ }*/
} }
void read_projection_matrix(cv::Mat &H, int &proj_matrix_read, char *path) void read_projection_matrix(cv::Mat &H, char *path)
{ {
FILE *fp; FILE *fp;
char *line = NULL; char *line = NULL;
@@ -123,16 +146,70 @@ void read_projection_matrix(cv::Mat &H, int &proj_matrix_read, char *path)
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;
} }
} }
free(line); free(line);
fclose(fp); fclose(fp);
fillMatrix(H, proj_matrix); fillMatrix(H, proj_matrix);
free(proj_matrix); free(proj_matrix);
} }
void draw_arrow(float angleRad, float vel, cv::Scalar color, cv::Point center, cv::Mat &frame)
{
int angle = angleRad * 180.0 / CV_PI;
auto length = 10 * vel;
auto direction = cv::Point(length * cos(angleRad), length * sin(angleRad)); // calculate direction
double tipLength = .2 + 0.4 * (angle % 180) / 360;
int lineType = 8;
int thickness = 2;
cv::arrowedLine(frame, center, center + direction, color, thickness, lineType, 0, tipLength); // draw arrow!
}
unsigned long long time_in_ms()
{
struct timeval tv;
gettimeofday(&tv, NULL);
unsigned long long t_stamp_ms = (unsigned long long)(tv.tv_sec) * 1000 + (unsigned long long)(tv.tv_usec) / 1000;
return t_stamp_ms;
}
void prepare_message(Message *m, const std::vector<ObjCoords>& coords, int idx)
{
m->cam_idx = idx;
m->t_stamp_ms = time_in_ms();
m->num_objects = coords.size();
m->objects.clear();
for (int i = 0; i < coords.size(); i++)
{
Categories cat;
switch (coords[i].class_)
{
case 0:
cat = Categories::C_person;
break;
case 1:
cat = Categories::C_car;
break;
case 2:
cat = Categories::C_car;
break;
case 3:
cat = Categories::C_bus;
break;
case 4:
cat = Categories::C_motorbike;
break;
case 5:
cat = Categories::C_bycicle;
break;
}
RoadUser r{coords[i].lat_, coords[i].long_, 0, 1, C_car};
m->objects.push_back(r);
}
m->lights.clear();
}
#endif /*CLASSUTILS_H*/ #endif /*CLASSUTILS_H*/