new send and submodule masa_protocol added
This commit is contained in:
@@ -1,3 +1,6 @@
|
|||||||
[submodule "tracker_CLASS"]
|
[submodule "tracker_CLASS"]
|
||||||
path = tracker_CLASS
|
path = tracker_CLASS
|
||||||
url = https://github.com/mive93/tracker_CLASS.git
|
url = https://github.com/mive93/tracker_CLASS.git
|
||||||
|
[submodule "masa_protocol"]
|
||||||
|
path = masa_protocol
|
||||||
|
url = https://git.hipert.unimore.it/rcavicchioli/masa_protocol.git
|
||||||
|
|||||||
@@ -107,8 +107,6 @@ add_executable(yolo3_demo demo/demo/demo.cpp
|
|||||||
target_link_libraries(yolo3_demo tkDNN)
|
target_link_libraries(yolo3_demo tkDNN)
|
||||||
target_link_libraries(yolo3_demo python2.7 yaml-cpp)
|
target_link_libraries(yolo3_demo python2.7 yaml-cpp)
|
||||||
|
|
||||||
add_executable(class_server demo/server/server_less_dummy.cpp)
|
|
||||||
target_link_libraries(class_server pthread tkDNN )
|
|
||||||
|
|
||||||
|
|
||||||
#install
|
#install
|
||||||
|
|||||||
+64
-5
@@ -14,7 +14,10 @@
|
|||||||
#include <opencv2/imgproc/imgproc.hpp>
|
#include <opencv2/imgproc/imgproc.hpp>
|
||||||
|
|
||||||
#include "Yolo3Detection.h"
|
#include "Yolo3Detection.h"
|
||||||
#include "send.h"
|
#include "classutils.h"
|
||||||
|
#include "../masa_protocol/include/send.hpp"
|
||||||
|
#include "../masa_protocol/include/serialize.hpp"
|
||||||
|
|
||||||
#include "ekf.h"
|
#include "ekf.h"
|
||||||
#include "trackutils.h"
|
#include "trackutils.h"
|
||||||
#include "plot.h"
|
#include "plot.h"
|
||||||
@@ -58,6 +61,52 @@ void draw_arrow(float angleRad, float vel, cv::Scalar color, cv::Point center, c
|
|||||||
cv::arrowedLine(frame, center, center + direction, color, thickness, lineType, 0, tipLength); // draw arrow!
|
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[])
|
||||||
{
|
{
|
||||||
|
|
||||||
@@ -76,7 +125,7 @@ int main(int argc, char *argv[])
|
|||||||
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];
|
||||||
int CAM_IDX = 0;
|
int CAM_IDX = 20936;
|
||||||
if (argc > 5)
|
if (argc > 5)
|
||||||
CAM_IDX = atoi(argv[5]);
|
CAM_IDX = atoi(argv[5]);
|
||||||
bool to_show = true;
|
bool to_show = true;
|
||||||
@@ -143,8 +192,10 @@ int main(int argc, char *argv[])
|
|||||||
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));
|
||||||
|
|
||||||
/*socket*/
|
/*socket*/
|
||||||
int sock;
|
|
||||||
int socket_opened = 0;
|
Communicator Comm(SOCK_DGRAM);
|
||||||
|
Comm.open_client_socket("127.0.0.1", 8888);
|
||||||
|
Message *m = new Message;
|
||||||
|
|
||||||
/*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;
|
||||||
@@ -313,7 +364,15 @@ int main(int argc, char *argv[])
|
|||||||
}
|
}
|
||||||
|
|
||||||
frame_nbr++;
|
frame_nbr++;
|
||||||
send_client_dummy(coords, coord_i, sock, socket_opened, CAM_IDX);
|
prepare_message(m,coords, coord_i,CAM_IDX);
|
||||||
|
std::stringbuf 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(coords);
|
||||||
|
|||||||
@@ -1,283 +0,0 @@
|
|||||||
/*
|
|
||||||
C socket server example, handles multiple clients using threads
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include <stdio.h>
|
|
||||||
#include <string.h> //strlen
|
|
||||||
#include <stdlib.h> //strlen
|
|
||||||
#include <time.h>
|
|
||||||
#include <sys/socket.h>
|
|
||||||
#include <arpa/inet.h> //inet_addr
|
|
||||||
#include <unistd.h> //write
|
|
||||||
#include <pthread.h> //for threading , link with lpthread
|
|
||||||
#include <semaphore.h>
|
|
||||||
#include <sys/types.h>
|
|
||||||
#include <sys/stat.h>
|
|
||||||
#include "serialize.hpp"
|
|
||||||
#include <string>
|
|
||||||
|
|
||||||
sem_t semaphore;
|
|
||||||
|
|
||||||
struct obj_coords
|
|
||||||
{
|
|
||||||
float LAT;
|
|
||||||
float LONG;
|
|
||||||
float cl;
|
|
||||||
};
|
|
||||||
|
|
||||||
int write_coords_to_file(std::string buffer, FILE *f)
|
|
||||||
{
|
|
||||||
|
|
||||||
std::istringstream is(buffer);
|
|
||||||
cereal::PortableBinaryInputArchive retrieve(is);
|
|
||||||
Message m;
|
|
||||||
retrieve(m);
|
|
||||||
|
|
||||||
int cam_id = m.cam_idx;
|
|
||||||
unsigned long long t_stamp_ms = m.t_stamp_ms;
|
|
||||||
int obj_n = m.num_objects;
|
|
||||||
|
|
||||||
struct obj_coords *c = (struct obj_coords *)malloc(obj_n * sizeof(struct obj_coords));
|
|
||||||
|
|
||||||
int i;
|
|
||||||
for (i = 0; i < obj_n; i++)
|
|
||||||
{
|
|
||||||
c[i].LAT = m.objects.at(i).latitude;
|
|
||||||
c[i].LONG = m.objects.at(i).longitude;
|
|
||||||
c[i].cl = m.objects.at(i).category;
|
|
||||||
//m.objects.at(i).speed;
|
|
||||||
//m.objects.at(i).orientation;
|
|
||||||
}
|
|
||||||
|
|
||||||
char *to_print = (char *)malloc(100000);
|
|
||||||
memset(to_print, 0, 100000);
|
|
||||||
|
|
||||||
/*int obj_n;
|
|
||||||
int cam_id;
|
|
||||||
unsigned long long t_stamp_ms;
|
|
||||||
char type_of_m;
|
|
||||||
|
|
||||||
int consumed_chars = 0;
|
|
||||||
char *shifted_chars = (char*)malloc(4000);
|
|
||||||
char *to_print = (char*)malloc(100000);
|
|
||||||
memset(to_print,0,100000);
|
|
||||||
|
|
||||||
sscanf(buffer, "%c %lld %d %d ", &type_of_m, &t_stamp_ms, &cam_id, &obj_n);
|
|
||||||
sprintf(shifted_chars, "%c %lld %d %d ", type_of_m, t_stamp_ms, cam_id, obj_n);
|
|
||||||
consumed_chars += strlen(shifted_chars);
|
|
||||||
//printf("%c %lld %d %d\n", type_of_m, t_stamp_ms, cam_id, *obj_n);
|
|
||||||
|
|
||||||
struct obj_coords *c = (struct obj_coords *)malloc(obj_n * sizeof(struct obj_coords));
|
|
||||||
|
|
||||||
int i;
|
|
||||||
for (i = 0; i < obj_n; i++)
|
|
||||||
{
|
|
||||||
sscanf(buffer + consumed_chars, "%f %f %f ", &c[i].LAT, &c[i].LONG, &c[i].cl);
|
|
||||||
sprintf(shifted_chars, "%.9f %.9f %.0f ", c[i].LAT, c[i].LONG, c[i].cl);
|
|
||||||
consumed_chars += strlen(shifted_chars);
|
|
||||||
//printf("%Lf %f %f \n", c[i].LAT, c[i].LONG, c[i].cl);
|
|
||||||
}*/
|
|
||||||
|
|
||||||
char *command = (char *)malloc(4000);
|
|
||||||
for (i = 0; i < obj_n; i++)
|
|
||||||
{
|
|
||||||
sprintf(command, "%d %lld %.0f %.9f %.9f %f %f\n", cam_id, t_stamp_ms, c[i].cl, c[i].LAT, c[i].LONG, 0.0f, 0.0f);
|
|
||||||
strcat(to_print, command);
|
|
||||||
}
|
|
||||||
|
|
||||||
printf("%s", to_print);
|
|
||||||
|
|
||||||
fprintf(f, "%s", to_print);
|
|
||||||
free(to_print);
|
|
||||||
free(command);
|
|
||||||
free(c);
|
|
||||||
|
|
||||||
return obj_n;
|
|
||||||
}
|
|
||||||
|
|
||||||
//the thread function
|
|
||||||
void *connection_handler(void *);
|
|
||||||
|
|
||||||
int main(int argc, char *argv[])
|
|
||||||
{
|
|
||||||
const int path_size = 400;
|
|
||||||
//const int message_size = 100000;
|
|
||||||
char path[path_size];
|
|
||||||
//int obj_n;
|
|
||||||
const char *basepath = "/tmp/";
|
|
||||||
struct stat st = {0};
|
|
||||||
|
|
||||||
sem_init(&semaphore, 0, 1);
|
|
||||||
|
|
||||||
int socket_desc, client_sock, c, *new_sock;
|
|
||||||
struct sockaddr_in server, client;
|
|
||||||
|
|
||||||
time_t rawtime;
|
|
||||||
time(&rawtime);
|
|
||||||
struct tm *tm_struct = localtime(&rawtime);
|
|
||||||
int tm_hour = tm_struct->tm_hour;
|
|
||||||
int tm_yday = tm_struct->tm_yday;
|
|
||||||
|
|
||||||
sprintf(path, "%s%d/", basepath, tm_yday);
|
|
||||||
if (stat(path, &st) == -1)
|
|
||||||
{
|
|
||||||
mkdir(path, 0700);
|
|
||||||
}
|
|
||||||
memset(path, 0, path_size);
|
|
||||||
|
|
||||||
sprintf(path, "%s%d/%d/", basepath, tm_yday, tm_hour);
|
|
||||||
if (stat(path, &st) == -1)
|
|
||||||
{
|
|
||||||
mkdir(path, 0700);
|
|
||||||
}
|
|
||||||
memset(path, 0, path_size);
|
|
||||||
|
|
||||||
//Create socket
|
|
||||||
socket_desc = socket(AF_INET, SOCK_STREAM, 0);
|
|
||||||
if (socket_desc == -1)
|
|
||||||
{
|
|
||||||
printf("Could not create socket");
|
|
||||||
}
|
|
||||||
puts("Socket created");
|
|
||||||
|
|
||||||
//Prepare the sockaddr_in structure
|
|
||||||
server.sin_family = AF_INET;
|
|
||||||
server.sin_addr.s_addr = INADDR_ANY;
|
|
||||||
server.sin_port = htons(8888);
|
|
||||||
|
|
||||||
//Bind
|
|
||||||
if (bind(socket_desc, (struct sockaddr *)&server, sizeof(server)) < 0)
|
|
||||||
{
|
|
||||||
//print the error message
|
|
||||||
perror("bind failed. Error");
|
|
||||||
return 1;
|
|
||||||
}
|
|
||||||
puts("bind done");
|
|
||||||
|
|
||||||
//Listen
|
|
||||||
listen(socket_desc, 3);
|
|
||||||
|
|
||||||
//Accept and incoming connection
|
|
||||||
puts("Waiting for incoming connections...");
|
|
||||||
c = sizeof(struct sockaddr_in);
|
|
||||||
while ((client_sock = accept(socket_desc, (struct sockaddr *)&client, (socklen_t *)&c)))
|
|
||||||
{
|
|
||||||
puts("Connection accepted");
|
|
||||||
|
|
||||||
pthread_t sniffer_thread;
|
|
||||||
new_sock = (int *)malloc(1);
|
|
||||||
*new_sock = client_sock;
|
|
||||||
|
|
||||||
if (pthread_create(&sniffer_thread, NULL, connection_handler, (void *)new_sock) < 0)
|
|
||||||
{
|
|
||||||
perror("could not create thread");
|
|
||||||
return 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
//Now join the thread , so that we dont terminate before the thread
|
|
||||||
//pthread_join( sniffer_thread , NULL);
|
|
||||||
puts("Handler assigned");
|
|
||||||
}
|
|
||||||
|
|
||||||
if (client_sock < 0)
|
|
||||||
{
|
|
||||||
perror("accept failed");
|
|
||||||
return 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
sem_destroy(&semaphore);
|
|
||||||
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
/*
|
|
||||||
* This will handle connection for each client
|
|
||||||
* */
|
|
||||||
void *connection_handler(void *socket_desc)
|
|
||||||
{
|
|
||||||
time_t rawtime;
|
|
||||||
struct tm *tm_struct;
|
|
||||||
int tm_hour;
|
|
||||||
int tm_yday;
|
|
||||||
const int path_size = 400;
|
|
||||||
const int message_size = 100000;
|
|
||||||
char path[path_size];
|
|
||||||
int obj_n;
|
|
||||||
const char *basepath = "/tmp/";
|
|
||||||
struct stat st = {0};
|
|
||||||
FILE *f;
|
|
||||||
int mess_hour, mess_min, mess_yday;
|
|
||||||
|
|
||||||
//Get the socket descriptor
|
|
||||||
int sock = *(int *)socket_desc;
|
|
||||||
int read_size;
|
|
||||||
|
|
||||||
void *client_message = (void *)malloc(message_size);
|
|
||||||
|
|
||||||
/* //Send some messages to the client
|
|
||||||
message = "Greetings! I am your connection handler\n";
|
|
||||||
write(sock, message, strlen(message)); */
|
|
||||||
|
|
||||||
//Receive a message from client
|
|
||||||
while ((read_size = recv(sock, client_message, message_size, 0)) > 0)
|
|
||||||
{
|
|
||||||
|
|
||||||
std::string s((char *)client_message, message_size);
|
|
||||||
//std::cout<<"Message received: "<<s<<std::endl;
|
|
||||||
|
|
||||||
time(&rawtime);
|
|
||||||
tm_struct = localtime(&rawtime);
|
|
||||||
mess_min = tm_struct->tm_min;
|
|
||||||
mess_hour = tm_struct->tm_hour;
|
|
||||||
mess_yday = tm_struct->tm_yday;
|
|
||||||
if (mess_yday != tm_yday)
|
|
||||||
{
|
|
||||||
tm_yday = mess_yday;
|
|
||||||
sprintf(path, "%s%d/", basepath, tm_yday);
|
|
||||||
if (stat(path, &st) == -1)
|
|
||||||
{
|
|
||||||
mkdir(path, 0700);
|
|
||||||
}
|
|
||||||
memset(path, 0, path_size);
|
|
||||||
}
|
|
||||||
if (mess_hour != tm_hour)
|
|
||||||
{
|
|
||||||
tm_hour = mess_hour;
|
|
||||||
sprintf(path, "%s%d/%d/", basepath, tm_yday, tm_hour);
|
|
||||||
if (stat(path, &st) == -1)
|
|
||||||
{
|
|
||||||
mkdir(path, 0700);
|
|
||||||
}
|
|
||||||
memset(path, 0, path_size);
|
|
||||||
}
|
|
||||||
|
|
||||||
sprintf(path, "%s%d/%d/%d.txt", basepath, tm_yday, tm_hour, mess_min);
|
|
||||||
|
|
||||||
//CRITICAL SECTION
|
|
||||||
sem_wait(&semaphore);
|
|
||||||
f = fopen(path, "a");
|
|
||||||
memset(path, 0, path_size);
|
|
||||||
|
|
||||||
obj_n = write_coords_to_file(s, f);
|
|
||||||
|
|
||||||
fclose(f);
|
|
||||||
sem_post(&semaphore);
|
|
||||||
}
|
|
||||||
|
|
||||||
if (read_size == 0)
|
|
||||||
{
|
|
||||||
puts("Client disconnected");
|
|
||||||
fflush(stdout);
|
|
||||||
}
|
|
||||||
else if (read_size == -1)
|
|
||||||
{
|
|
||||||
perror("recv failed");
|
|
||||||
}
|
|
||||||
|
|
||||||
//Free the socket pointer
|
|
||||||
free(socket_desc);
|
|
||||||
|
|
||||||
free(client_message);
|
|
||||||
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
@@ -0,0 +1,138 @@
|
|||||||
|
#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"
|
||||||
|
|
||||||
|
|
||||||
|
struct obj_coords
|
||||||
|
{
|
||||||
|
double LAT;
|
||||||
|
double LONG;
|
||||||
|
float cl;
|
||||||
|
};
|
||||||
|
|
||||||
|
void readTiff(char *filename, double *adfGeoTransform)
|
||||||
|
{
|
||||||
|
GDALDataset *poDataset;
|
||||||
|
GDALAllRegister();
|
||||||
|
poDataset = (GDALDataset *)GDALOpen(filename, GA_ReadOnly);
|
||||||
|
if (poDataset != NULL)
|
||||||
|
{
|
||||||
|
//int colms = poDataset->GetRasterXSize();
|
||||||
|
//int rows = poDataset->GetRasterYSize();
|
||||||
|
poDataset->GetGeoTransform(adfGeoTransform);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
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(struct obj_coords *coords, int i, 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);
|
||||||
|
coords[i].LAT = latitude;
|
||||||
|
coords[i].LONG = longitude;
|
||||||
|
coords[i].cl = detected_class;
|
||||||
|
|
||||||
|
/*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, coords[i].LAT, coords[i].LONG);
|
||||||
|
//printf( "%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)
|
||||||
|
{
|
||||||
|
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++;
|
||||||
|
proj_matrix_read = 1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
free(line);
|
||||||
|
fclose(fp);
|
||||||
|
fillMatrix(H, proj_matrix);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
free(proj_matrix);
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif /*CLASSUTILS_H*/
|
||||||
-268
@@ -1,268 +0,0 @@
|
|||||||
#ifndef SEND_H
|
|
||||||
#define SEND_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 "serialize.hpp"
|
|
||||||
|
|
||||||
struct obj_coords
|
|
||||||
{
|
|
||||||
double LAT;
|
|
||||||
double LONG;
|
|
||||||
float cl;
|
|
||||||
};
|
|
||||||
|
|
||||||
void readTiff(char *filename, double *adfGeoTransform)
|
|
||||||
{
|
|
||||||
GDALDataset *poDataset;
|
|
||||||
GDALAllRegister();
|
|
||||||
poDataset = (GDALDataset *)GDALOpen(filename, GA_ReadOnly);
|
|
||||||
if (poDataset != NULL)
|
|
||||||
{
|
|
||||||
//int colms = poDataset->GetRasterXSize();
|
|
||||||
//int rows = poDataset->GetRasterYSize();
|
|
||||||
poDataset->GetGeoTransform(adfGeoTransform);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
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";
|
|
||||||
}
|
|
||||||
|
|
||||||
void serialize_coords(struct obj_coords *c, int obj_n, int CAM_IDX, std::stringbuf *buf)
|
|
||||||
{
|
|
||||||
|
|
||||||
std::ostream os(buf);
|
|
||||||
cereal::PortableBinaryOutputArchive archive(os);
|
|
||||||
|
|
||||||
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;
|
|
||||||
|
|
||||||
std::vector<Road_User> ruv;
|
|
||||||
|
|
||||||
int i;
|
|
||||||
for (i = 0; i < obj_n; i++)
|
|
||||||
{
|
|
||||||
Road_User r{c[i].LAT, c[i].LONG, 0, 0, (int)c[i].cl};
|
|
||||||
ruv.push_back(r);
|
|
||||||
}
|
|
||||||
|
|
||||||
Message m{CAM_IDX, t_stamp_ms, ruv.size(), ruv};
|
|
||||||
archive(m);
|
|
||||||
|
|
||||||
//std::cout<<buf->str()<<std::endl;
|
|
||||||
|
|
||||||
//return buf;
|
|
||||||
}
|
|
||||||
|
|
||||||
struct obj_coords *deserialize_coords(char *buffer, int *obj_n)
|
|
||||||
{
|
|
||||||
|
|
||||||
std::stringbuf buf(buffer);
|
|
||||||
std::istream is(&buf);
|
|
||||||
cereal::PortableBinaryInputArchive retrieve(is);
|
|
||||||
Message m;
|
|
||||||
retrieve(m);
|
|
||||||
|
|
||||||
int cam_id = m.cam_idx;
|
|
||||||
unsigned long long t_stamp_ms = m.t_stamp_ms;
|
|
||||||
*obj_n = m.num_objects;
|
|
||||||
|
|
||||||
struct obj_coords *c = (struct obj_coords *)malloc(*obj_n * sizeof(struct obj_coords));
|
|
||||||
|
|
||||||
int i;
|
|
||||||
for (i = 0; i < *obj_n; i++)
|
|
||||||
{
|
|
||||||
c[i].LAT = m.objects.at(i).latitude;
|
|
||||||
c[i].LONG = m.objects.at(i).longitude;
|
|
||||||
c[i].cl = m.objects.at(i).category;
|
|
||||||
//m.objects.at(i).speed;
|
|
||||||
//m.objects.at(i).orientation;
|
|
||||||
}
|
|
||||||
|
|
||||||
return c;
|
|
||||||
}
|
|
||||||
|
|
||||||
int map_class_coco_to_voc(int coco_class)
|
|
||||||
{
|
|
||||||
switch (coco_class)
|
|
||||||
{
|
|
||||||
case 0:
|
|
||||||
return 14; //person
|
|
||||||
case 1:
|
|
||||||
return 1; //bicycle
|
|
||||||
case 2:
|
|
||||||
return 6; //car
|
|
||||||
case 3:
|
|
||||||
return 13; //motorkbike
|
|
||||||
case 5:
|
|
||||||
return 5; //bus
|
|
||||||
}
|
|
||||||
return -1;
|
|
||||||
}
|
|
||||||
|
|
||||||
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)
|
|
||||||
{
|
|
||||||
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);
|
|
||||||
coords[i].LAT = latitude;
|
|
||||||
coords[i].LONG = longitude;
|
|
||||||
coords[i].cl = map_class_coco_to_voc(detected_class);
|
|
||||||
|
|
||||||
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;
|
|
||||||
|
|
||||||
//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);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void read_projection_matrix(cv::Mat &H, int &proj_matrix_read, 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++;
|
|
||||||
proj_matrix_read = 1;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
free(line);
|
|
||||||
fclose(fp);
|
|
||||||
fillMatrix(H, proj_matrix);
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
free(proj_matrix);
|
|
||||||
}
|
|
||||||
|
|
||||||
int open_socket(char *ip, int &sock, int &socket_opened)
|
|
||||||
{
|
|
||||||
struct sockaddr_in server;
|
|
||||||
sock = socket(AF_INET, SOCK_STREAM, 0);
|
|
||||||
if (sock == -1)
|
|
||||||
{
|
|
||||||
printf("Could not create socket");
|
|
||||||
}
|
|
||||||
puts("Socket created");
|
|
||||||
|
|
||||||
server.sin_addr.s_addr = inet_addr(ip);
|
|
||||||
server.sin_family = AF_INET;
|
|
||||||
server.sin_port = htons(8888);
|
|
||||||
|
|
||||||
/*connect to remote server*/
|
|
||||||
if (connect(sock, (struct sockaddr *)&server, sizeof(server)) < 0)
|
|
||||||
{
|
|
||||||
perror("connect failed. Error");
|
|
||||||
socket_opened = 0;
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
puts("Connected\n");
|
|
||||||
socket_opened = 1;
|
|
||||||
return 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
int send_client_dummy(struct obj_coords *coords, int n_coords, int &sock, int &socket_opened, int CAM_IDX)
|
|
||||||
{
|
|
||||||
/*serialize coords*/
|
|
||||||
std::stringbuf *message = new std::stringbuf();
|
|
||||||
serialize_coords(coords, n_coords, CAM_IDX, message);
|
|
||||||
|
|
||||||
//std::cout<<message->str().length()<<std::endl;
|
|
||||||
|
|
||||||
/*open socket if not already opened*/
|
|
||||||
if (socket_opened == 0)
|
|
||||||
{
|
|
||||||
int res = open_socket("127.0.0.1", sock, socket_opened);
|
|
||||||
if (res)
|
|
||||||
printf("Socket opened!\n");
|
|
||||||
else
|
|
||||||
{
|
|
||||||
printf("Problem: socket NOT opened!\n");
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/*send message to server*/
|
|
||||||
if (send(sock, message->str().data(), message->str().length(), 0) < 0)
|
|
||||||
{
|
|
||||||
puts("Send failed");
|
|
||||||
socket_opened = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
delete message;
|
|
||||||
//free(message);
|
|
||||||
//close(sock);
|
|
||||||
return 1;
|
|
||||||
}
|
|
||||||
|
|
||||||
#endif /*SEND_H*/
|
|
||||||
@@ -1,38 +0,0 @@
|
|||||||
#ifndef SERIALIZE_H
|
|
||||||
#define SERIALIZE_H
|
|
||||||
|
|
||||||
#include <cereal/archives/portable_binary.hpp>
|
|
||||||
#include <cereal/types/vector.hpp>
|
|
||||||
|
|
||||||
struct Road_User{
|
|
||||||
float latitude;
|
|
||||||
float longitude;
|
|
||||||
uint8_t speed;
|
|
||||||
uint8_t orientation;
|
|
||||||
uint8_t category;
|
|
||||||
|
|
||||||
template<class Archive>
|
|
||||||
void serialize(Archive & archive)
|
|
||||||
{
|
|
||||||
archive( latitude, longitude, speed, orientation, category );
|
|
||||||
}
|
|
||||||
|
|
||||||
};
|
|
||||||
|
|
||||||
struct Message{
|
|
||||||
uint32_t cam_idx;
|
|
||||||
uint64_t t_stamp_ms;
|
|
||||||
uint16_t num_objects;
|
|
||||||
std::vector<Road_User> objects;
|
|
||||||
|
|
||||||
template<class Archive>
|
|
||||||
void serialize(Archive & archive)
|
|
||||||
{
|
|
||||||
archive( cam_idx, t_stamp_ms, num_objects, objects );
|
|
||||||
}
|
|
||||||
};
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
#endif
|
|
||||||
Submodule
+1
Submodule masa_protocol added at 63f65f3397
Reference in New Issue
Block a user