georeferencing

CMakeLists.txt

@@ -34,7 +34,7 @@ cuda_include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include ${CUDA_INCLUDE_DIRS
 cuda_add_library(kernels SHARED ${tkdnn_CUSRC})
 
 file(GLOB tkdnn_SRC "src/*.cpp")
-set(tkdnn_LIBS kernels ${CUDA_LIBRARIES} ${CUDA_CUBLAS_LIBRARIES} -lcudnn -lnvinfer ${OpenCV_LIBS})
+set(tkdnn_LIBS kernels ${CUDA_LIBRARIES} ${CUDA_CUBLAS_LIBRARIES} -lcudnn -lnvinfer ${OpenCV_LIBS} -lgdal)
 
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -std=c++11")
 include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include ${CUDA_INCLUDE_DIRS} ${OPENCV_INCLUDE_DIRS} ${NVINFER_INCLUDES})

demo/demo/demo.cpp

@@ -12,6 +12,7 @@
 #include "Yolo3Detection.h"
 #include "send.h"
 
+
 #define MAX_DETECT_SIZE 100
 
 
@@ -38,13 +39,16 @@ int main(int argc, char *argv[]) {
     char *pmatrix = "/home/classfog1/repos/MASA_server/pmatrix/proj_matrix_20937.txt";
     if(argc > 3)
         pmatrix = argv[3];
+    char *tiffile = "/home/davide/repos/projection_tool/img/map_b.tif";
+    if(argc > 4)
+        tiffile = argv[4];
     /*CAMID*/
     int CAM_IDX = 0;
-    if(argc > 4)
-        CAM_IDX = atoi(argv[4]);
-    bool to_show = true;
     if(argc > 5)
-        to_show = atoi(argv[5]);
+        CAM_IDX = atoi(argv[5]);
+    bool to_show = true;
+    if(argc > 6)
+        to_show = atoi(argv[6]);
 
     tk::dnn::Yolo3Detection yolo;
     yolo.init(net);
@@ -63,8 +67,14 @@ int main(int argc, char *argv[]) {
     cv::namedWindow("detection", cv::WINDOW_NORMAL);    
 
     /*projection matrix*/
-    float* proj_matrix = (float*) malloc(9*sizeof(float));
+    
     int proj_matrix_read = 0;
+    cv::Mat H(cv::Size(3,3),CV_64FC1);
+
+    /*GPS information*/
+    double *adfGeoTransform = (double*)malloc(6*sizeof(double));
+    readTiff(tiffile, adfGeoTransform);
+    
 
     /*socket*/
     int sock;
@@ -72,6 +82,7 @@ int main(int argc, char *argv[]) {
     
     struct obj_coords *coords = (struct obj_coords*)malloc(MAX_DETECT_SIZE*sizeof(struct obj_coords));
 
+    int frame_nbr = 0;
     while(gRun) {
 
 
@@ -86,12 +97,13 @@ int main(int argc, char *argv[]) {
         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(proj_matrix, proj_matrix_read, pmatrix);
+            read_projection_matrix(H, proj_matrix_read, pmatrix);
         
         /*printf("%f %f %f \n%f %f %f\n %f %f %f\n\n", proj_matrix[0],proj_matrix[1],
             proj_matrix[2],proj_matrix[3],proj_matrix[4],proj_matrix[5],
@@ -110,7 +122,7 @@ int main(int argc, char *argv[]) {
             if(obj_class == 0 /*person*/ || obj_class == 1/*bicycle*/ || obj_class == 2/*car*/ 
                 || obj_class == 3/*motorbike*/ || obj_class == 5/*bus*/)
             {
-                convert_coords(coords, coord_i,x0+b.w/2, y1,obj_class, proj_matrix);
+                convert_coords(coords, coord_i,x0+b.w/2, y1,obj_class, H, adfGeoTransform, frame_nbr);
                 coord_i++;
             }
 
@@ -120,6 +132,8 @@ int main(int argc, char *argv[]) {
             cv::rectangle(frame, cv::Point(x0, y0), cv::Point(x1, y1), yolo.colors[obj_class], 2);                
         }
 
+        frame_nbr++;
+
         send_client_dummy(coords, coord_i, sock, socket_opened, CAM_IDX);
     
         if (to_show)
@@ -130,7 +144,7 @@ int main(int argc, char *argv[]) {
     }
 
     free(coords);
-    free(proj_matrix);
+    free(adfGeoTransform);
 
     std::cout<<"detection end\n";   
     return 0;

include/send.h

@@ -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));
 
-    
+    //transform camera pixel to map pixel
-
+    cv::perspectiveTransform( x_y, ll, H);
-    float obj_x = x, obj_y = y, obj_z = 1;
+    //tranform to map pixel to map gps
-    float tmp_z = 0;
+    pixel2coord(ll[0].x, ll[0].y, latitude,longitude, adfGeoTransform);
-
+    printf("lat: %f, long:%f \n", latitude, longitude);
-    coords[i].LAT = proj_matrix[0] * obj_x + proj_matrix[1] * obj_y + proj_matrix[2] * obj_z;
+    coords[i].LAT = latitude;
-    coords[i].LONG = proj_matrix[3] * obj_x + proj_matrix[4] * obj_y + proj_matrix[5] * obj_z;
+    coords[i].LONG = longitude;
-    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");
     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)