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

georeferencing

Browse Source
This commit is contained in:
Francesco Gatti
2019-04-19 16:21:56 +02:00
parent 46c32edb94
commit 3c32d0c876
3 changed files with 106 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
include/send.h
+83 -19
View File
@@ -9,6 +9,16 @@
#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
{
float LAT;
@@ -17,6 +27,47 @@ struct obj_coords
};
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)
{
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 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";
}
char *serialize_coords(struct obj_coords *c, int obj_n, int CAM_IDX)
{
@@ -83,36 +134,46 @@ int map_class_coco_to_voc(int coco_class)
return -1;
}
void convert_coords(struct obj_coords *coords, int i, int x, int y, int detected_class,float * proj_matrix)
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));
float obj_x = x, obj_y = y, obj_z = 1;
float tmp_z = 0;
coords[i].LAT = proj_matrix[0] * obj_x + proj_matrix[1] * obj_y + proj_matrix[2] * obj_z;
coords[i].LONG = proj_matrix[3] * obj_x + proj_matrix[4] * obj_y + proj_matrix[5] * obj_z;
tmp_z = proj_matrix[6] * obj_x + proj_matrix[7] * obj_y + proj_matrix[8] * obj_z;
if(tmp_z != 0.0)
{
coords[i].LAT = coords[i].LAT / tmp_z;
coords[i].LONG = coords[i].LONG / tmp_z;
//printf("lat: %f, long %f\n", coords[i].LAT, coords[i].LONG);
}
else
printf("Division by 0 (tmp_z)\n");
//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(float * proj_matrix, int &proj_matrix_read, char* path)
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);
@@ -128,6 +189,9 @@ void read_projection_matrix(float * proj_matrix, int &proj_matrix_read, char* pa
}
free(line);
fclose(fp);
fillMatrix(H, proj_matrix);
free(proj_matrix);
}
int open_socket(char *ip, int &sock, int &socket_opened)