new send and submodule masa_protocol added
This commit is contained in:
@@ -1,3 +1,6 @@
|
||||
[submodule "tracker_CLASS"]
|
||||
path = tracker_CLASS
|
||||
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 python2.7 yaml-cpp)
|
||||
|
||||
add_executable(class_server demo/server/server_less_dummy.cpp)
|
||||
target_link_libraries(class_server pthread tkDNN )
|
||||
|
||||
|
||||
#install
|
||||
|
||||
+64
-5
@@ -14,7 +14,10 @@
|
||||
#include <opencv2/imgproc/imgproc.hpp>
|
||||
|
||||
#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 "trackutils.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!
|
||||
}
|
||||
|
||||
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[])
|
||||
{
|
||||
|
||||
@@ -76,7 +125,7 @@ int main(int argc, char *argv[])
|
||||
char *tiffile = "../demo/demo/data/map_b.tif";
|
||||
if (argc > 4)
|
||||
tiffile = argv[4];
|
||||
int CAM_IDX = 0;
|
||||
int CAM_IDX = 20936;
|
||||
if (argc > 5)
|
||||
CAM_IDX = atoi(argv[5]);
|
||||
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));
|
||||
|
||||
/*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*/
|
||||
geodetic_converter::GeodeticConverter gc;
|
||||
@@ -313,7 +364,15 @@ int main(int argc, char *argv[])
|
||||
}
|
||||
|
||||
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);
|
||||
|
||||
@@ -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