Davide Sapienza
b1a3620061
This commit splits some operations into different threads. Some threads compute the visualization preprocessing for the live, detection, top view and disparity visualization. Only one thread has the role to display the different views.
283 lines
8.5 KiB
C++
283 lines
8.5 KiB
C++
#ifndef CLASSUTILS_H
|
|
#define CLASSUTILS_H
|
|
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
#include <sys/time.h>
|
|
#include <sys/socket.h> //socket
|
|
#include <arpa/inet.h> //inet_addr
|
|
#include <unistd.h> //write
|
|
|
|
#include <opencv2/calib3d.hpp>
|
|
#include <opencv2/core.hpp>
|
|
#include "gdal.h"
|
|
#include <gdal_priv.h>
|
|
#include <gdal/gdal.h>
|
|
#include "gdal/gdal_priv.h"
|
|
#include "gdal/cpl_conv.h"
|
|
|
|
#include "tracker.h"
|
|
|
|
#include <yaml-cpp/yaml.h>
|
|
|
|
#include "../masa_protocol/include/send.hpp"
|
|
#include "../masa_protocol/include/serialize.hpp"
|
|
|
|
struct ObjCoords
|
|
{
|
|
double lat_;
|
|
double long_;
|
|
int class_;
|
|
};
|
|
|
|
void readTiff(char *filename, double *adfGeoTransform)
|
|
{
|
|
GDALDataset *poDataset;
|
|
GDALAllRegister();
|
|
poDataset = (GDALDataset *)GDALOpen(filename, GA_ReadOnly);
|
|
if (poDataset != NULL)
|
|
{
|
|
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
|
|
double xoff, a, b, yoff, d, e;
|
|
xoff = adfGeoTransform[0];
|
|
a = adfGeoTransform[1];
|
|
b = adfGeoTransform[2];
|
|
yoff = adfGeoTransform[3];
|
|
d = adfGeoTransform[4];
|
|
e = adfGeoTransform[5];
|
|
|
|
//printf("%f %f %f %f %f %f\n",xoff, a, b, yoff, d, e );
|
|
|
|
lon = a * x + b * y + xoff;
|
|
lat = d * x + e * y + yoff;
|
|
}
|
|
void coord2pixel(double lat, double lon, int &x, int &y, double *adfGeoTransform)
|
|
{
|
|
x = int(round((lon - adfGeoTransform[0]) / adfGeoTransform[1]));
|
|
y = int(round((lat - adfGeoTransform[3]) / adfGeoTransform[5]));
|
|
}
|
|
|
|
void fillMatrix(cv::Mat &H, float *matrix, bool show = false)
|
|
{
|
|
double *vals = (double *)H.data;
|
|
for (int i = 0; i < 9; i++)
|
|
{
|
|
vals[i] = matrix[i];
|
|
}
|
|
if (show)
|
|
std::cout << H << "\n";
|
|
}
|
|
|
|
//FILE *out_file = fopen("prova_pixel.txt", "w");
|
|
|
|
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;
|
|
x_y.push_back(cv::Point2f(x, y));
|
|
|
|
//transform camera pixel to map pixel
|
|
cv::perspectiveTransform(x_y, ll, H);
|
|
//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);
|
|
|
|
ObjCoords coord;
|
|
coord.lat_ = latitude;
|
|
coord.long_ = longitude;
|
|
coord.class_ = detected_class;
|
|
coords.push_back(coord);
|
|
|
|
/*if (detected_class == 0)
|
|
{
|
|
|
|
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;
|
|
|
|
//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, 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, char *path)
|
|
{
|
|
FILE *fp;
|
|
char *line = NULL;
|
|
size_t len = 0;
|
|
ssize_t read;
|
|
|
|
float *proj_matrix = (float *)malloc(9 * sizeof(float));
|
|
|
|
fp = fopen(path, "r");
|
|
if (fp == NULL)
|
|
exit(EXIT_FAILURE);
|
|
|
|
int i = 0;
|
|
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]))
|
|
{
|
|
i++;
|
|
}
|
|
}
|
|
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 addRoadUserfromTracker(const std::vector<Tracker> &trackers, Message *m, geodetic_converter::GeodeticConverter &gc, const cv::Mat& maskOrient, double *adfGeoTransform)
|
|
{
|
|
m->t_stamp_ms = time_in_ms();
|
|
m->num_objects = trackers.size();
|
|
m->objects.clear();
|
|
double lat, lon, alt;
|
|
|
|
for (auto t : trackers)
|
|
{
|
|
if (t.pred_list_.size() > 0)
|
|
{
|
|
Categories cat;
|
|
switch (t.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;
|
|
}
|
|
//std::cout << t.pred_list_.size() << std::endl;
|
|
gc.enu2Geodetic(t.pred_list_.back().x_, t.pred_list_.back().y_, 0, &lat, &lon, &alt);
|
|
|
|
int pix_x, pix_y;
|
|
coord2pixel(lat, lon, pix_x, pix_y, adfGeoTransform);
|
|
|
|
// std::cout<<maskOrient.at<cv::Vec3b>(pix_y,pix_x)<<std::endl;
|
|
|
|
uint8_t maskOrientPixel = maskOrient.at<cv::Vec3b>(pix_y,pix_x)[0];
|
|
uint8_t orientation;
|
|
if(maskOrientPixel != 0)
|
|
{
|
|
orientation = maskOrientPixel;
|
|
// std::cout<<"orientation given by the mask "<< int(orientation)<<std::endl;
|
|
}
|
|
else
|
|
{
|
|
orientation = uint8_t((int((t.pred_list_.back().yaw_ * 57.29 + 360)) % 360) * 17 / 24);
|
|
//std::cout<<"orientation given by the tracker "<< int(orientation)<<std::endl;
|
|
}
|
|
|
|
//std::cout << "lat: " << lat << " lon: " << lon << std::endl;
|
|
uint8_t velocity = uint8_t(std::abs(t.pred_list_.back().vel_ * 3.6 / 2));
|
|
|
|
RoadUser r{static_cast<float>(lat), static_cast<float>(lon), velocity, orientation, cat};
|
|
//std::cout << std::setprecision(10) << r.latitude << " , " << r.longitude << " " << int(r.speed) << " " << int(r.orientation) << " " << r.category << std::endl;
|
|
m->objects.push_back(r);
|
|
}
|
|
}
|
|
}
|
|
|
|
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{static_cast<float>(coords[i].lat_), static_cast<float>(coords[i].long_), 0, 1, cat};
|
|
std::cout << std::setprecision(10) << r.latitude << " , " << r.longitude << " " << cat << std::endl;
|
|
m->objects.push_back(r);
|
|
}
|
|
|
|
m->lights.clear();
|
|
}
|
|
|
|
#endif /*CLASSUTILS_H*/ |