提交打印

TrackingRos.cpp

@@ -7,6 +7,18 @@
 #include "matrix.h"
 #include <eigen3/Eigen/Dense>
 
+void to360(float& rot_y)
+{
+    if (rot_y > 0)
+    {
+        rot_y = 2 * 3.1415926 - rot_y;
+    }
+    else
+    {
+        rot_y = -rot_y;
+    }
+}
+
 void CalculateGuassPos(float x, float y,float z,std::vector<std::vector<float>>& trans, float& gx, float& gy)
 {
     Eigen::Vector4f svTmp;
@@ -158,8 +170,11 @@ void TrackingRos::ThreadTrackingProcess()
             m_tracker.Run(input, detectionsId, updateId, lostId);
             uint64_t rTime = GetCurTime() - begin;
             //SDK_LOG(SDK_INFO, "m_tracker.Run time = %llu", rTime);
+            std::map<uint64_t, std::shared_ptr<Track3D> >& trackers = m_tracker.GetStates();
             static int count = 0;
+            static int calcCount = 0;
             count++;
+            calcCount += trackers.size();
             static uint64_t totelTime = 0;
             totelTime += rTime;
             static uint64_t countBegin = 0;
@@ -167,14 +182,12 @@ void TrackingRos::ThreadTrackingProcess()
                 countBegin = GetCurTime();
             if (GetCurTime() - countBegin > 3000 * 1000)
             {
-                SDK_LOG(SDK_INFO, "run kf average time = %llu", totelTime/ count);
+                SDK_LOG(SDK_INFO, "run kf average time = %llu,average count = %d", totelTime / count, calcCount/ count);
                 countBegin = GetCurTime();
                 count = 0;
+                calcCount = 0;
                 totelTime = 0;
             }
-
-            std::map<uint64_t, std::shared_ptr<Track3D> >& trackers = m_tracker.GetStates();
-            
             //SDK_LOG(SDK_INFO, "run before size = %d, after size = %d,",objsPtr->array.size(), trackers.size());
             jfx_common_msgs::det_tracking_array sendObjs = {};
             jsk_recognition_msgs::BoundingBoxArray sendBoxes = {};
@@ -244,6 +257,8 @@ void TrackingRos::ThreadTrackingProcess()
                         //SDK_LOG(SDK_INFO, "lat = %.10f, lon = %.10f", obj.Lat, obj.Long);
                     }
                 }
+                //修正rot_y值
+
                 jsk_recognition_msgs::BoundingBox box = {};
                 box.label = obj.obj_id;
                 box.value = 2;