提交相机物体的朝向更新

BaseTracker/Track3D.cpp

@@ -107,9 +107,9 @@ Track3D::Track3D():BaseTrack(10, 7)
 }
 void Track3D::Init(const std::vector<float>& data)
 {
-    if (data.size() != 8)
+    if (data.size() != 9)
         return;
-    std::vector<float> tmp(data.begin() + 1, data.end());
+    std::vector<float> tmp(data.begin() + 1, data.begin() + 8);
     //修正角度在-PI到PI
     while (tmp[3] > _PI_)
         tmp[3] -= 2 * _PI_;
@@ -200,9 +200,9 @@ float calcIOU(cv::RotatedRect rect1, cv::RotatedRect rect2) {
 
 double Track3D::CalculateIou(const std::vector<float>& data)
 {
-    if (data.size() != 8)
+    if (data.size() != 9)
     {
-        SDK_LOG(SDK_INFO, "CalculateIou data size != 8");
+        SDK_LOG(SDK_INFO, "CalculateIou data size != 9");
         return 0.0f;
     }
     if (m_obj == nullptr)
@@ -329,14 +329,15 @@ void Track3D::UpdateDataCheck(const std::vector<float>& data, std::vector<float>
 {
     if (kf_ == nullptr)
         return;
-    if (data.size() != 8)
+    if (data.size() != 9)
     {
         SDK_LOG(SDK_INFO, "UpdateDataCheck data size is not 8");
         return;
     }
-    std::vector<float> tmp(data.begin() + 1, data.end());
+    std::vector<float> tmp(data.begin() + 1, data.begin() + 8);
     double rot_y = tmp[3];
-    if (m_points.size() >= 5 && (abs(m_points[4].x - m_points[0].x) > DIST_THRED || abs(m_points[4].y - m_points[0].y) > DIST_THRED))
+    int dataSource = data[8];
+    if (dataSource == 1 && m_points.size() >= 5 && (abs(m_points[4].x - m_points[0].x) > DIST_THRED || abs(m_points[4].y - m_points[0].y) > DIST_THRED))
     {
         double center_rot_y = correct_angle(m_points);
         //SDK_LOG(SDK_INFO, "center_rot_y = %f,rot_y = %f", center_rot_y,rot_y);
@@ -350,6 +351,11 @@ void Track3D::UpdateDataCheck(const std::vector<float>& data, std::vector<float>
             rot_y += _PI_;
         }
     }
+    else if (dataSource > 1 && m_points.size() >= 5 && (abs(m_points[4].x - m_points[0].x) > DIST_THRED || abs(m_points[4].y - m_points[0].y) > DIST_THRED))
+    {
+        double center_rot_y = correct_angle(m_points);
+        rot_y = center_rot_y;
+    }
     double x_angle = kf_->x_[3];
     double deta = rot_y - x_angle;
     double deta2 = rot_y + _PI_ - x_angle;
@@ -415,9 +421,9 @@ int Track3D::GetIouData(std::vector<float>& data, int& obj_type)
 
 void Track3D::MeasureIouData(const std::vector<float>& input, std::vector<float>& out, int& obj_type)
 {
-    if (input.size() != 8)
+    if (input.size() != 9)
     {
-        SDK_LOG(SDK_INFO, "input is not 8");
+        SDK_LOG(SDK_INFO, "input is not 9");
         return;
     }
     obj_type = input[0];

TrackingRos.cpp

@@ -385,6 +385,7 @@ void TrackingRos::ThreadTrackingProcess()
                 data.push_back(obj.l);
                 data.push_back(obj.w);
                 data.push_back(obj.h);
+                data.push_back(obj.dataSource);
                 input.emplace_back(data);
             }
             std::vector<uint64_t> detectionsId;