加入新的跟踪逻辑

TrackEx/BaseTrack3D.cpp

@@ -22,7 +22,10 @@
 #include "BaseTrack3D.hpp"
 #include "TrackFunc.hpp"
 #include "Component.h"
+#include "LogBase.h"
+#include "TrackFunc.hpp"
 
+#define DIST_THRED  0.5
 
 BaseTrack3D::BaseTrack3D(unsigned int num_states, unsigned int num_obs):BaseTrack(num_states,num_obs)
 {
@@ -32,7 +35,7 @@ BaseTrack3D::BaseTrack3D(unsigned int num_states, unsigned int num_obs):BaseTrac
 void BaseTrack3D::Init(const std::vector<float>& data)
 {
     //输入的数据数量必须是大于等于观测量的大小，然后观测量必须放在前面，后面可以放入其他的数据
-    if (data.size() < num_obs)
+    if (data.size() < m_num_obs)
         return;
     std::vector<float> tmp;
     if(GetInputObsData(data,tmp) == 0)
@@ -40,10 +43,10 @@ void BaseTrack3D::Init(const std::vector<float>& data)
 }
 int BaseTrack3D::GetInputObsData(const std::vector<float>& data,std::vector<float>& out)
 {
-    if (data.size() < num_obs)
+    if (data.size() < m_num_obs)
         return -1;
     out.clear();
-    out.assign(data.begin(), data.begin() + num_obs);
+    out.assign(data.begin(), data.begin() + m_num_obs);
     return 0;
 }
 int BaseTrack3D::GetObsRotYData(std::vector<float>& obs,float& rot_y)
@@ -89,8 +92,8 @@ int BaseTrack3D::GetObjScale(std::vector<float>& scale)
     if (kf_ == nullptr)
         return -1;    
     scale.push_back(kf_->x_[4]);
-    scale.push_bakc(kf_->x_[5]);
-    scale.push_bakc(kf_->x_[6]);    
+    scale.push_back(kf_->x_[5]);
+    scale.push_back(kf_->x_[6]);    
     return 0;
 }
 int BaseTrack3D::GetObjSpeed(std::vector<float>& speed)
@@ -98,8 +101,8 @@ int BaseTrack3D::GetObjSpeed(std::vector<float>& speed)
     if (kf_ == nullptr)
         return -1;    
     speed.push_back(kf_->x_[7]);
-    speed.push_bakc(kf_->x_[8]);
-    speed.push_bakc(kf_->x_[9]);    
+    speed.push_back(kf_->x_[8]);
+    speed.push_back(kf_->x_[9]);    
     return 0;    
 }
 int BaseTrack3D::GetInputRotY(const std::vector<float>& data, float& rot_y)
