tracking integrated

demo/demo/data/proj_matrix_map_b.txt

@@ -1,3 +1,3 @@
-0.1472053693584627 -13.052205853952705 439.9459667398692 
+-0.4122368700442484 -10.479982981650977 812.1821012303 
--0.6469057430349019 -6.339418443938649 578.7156423035825 
+-0.6536846365005352 -4.951476039607947 512.0831604767536 
--0.00025681337819676855 -0.007446975429175878 1.0 
+-0.00042630219831388434 -0.006003223898658603 1.0 

demo/demo/demo.cpp

@@ -16,24 +16,22 @@
 #include "plot.h"
 #include "tracker.h"
 
-
 #define MAX_DETECT_SIZE 100
 
-
-
 bool gRun;
 
-void sig_handler(int signo) {
+void sig_handler(int signo)
+{
     std::cout << "request gateway stop\n";
     gRun = false;
 }
 
-int main(int argc, char *argv[]) {
+int main(int argc, char *argv[])
+{
 
     std::cout << "detection\n";
     signal(SIGINT, sig_handler);
 
-
     char *net = "yolo3_coco4.rt";
     if (argc > 1)
         net = argv[1];
@@ -79,7 +77,6 @@ int main(int argc, char *argv[]) {
     double *adfGeoTransform = (double *)malloc(6 * sizeof(double));
     readTiff(tiffile, adfGeoTransform);
 
-
     /*socket*/
     int sock;
     int socket_opened = 0;
@@ -94,24 +91,23 @@ int main(int argc, char *argv[]) {
     std::vector<Tracker> trackers;
     std::vector<Data> cur_frame;
     int initial_age = -5;
-    int age_threshold = -10;
+    int age_threshold = -20;
     int n_states = 5;
     float dt = 0.03;
 
     struct obj_coords *coords = (struct obj_coords *)malloc(MAX_DETECT_SIZE * sizeof(struct obj_coords));
 
-
     int frame_nbr = 0;
-    while(gRun) {
+    while (gRun)
-
+    {
 
         cap >> frame;
-        if(!frame.data) {
+        if (!frame.data)
+        {
             usleep(1000000);
             cap.open(input);
             printf("cap reinitialize\n");
             continue;
-
         }
 
         // this will be resized to the net format
@@ -126,16 +122,17 @@ int main(int argc, char *argv[]) {
             num_detected = MAX_DETECT_SIZE;
 
         if (proj_matrix_read == 0)
+        {
             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],
             proj_matrix[6],proj_matrix[7],proj_matrix[8]);*/
 
         // draw dets
-        for(int i=0; i<num_detected; i++) {
+        for (int i = 0; i < num_detected; i++)
-
+        {
-            
 
             tk::dnn::box b = yolo.detected[i];
             int x0 = b.x;
@@ -144,7 +141,6 @@ int main(int argc, char *argv[]) {
             int y1 = b.y + b.h;
             int obj_class = b.cl;
 
-
             if (obj_class == 0 /*person*/ || obj_class == 1 /*bicycle*/ || obj_class == 2 /*car*/
                 || obj_class == 3 /*motorbike*/ || obj_class == 5 /*bus*/)
             {
@@ -158,16 +154,17 @@ int main(int argc, char *argv[]) {
             cv::rectangle(frame, cv::Point(x0, y0), cv::Point(x1, y1), yolo.colors[obj_class], 2);
         }
 
-
         cur_frame.clear();
         for (int i = 0; i < coord_i; i++)
         {
-            std::cout<<"lat orig: "<<coords[i].LAT<<" lon orig: "<<coords[i].LONG<<std::endl;
+            //std::cout << "lat orig: " << coords[i].LAT << " lon orig: " << coords[i].LONG << std::endl;
             gc.geodetic2Enu(coords[i].LAT, coords[i].LONG, 0, &east, &north, &up);
-            std::cout<<"east: "<<east<<" north: "<<north<<std::endl;
+            //std::cout << "east: " << east << " north: " << north << std::endl;
             cur_frame.push_back(Data(east, north, frame_nbr));
         }
 
+        
+
         if (frame_nbr == 0)
         {
             for (auto f : cur_frame)
@@ -178,18 +175,29 @@ int main(int argc, char *argv[]) {
             Track(cur_frame, dt, n_states, initial_age, age_threshold, trackers);
         }
 
-        std::cout<<"There are "<<trackers.size()<<" trackers"<<std::endl;
+        //std::cout << "There are " << trackers.size() << " trackers" << std::endl;
 
         for (auto t : trackers)
         {
-            State s = t.ekf_.getEstimatedState();
+            for(auto pred_pos: t.pred_list_ )
-            gc.enu2Geodetic(s.x_, s.y_, 0, &lat, &lon, &alt);
+            {
-            std::cout<<"lat: "<<lat<<" lon: "<<lon<<std::endl;
+                
+                gc.enu2Geodetic(pred_pos.x_, pred_pos.y_, 0, &lat, &lon, &alt);
+                //std::cout << "lat: " << lat << " lon: " << lon << std::endl;
                 int pix_x, pix_y;
                 coord2pixel(lat, lon, pix_x, pix_y, adfGeoTransform);
-            std::cout<<"pix_x: "<<pix_x<<" pix_y: "<<pix_y<<std::endl;
+                //std::cout << "pix_x: " << pix_x << " pix_y: " << pix_y << std::endl;
-            //if(t.age_ > 0)
+
-                cv::circle( frame, cv::Point( pix_x, pix_y ), 10.0, cv::Scalar( 255, 0, 0 ), CV_FILLED, 8, 0);
+                std::vector<cv::Point2f> map_p, camera_p;
+                map_p.push_back(cv::Point2f(pix_x, pix_y));
+
+                //transform camera pixel to map pixel
+                cv::perspectiveTransform(map_p, camera_p, H.inv());
+                //std::cout << "pix_x: " << camera_p[0].x << " pix_y: " << camera_p[0].y << std::endl;
+
+                cv::circle(frame, cv::Point(camera_p[0].x, camera_p[0].y), 3.0, cv::Scalar(255, 0, 0), CV_FILLED, 8, 0);
+
+            }
         }
 
         frame_nbr++;
@@ -203,7 +211,6 @@ int main(int argc, char *argv[]) {
         }
     }
 
-
     /*     for (size_t i = 0; i < trackers.size(); i++)
         if (trackers[i].z_list_.size() > 10)
             plotTruthvsPred(trackers[i].z_list_, trackers[i].pred_list_); */
@@ -214,4 +221,3 @@ int main(int argc, char *argv[]) {
     std::cout << "detection end\n";
     return 0;
 }
-

include/send.h

@@ -17,8 +17,6 @@
 #include "gdal/gdal_priv.h"
 #include "gdal/cpl_conv.h"
 
-
-
 #include "serialize.hpp"
 
 struct obj_coords
@@ -28,7 +26,6 @@ struct obj_coords
     float cl;
 };
 
-
 void readTiff(char *filename, double *adfGeoTransform)
 {
     GDALDataset *poDataset;
@@ -57,7 +54,6 @@ void pixel2coord(int x, int y, double &lat, double &lon, double *adfGeoTransform
 
     lon = a * x + b * y + xoff;
     lat = d * x + e * y + yoff;
-
 }
 void coord2pixel(double lat, double lon, int &x, int &y, double *adfGeoTransform)
 {
@@ -65,18 +61,17 @@ void coord2pixel(double lat, double lon, int &x, int &y,  double *adfGeoTransfor
     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++) {
+    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)
 {
 
@@ -99,7 +94,6 @@ void serialize_coords(struct obj_coords *c, int obj_n, int CAM_IDX, std::stringb
     Message m{CAM_IDX, t_stamp_ms, ruv.size(), ruv};
     archive(m);
 
-
     //std::cout<<buf->str()<<std::endl;
 
     //return buf;
@@ -177,9 +171,7 @@ void convert_coords(struct obj_coords *coords, int i, int x, int y, int detected
 
         //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)
@@ -208,6 +200,8 @@ void read_projection_matrix(cv::Mat &H, int &proj_matrix_read, char* path)
     fclose(fp);
     fillMatrix(H, proj_matrix);
 
+
+
     free(proj_matrix);
 }
 
@@ -243,7 +237,6 @@ int send_client_dummy(struct obj_coords *coords, int n_coords, int &sock, int &s
     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*/
@@ -259,8 +252,6 @@ int send_client_dummy(struct obj_coords *coords, int n_coords, int &sock, int &s
         }
     }
 
-    
-
     /*send message to server*/
     if (send(sock, message->str().data(), message->str().length(), 0) < 0)
     {