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 <pthread.h>
#include <yaml-cpp/yaml.h>
#include "utils.h"
#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[])
{
@@ -166,33 +108,21 @@ int main(int argc, char *argv[])
}
/*projection matrix from camera to map*/
int proj_matrix_read = 0;
cv::Mat H(cv::Size(3, 3), CV_64FC1);
read_projection_matrix(H, pmatrix);
/*Camera calibration*/
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);
cv::Mat cameraMat, distCoeff;
readCameraCalibrationYaml(cameraCalib, cameraMat, distCoeff);
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;
/*GPS information*/
double *adfGeoTransform = (double *)malloc(6 * sizeof(double));
readTiff(tiffile, adfGeoTransform);
struct obj_coords *coords = (struct obj_coords *)malloc(MAX_DETECT_SIZE * sizeof(struct obj_coords));
std::vector<ObjCoords> coords;
/*socket*/
Communicator Comm(SOCK_DGRAM);
Comm.open_client_socket("127.0.0.1", 8888);
Message *m = new Message;
@@ -204,8 +134,7 @@ int main(int argc, char *argv[])
double lat, lon, alt;
/*Mask info*/
cv::Mat mask;
mask = cv::imread(maskfile, cv::IMREAD_GRAYSCALE);
cv::Mat mask = cv::imread(maskfile, cv::IMREAD_GRAYSCALE);
/*tracker infos*/
srand(time(NULL));
@@ -217,6 +146,7 @@ int main(int argc, char *argv[])
float dt = 0.03;
int frame_nbr = 0;
while (gRun)
{
@@ -239,14 +169,11 @@ int main(int argc, char *argv[])
// TODO: async infer
yolo.update(dnn_input);
int coord_i = 0;
int num_detected = yolo.detected.size();
if (num_detected > MAX_DETECT_SIZE)
num_detected = MAX_DETECT_SIZE;
if (proj_matrix_read == 0)
read_projection_matrix(H, proj_matrix_read, pmatrix);
coords.clear();
// draw dets
for (int i = 0; i < num_detected; i++)
@@ -268,8 +195,7 @@ int main(int argc, char *argv[])
if (objClass < 6)
{
convert_coords(coords, coord_i, x0 + b.w / 2, y1, objClass, H, adfGeoTransform, frame_nbr);
coord_i++;
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);
@@ -284,16 +210,13 @@ int main(int argc, char *argv[])
}
}
TIMER_START
//convert from latitude and longitude to meters for ekf
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));
}
if (frame_nbr == 0)
{
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);
}
TIMER_STOP
std::cout << "There are " << trackers.size() << " trackers" << std::endl;
if (to_show)
@@ -364,18 +286,14 @@ int main(int argc, char *argv[])
}
frame_nbr++;
prepare_message(m,coords, coord_i,CAM_IDX);
prepare_message(m, coords, CAM_IDX);
std::stringbuf s;
Comm.serialize_coords(m,&s);
Comm.serialize_coords(m, &s);
//std::cout<<s.str()<<std::endl;
//std::cout<<s.str().length()<<std::endl;
Comm.send_message(m);
}
free(coords);
free(adfGeoTransform);
std::cout << "detection end\n";
include/classutils.h
+96 -19
View File
@@ -17,12 +17,16 @@
#include "gdal/gdal_priv.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 LONG;
float cl;
double lat_;
double long_;
int class_;
};
void readTiff(char *filename, double *adfGeoTransform)
@@ -32,12 +36,31 @@ void readTiff(char *filename, double *adfGeoTransform)
poDataset = (GDALDataset *)GDALOpen(filename, GA_ReadOnly);
if (poDataset != NULL)
{
//int colms = poDataset->GetRasterXSize();
//int rows = poDataset->GetRasterYSize();
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)
{
//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";
}
//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;
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
pixel2coord(ll[0].x, ll[0].y, latitude, longitude, adfGeoTransform);
//printf("lat: %f, long:%f \n", latitude, longitude);
coords[i].LAT = latitude;
coords[i].LONG = longitude;
coords[i].cl = detected_class;
ObjCoords coord;
coord.lat_ = latitude;
coord.long_ = longitude;
coord.class_ = detected_class;
coords.push_back(coord);
/*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;
//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);
//printf( "%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, 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;
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]))
{
i++;
proj_matrix_read = 1;
}
}
free(line);
fclose(fp);
fillMatrix(H, 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*/