@@ -120,8 +123,8 @@ void BaseTrack3D::Predict()
 {
     if (kf_ == nullptr)
         return;
-    std::vector<double> pos;
-    GetPos(pos);
+    std::vector<float> pos;
+    GetObjPos(pos);
     if (m_points.size() >= 5)
     {
         m_points.erase(m_points.begin());
@@ -135,8 +138,8 @@ int BaseTrack3D::GetObjPos(std::vector<float>& pos)
     if (kf_ == nullptr)
         return -1;    
     pos.push_back(kf_->x_[0]);//默认坐标存储在第一、二、三个元素
-    pos.push_bakc(kf_->x_[1]);
-    pos.push_bakc(kf_->x_[2]);    
+    pos.push_back(kf_->x_[1]);
+    pos.push_back(kf_->x_[2]);    
     return 0;
 }
 
@@ -151,22 +154,22 @@ void BaseTrack3D::UpdateDataCheck(const std::vector<float>& data, std::vector<fl
 {
     if (kf_ == nullptr)
         return;
-    if (data.size() < num_obs)
+    if (data.size() < m_num_obs)
     {
         SDK_LOG(SDK_INFO, "UpdateDataCheck data size is small");
         return;
     }
     out.clear();
-    GetInputObsData(data,out);
+    GetInputObsData(data,out);//获取输入的数据中给卡尔曼用的观测数据
     float rot_y = 0;
-    GetObsRotYData(tmp,rot_y);
+    GetObsRotYData(out,rot_y);//从观测数据中取到朝向
     int rot_reliable = IsRotYReliable(data);
     float rot_y_detect = rot_y;//检测到的朝向
     if(rot_reliable = 1)
     {
         if (m_points.size() >= 5 && (abs(m_points[4][0] - m_points[0][0]) > DIST_THRED || abs(m_points[4][1] - m_points[0][1]) > DIST_THRED))
         {
-            float center_rot_y = correct_angle(m_points);
+            float center_rot_y = correct_angle_(m_points);
             m_center_rot_y = center_rot_y;
             m_isCalcRotY = 1;
             //SDK_LOG(SDK_INFO, "center_rot_y = %f,rot_y = %f", center_rot_y,rot_y);
@@ -199,7 +202,7 @@ void BaseTrack3D::UpdateDataCheck(const std::vector<float>& data, std::vector<fl
     {
         if (m_points.size() >= 5 && (abs(m_points[4][0] - m_points[0][0]) > DIST_THRED || abs(m_points[4][1] - m_points[0][1]) > DIST_THRED))
         {
-            float center_rot_y = correct_angle(m_points);
+            float center_rot_y = correct_angle_(m_points);
             m_center_rot_y = center_rot_y;
             m_isCalcRotY = 1;
             rot_y = center_rot_y;
@@ -248,13 +251,12 @@ void BaseTrack3D::UpdateDataCheck(const std::vector<float>& data, std::vector<fl
 
 double BaseTrack3D::CalculateIou(const std::vector<float>& data)
 {
-    if (data.size() < num_obs)
+    if (data.size() < m_num_obs)
     {
         SDK_LOG(SDK_INFO, "CalculateIou data size too small");
         return 0.0f;
     }
-    if (m_obj == nullptr)
-        return 0.0f;
+
     int input_type = GetInputObjType(data);
     int obj_type = GetObjType();
     if (input_type != obj_type)
@@ -264,7 +266,7 @@ double BaseTrack3D::CalculateIou(const std::vector<float>& data)
     std::vector<float> inputScale;
     GetInputScale(data,inputScale);
     float input_rotY = 0;
-    GetInputRotY(input_rotY);
+    GetInputRotY(data,input_rotY);
     std::vector<float> pos;
     GetObjPos(pos);
     std::vector<float> scale;
@@ -317,4 +319,21 @@ float BaseTrack3D::GetRotY(float rot_y)
     while(rot_y_real < 0) rot_y_real += _PI_ * 2;
     while(rot_y_real >= _PI_ * 2) rot_y_real -= _PI_ * 2;
     return rot_y_real;
+}
+
+void BaseTrack3D::MeasureIouData(const std::vector<float>& input, std::vector<float>& out, int& obj_type)
+{
+    if (input.size() != 9)
+    {
+        SDK_LOG(SDK_INFO, "input is not 9");
+        return;
+    }
+    obj_type = input[0];
+    out.push_back(input[1]);
+    out.push_back(input[2]);
+    out.push_back(input[3]);
+    out.push_back(input[5]);
+    out.push_back(input[6]);
+    out.push_back(input[7]);
+    out.push_back(input[4]);
 }
\ No newline at end of file

TrackEx/BaseTrack3D.hpp

@@ -33,7 +33,7 @@ public:
 	/// 构造函数BaseTrack3D
 	BaseTrack3D(unsigned int num_states, unsigned int num_obs);
 	/// 析构函数~BaseTrack3D()
-	~BaseTrack3D() default;    
+	~BaseTrack3D() = default;    
 
     virtual void Init(const std::vector<float>& data);
     virtual void Predict();
@@ -56,11 +56,16 @@ public:
     virtual int GetObjScale(std::vector<float>& scale);
     virtual int GetObjSpeed(std::vector<float>& speed);
     virtual int GetObjRotY(float& rot_y);
+    virtual float GetRotY(float rot_y);
 
+    static void MeasureIouData(const std::vector<float>& input, std::vector<float>& out, int& obj_type);
 
 
 public:
 
+    float m_center_rot_y = 0;//物体通过移动轨迹计算出的朝向
+    int m_isCalcRotY = 0;//是否已经计算出朝向
+    
     std::vector<std::vector<float>> m_points;//保存坐标值
 
     std::vector<int> m_rot_y_diret;///< 每帧检测到是否转向

TrackEx/TrackEx.cpp

@@ -119,3 +119,118 @@ void TrackEx::Predict(float dt)
     kf_->F_(9, 12) = dt;
     BaseTrack3D::Predict();
 }
+
+int TrackEx::GetInputObsData(const std::vector<float>& data,std::vector<float>& out)
+{
+    if (data.size() < m_num_obs)
+        return -1;
+    out.clear();
+    out.assign(data.begin() + 1, data.begin() + 8);
+    return 0;    
+}
+int TrackEx::IsRotYReliable(const std::vector<float>& data)
+{
+    int dataSource = data[8];
+    if(dataSource == 1)
+        return 1;
+    else
+        return 0;
+}
+int TrackEx::GetInputObjType(const std::vector<float>& data)
+{
+    int type = data[0];
+    return type;
+}
+int TrackEx::GetInputPos(const std::vector<float>& data, std::vector<float>& pos)
+{
+    pos.clear();
+    pos.push_back(data[1]);
+    pos.push_back(data[2]);
+    pos.push_back(data[3]);
+    return 0;
+}
+int TrackEx::GetInputScale(const std::vector<float>& data, std::vector<float>& scale)
+{
+    scale.clear();
+    scale.push_back(data[5]);
+    scale.push_back(data[6]);
+    scale.push_back(data[7]);
+    return 0;
+}
+int TrackEx::GetInputRotY(const std::vector<float>& data, float& rot_y)
+{
+    rot_y = data[4];
+    return 0;
+}
+
+int TrackEx::GetObjPos(std::vector<float>& pos)
+{
+    if(kf_ == nullptr)
+        return -1;
+    pos.clear();
+    pos.push_back(kf_->x_[0]);
+    pos.push_back(kf_->x_[1]);
+    pos.push_back(kf_->x_[2]);
+    return 0;
+}
+int TrackEx::GetObjType()
+{
+    if(m_obj == nullptr)
+        return -1;
+    return m_obj->type;
+}
+int TrackEx::GetObjScale(std::vector<float>& scale)
+{
+    if(kf_ == nullptr)
+        return -1;    
+    scale.clear();
+    scale.push_back(kf_->x_[4]);
+    scale.push_back(kf_->x_[5]);
+    scale.push_back(kf_->x_[6]);
+    return 0;
+}
+int TrackEx::GetObjSpeed(std::vector<float>& speed)
+{
+    if (kf_ == nullptr)
+        return -1;    
+    speed.clear();
+    speed.push_back(kf_->x_[7]);
+    speed.push_back(kf_->x_[8]);
+    speed.push_back(kf_->x_[9]);      
+    return 0;
+}
+int TrackEx::GetObjRotY(float& rot_y)
+{
+    if (kf_ == nullptr)
+        return -1;       
+    rot_y = kf_->x_[3];
+    return 0;
+}
+
+int TrackEx::UpdateColorInfo(std::string color)
+{
+    if(color.empty() || color == "unknown" || color == "null" || color == "None")
+        return -1;
+    if(m_colorInfos.find(color) == m_colorInfos.end())
+        m_colorInfos[color] = 0;
+    m_colorInfos[color]++;
+    return 0;
+}
+int TrackEx::GetColorInfo(std::string& color)
+{
+    if(m_colorInfos.empty())
+    {
+        color = "unknown";
+        return -1;
+    }
+    int max = 0;
+    for(auto& iter : m_colorInfos)
+    {
+        if(iter.second > max)
+        {
+            max = iter.second;
+            color = iter.first;
+        }
+    }
+    return 0;    
+}
\ No newline at end of file

TrackEx/TrackEx.hpp

@@ -21,9 +21,20 @@
 #ifndef _TRACK_EX_HPP_
 #define _TRACK_EX_HPP_
 
-#include "BaseTrack3D.h"
+#include "BaseTrack3D.hpp"
 #include <vector>
+#ifdef _USING_DEFINE_MSG_H_
+#include "RosMsg.h"
+#else
+#ifdef _QICHECHENG_
+#include "jfxrosperceiver/det_tracking.h"
+#define jfx_common_msgs jfxrosperceiver
+#else
+#include "jfx_common_msgs/det_tracking.h"
+#endif
 
+using trackOjbPtr = std::shared_ptr< jfx_common_msgs::det_tracking>;
+#endif
 
 /// 类ClassName的注释
 ///
@@ -34,14 +45,34 @@ public:
 	/// 构造函数TrackEx
 	TrackEx();
 	/// 析构函数~TrackEx()
-	~TrackEx() default;
+	~TrackEx() = default;
 
     void Predict(float dt);
 
-    float m_center_rot_y = 0;//物体通过移动轨迹计算出的朝向
-    int m_isCalcRotY = 0;//是否已经计算出朝向
-    std::vector<int> m_rot_y_diret;///< 每帧检测到是否转向
 
+    virtual int GetInputObsData(const std::vector<float>& data,std::vector<float>& out);
+    // virtual int GetObsRotYData(std::vector<float>& obs,float& rot_y);
+    // virtual int SetObsRotYData(std::vector<float>& obs,float rot_y);
+    virtual int IsRotYReliable(const std::vector<float>& data);//获取朝向是否可信，如果是激光雷达检测的就是可信的，如果是相机的就是不可信的，主要用户通过轨迹计算朝向。0是不可信，1是可信
+    virtual int GetInputObjType(const std::vector<float>& data);
+    virtual int GetInputPos(const std::vector<float>& data, std::vector<float>& pos);
+    virtual int GetInputScale(const std::vector<float>& data, std::vector<float>& scale);
+    virtual int GetInputRotY(const std::vector<float>& data, float& rot_y);
+
+    virtual int GetObjPos(std::vector<float>& pos);
+    virtual int GetObjType();
+    virtual int GetObjScale(std::vector<float>& scale);
+    virtual int GetObjSpeed(std::vector<float>& speed);
+    virtual int GetObjRotY(float& rot_y);
+    // virtual float GetRotY(float rot_y);
+
+    int UpdateColorInfo(std::string color);
+    int GetColorInfo(std::string& color);
+
+    trackOjbPtr m_obj = nullptr;    
+
+    std::string m_number;//记录车牌号，出现一次就一直记录
+    std::map<std::string,int> m_colorInfos;///< 记录车辆的颜色信息    
 };
 
 #endif  // _TRACK_EX_HPP_

TrackEx/TrackFunc.cpp

@@ -18,7 +18,7 @@
  * @copyright Copyright ©2022 Beijing JueFx Technology Co.Ltd. All rights reserved.
  **/
 
-
+#include <eigen3/Eigen/Dense>
 #include "TrackFunc.hpp"
 
 float correct_angle_(std::vector<std::vector<float>>& points)