Merge branch 'class' of https://github.com/ceccocats/tkDNN into class

demo/demo/demo.cpp

@@ -21,6 +21,8 @@
 #define MAX_DETECT_SIZE 100
 
 bool gRun;
+
+
 cv::Mat frame_v;
 cv::Mat frame_top_v;
 std::mutex sem;
@@ -33,7 +35,6 @@ void sig_handler(int signo)
 
 void *showImages(void *x_void_ptr)
 {
-
     while (gRun)
     {
         sem.lock();
@@ -44,6 +45,18 @@ void *showImages(void *x_void_ptr)
     }
 }
 
+
+void draw_arrow(float angleRad, float vel,  cv::Scalar color, cv::Point center, cv::Mat &frame)
+{
+    int angle =  angleRad*180.0/CV_PI;
+    auto length = 10*vel;
+    auto direction = cv::Point(length * cos(angleRad), length * sin(angleRad)); // calculate direction
+    double tipLength = .2 + 0.4 * ( angle%180) / 360;
+    int lineType = 8;
+    int thickness = 2;
+    cv::arrowedLine(frame, center, center + direction, color, thickness, lineType, 0, tipLength); // draw arrow!
+}
+
 int main(int argc, char *argv[])
 {
 
@@ -62,7 +75,6 @@ int main(int argc, char *argv[])
     char *tiffile = "../demo/demo/data/map_b.tif";
     if (argc > 4)
         tiffile = argv[4];
-    /*CAMID*/
     int CAM_IDX = 0;
     if (argc > 5)
         CAM_IDX = atoi(argv[5]);
@@ -70,6 +82,8 @@ int main(int argc, char *argv[])
     if (argc > 6)
         to_show = atoi(argv[6]);
 
+    
+    
     tk::dnn::Yolo3Detection yolo;
     yolo.init(net);
     yolo.thresh = 0.25;
@@ -84,23 +98,27 @@ int main(int argc, char *argv[])
 
     cv::Mat frame;
     cv::Mat frame_top;
-
     cv::Mat dnn_input;
-    cv::namedWindow("detection", cv::WINDOW_NORMAL);
+    cv::Mat original_frame_top;
-    cv::namedWindow("topview", cv::WINDOW_NORMAL);
-
-    frame_top = cv::imread("../demo/demo/data/map_b.jpg");
     
     pthread_t visual;
     
-    /*projection matrix*/
+    if(to_show)
+    {
+        cv::namedWindow("detection", cv::WINDOW_NORMAL);
+        cv::namedWindow("topview", cv::WINDOW_NORMAL);
+        frame_top = cv::imread("../demo/demo/data/map_b.jpg");
+        original_frame_top = frame_top.clone();
+    }
 
+    /*projection matrix*/
     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);
+    struct obj_coords *coords = (struct obj_coords *)malloc(MAX_DETECT_SIZE * sizeof(struct obj_coords));
 
     /*socket*/
     int sock;
@@ -121,12 +139,9 @@ int main(int argc, char *argv[])
     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)
     {
-
         cap >> frame;
         if (!frame.data)
         {
@@ -148,13 +163,7 @@ 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++)
@@ -178,16 +187,16 @@ int main(int argc, char *argv[])
 
             //std::cout<<obj_class<<" ("<<prob<<"): "<<x0<<" "<<y0<<" "<<x1<<" "<<y1<<"\n";
             cv::rectangle(frame, cv::Point(x0, y0), cv::Point(x1, y1), yolo.colors[obj_class], 2);
+        
         }
 
-        //TIMER_START
+        TIMER_START
 
+        //convert from latitude and longitude to meters for ekf
         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;
             gc.geodetic2Enu(coords[i].LAT, coords[i].LONG, 0, &east, &north, &up);
-            //std::cout << "east: " << east << " north: " << north << std::endl;
             cur_frame.push_back(Data(east, north, frame_nbr));
         }
 
@@ -201,16 +210,18 @@ int main(int argc, char *argv[])
             Track(cur_frame, dt, n_states, initial_age, age_threshold, trackers);
         }
 
-        //TIMER_STOP
+        TIMER_STOP
-        //std::cout << "There are " << trackers.size() << " trackers" << std::endl;
+        std::cout << "There are " << trackers.size() << " trackers" << std::endl;
-
 
+        if(to_show)
+        {
+            frame_top = original_frame_top.clone();
             for (auto t : trackers)
             {
-            for (auto pred_pos : t.pred_list_)
+                for(size_t p=1; p<t.pred_list_.size(); p++ )
                 {
                     
-                gc.enu2Geodetic(pred_pos.x_, pred_pos.y_, 0, &lat, &lon, &alt);
+                    gc.enu2Geodetic(t.pred_list_[p].x_, t.pred_list_[p].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);
@@ -227,21 +238,30 @@ int main(int argc, char *argv[])
                     //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(t.r_, t.g_, t.b_), CV_FILLED, 8, 0);
+
+                    if(p == t.pred_list_.size()-1)
+                    {
+                        auto center = cv::Point(camera_p[0].x, camera_p[0].y);
+                        auto color = cv::Scalar(t.r_, t.g_, t.b_);
+                        draw_arrow(t.pred_list_[p].yaw_, t.pred_list_[p].vel_,color, center, frame);
+
+                        center = cv::Point(pix_x,pix_y);
+                        draw_arrow(t.pred_list_[p].yaw_, t.pred_list_[p].vel_,color, center, frame_top);
+                    }
+                }
             }
         }
 
-
+        if(to_show)
-        TIMER_START
+        {
             sem.lock();
             frame_v = frame.clone();
             frame_top_v = frame_top.clone();
             sem.unlock();
+        }
 
-        TIMER_STOP
+        if (frame_nbr == 0 && to_show)
-
-        if (frame_nbr == 0)
         {
-
             if (pthread_create(&visual, NULL, showImages, NULL))
             {
                 fprintf(stderr, "Error creating thread\n");
@@ -250,16 +270,9 @@ int main(int argc, char *argv[])
         }
 
         frame_nbr++;
-
         send_client_dummy(coords, coord_i, sock, socket_opened, CAM_IDX);
-
-        
     }
 
-    /*     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_); */
-
     free(coords);
     free(adfGeoTransform);