Merge branch 'master' of http://git.corp.roadlinks.cn:2020/oscar/jfx_kalman_filter_src

BaseTracker/BaseTrack.h

@@ -37,8 +37,8 @@ public:
 
     virtual int GetIouData(std::vector<float>& data,int& obj_type);
     
-    virtual int GetIouDataOrder(std::vector<int>& order) = 0;
-    virtual int GetKFDataOrder(std::vector<int>& order) = 0;
+    // virtual int GetIouDataOrder(std::vector<int>& order) = 0;
+    // virtual int GetKFDataOrder(std::vector<int>& order) = 0;
 
     virtual double CalculateIou(const std::vector<float>& data) = 0;
 

TrackEx/BaseTrack3D.cpp

+// Copyright ©2022 Beijing JueFx Technology Co.Ltd. All rights reserved.
+// Copyright ©2022 北京觉非科技有限公司 版权所有 保留一切权利
+
+// This software is furnished under a license and may be used and copied only in accordance with the
+// terms of such license and with the inclusion of the above copyright notice. This software or any
+// other copies thereof may not be provided or otherwise made available to any other person.
+
+// The information in this software is subject to change without notice and should not be
+// constructed as a commitment by Beijing JUEFX Technology Co.Ltd. JUEFX assumes no responsibility
+// for the use or reliability of its software on equipment which is not supported by JUEFX.
+
+/**
+ * @file file_name.cpp
+ * @author Jfx
+ * @brief Jfx cxx template file
+ * @version 0.1
+ * @date 2022-10-24
+ * @copyright Copyright ©2022 Beijing JueFx Technology Co.Ltd. All rights reserved.
+ **/
+
+
+#include "BaseTrack3D.hpp"
+#include "TrackFunc.hpp"
+#include "Component.h"
+
+
+BaseTrack3D::BaseTrack3D(unsigned int num_states, unsigned int num_obs):BaseTrack(num_states,num_obs)
+{
+    //矩阵赋值需要在派生类里实现
+}
+
+void BaseTrack3D::Init(const std::vector<float>& data)
+{
+    //输入的数据数量必须是大于等于观测量的大小，然后观测量必须放在前面，后面可以放入其他的数据
+    if (data.size() < num_obs)
+        return;
+    std::vector<float> tmp;
+    if(GetInputObsData(data,tmp) == 0)
+        BaseTrack::Init(tmp);
+}
+int BaseTrack3D::GetInputObsData(const std::vector<float>& data,std::vector<float>& out)
+{
+    if (data.size() < num_obs)
+        return -1;
+    out.clear();
+    out.assign(data.begin(), data.begin() + num_obs);
+    return 0;
+}
+int BaseTrack3D::GetObsRotYData(std::vector<float>& obs,float& rot_y)
+{
+    rot_y = obs[3];
+    return 0;
+}
+int BaseTrack3D::SetObsRotYData(std::vector<float>& obs,float rot_y)
+{
+    obs[3] = rot_y;
+    return 0;
+}
+int BaseTrack3D::IsRotYReliable(const std::vector<float>& data)
+{
+    return 1;
+}
+int BaseTrack3D::GetInputObjType(const std::vector<float>& data)
+{
+    return 0;
+}
+int BaseTrack3D::GetObjType()
+{
+    return 0;
+}
+int BaseTrack3D::GetInputPos(const std::vector<float>& data, std::vector<float>& pos)
+{
+    pos.clear();
+    pos.push_back(data[0]);
+    pos.push_back(data[1]);
+    pos.push_back(data[2]);
+    return 0;
+}
+int BaseTrack3D::GetInputScale(const std::vector<float>& data, std::vector<float>& scale)
+{
+    scale.clear();
+    scale.push_back(data[4]);
+    scale.push_back(data[5]);
+    scale.push_back(data[6]);
+    return 0;
+}
+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]);    
+    return 0;
+}
+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]);    
+    return 0;    
+}
+int BaseTrack3D::GetInputRotY(const std::vector<float>& data, float& rot_y)
+{
+    rot_y = data[3];
+    return 0;
+}
+int BaseTrack3D::GetObjRotY(float& rot_y)
+{
+    if (kf_ == nullptr)
+        return -1;
+    rot_y = kf_->x_[3];
+    return 0;      
+}
+
+
+void BaseTrack3D::Predict()
+{
+    if (kf_ == nullptr)
+        return;
+    std::vector<double> pos;
+    GetPos(pos);
+    if (m_points.size() >= 5)
+    {
+        m_points.erase(m_points.begin());
+    }
+    m_points.emplace_back(pos);
+    BaseTrack::Predict();
+}
+
+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]);    
+    return 0;
+}
+
+void BaseTrack3D::Update(const std::vector<float>& data)
+{
+    std::vector<float> out;
+    UpdateDataCheck(data, out);
+    BaseTrack::Update(out);
+}
+
+void BaseTrack3D::UpdateDataCheck(const std::vector<float>& data, std::vector<float>& out)
+{
+    if (kf_ == nullptr)
+        return;
+    if (data.size() < num_obs)
+    {
+        SDK_LOG(SDK_INFO, "UpdateDataCheck data size is small");
+        return;
+    }
+    out.clear();
+    GetInputObsData(data,out);
+    float rot_y = 0;
+    GetObsRotYData(tmp,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);
+            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);
+            float rotate = abs(center_rot_y - rot_y);
+            while (rotate > M_PI)
+                rotate -= 2 * M_PI;
+            while (rotate < -M_PI)
+                rotate += 2 * M_PI;
+            if (rotate > M_PI / 2.0f && rotate < M_PI * 3 / 2.0f)//夹角在90到270度，认为角度计算错误，差180度
+            {
+                rot_y += M_PI;
+            }
+        }
+        else if(m_isCalcRotY == 1)
+        {
+            float center_rot_y = m_center_rot_y;
+            //SDK_LOG(SDK_INFO, "center_rot_y = %f,rot_y = %f", center_rot_y,rot_y);
+            float rotate = abs(center_rot_y - rot_y);
+            while (rotate > M_PI)
+                rotate -= 2 * M_PI;
+            while (rotate < -M_PI)
+                rotate += 2 * M_PI;
+            if (rotate > M_PI / 2.0f && rotate < M_PI * 3 / 2.0f)//夹角在90到270度，认为角度计算错误，差180度
+            {
+                rot_y += M_PI;
+            }        
+        }
+    }
+    else
+    {
+        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);
+            m_center_rot_y = center_rot_y;
+            m_isCalcRotY = 1;
+            rot_y = center_rot_y;
+        }
+        else if(m_isCalcRotY == 1)
+        {
+            rot_y = m_center_rot_y;
+        }    
+        rot_y_detect = rot_y;         
+    }
+    float x_angle = kf_->x_[3];
+    float deta = rot_y - x_angle;
+    float deta2 = rot_y + M_PI - x_angle;
+    while (deta >= M_PI)
+    {
+        deta -= M_PI * 2;
+    }
+    while (deta < -M_PI)
+    {
+        deta += M_PI * 2;
+    }
+    while (deta2 >= M_PI)
+    {
+        deta2 -= M_PI * 2;
+    }
+    while (deta2 < -M_PI)
+    {
+        deta2 += M_PI * 2;
+    }
+    float detaT = abs(deta) > abs(deta2) ? deta2:deta;
+    rot_y = x_angle + detaT;
+    SetObsRotYData(out,rot_y);
+
+    //检测朝向是否需要转向
+    float rot_y_correct = rot_y;//修正后的朝向
+    float rotation = rot_y_correct - rot_y_detect;
+    while(rotation < -M_PI) rotation += M_PI * 2;
+    while(rotation >= M_PI) rotation -= M_PI * 2;
+    int isRotation = 0;
+    if(abs(rotation) > M_PI / 2) //钝角说明转向了
+        isRotation = 1;
+    if(m_rot_y_diret.size() >= 10)
+        m_rot_y_diret.erase(m_rot_y_diret.begin());
+    m_rot_y_diret.push_back(isRotation);
+}
+
+double BaseTrack3D::CalculateIou(const std::vector<float>& data)
+{
+    if (data.size() < 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)
+        return 0.0f;
+    std::vector<float> inputPos;
+    GetInputPos(data,inputPos);
+    std::vector<float> inputScale;
+    GetInputScale(data,inputScale);
+    float input_rotY = 0;
+    GetInputRotY(input_rotY);
+    std::vector<float> pos;
+    GetObjPos(pos);
+    std::vector<float> scale;
+    GetObjScale(scale);       
+    std::vector<float> speed;
+    GetObjSpeed(speed);
+    float rot_y = 0;
+    GetRotY(rot_y);
+
+    float v = sqrt(speed[0] * speed[0] + speed[1] * speed[1]);
+
+    cv::RotatedRect rect1 = cv::RotatedRect(cv::Point2f(pos[0], pos[1]), cv::Size2f(scale[0] + v, scale[1]), rot_y*180 / M_PI);
+    cv::RotatedRect rect2 = cv::RotatedRect(cv::Point2f(inputPos[0], inputPos[1]), cv::Size2f(inputScale[0], inputScale[1]), input_rotY*180/M_PI);
+
+    double iou_3d = 0.0f;
+    double area = 0.0f;
+    try
+    {
+        area = calcIntersectionArea(rect1,rect2);
+    }
+    catch(cv::Exception& e)
+    {
+        const char* err_msg = e.what();
+        SDK_LOG(SDK_INFO,"calcIntersectionArea catch error err_msg = %s,rect1 = [%f,%f][%f,%f][%f],rect2 = [%f,%f][%f,%f][%f]",err_msg,
+            rect1.center.x,rect1.center.y,rect1.size.width,rect1.size.height,rect1.angle,
+            rect2.center.x,rect2.center.y,rect2.size.width,rect2.size.height,rect2.angle);
+        area = 0.0f;
+    }
+    if (area > 0)
+    {
+        iou_3d = area / (scale[0] * scale[1] + inputScale[0] * inputScale[1]);
+    }
+    return iou_3d;
+}
+
+float BaseTrack3D::GetRotY(float rot_y)
+{
+    float rot_y_real = rot_y;
+    int size = m_rot_y_diret.size();
+    int count = 0;
+    for(auto iter: m_rot_y_diret)
+    {
+        if(iter == 1)
+            count++;
+    }
+    if(count > size/2)
+    {
+        rot_y_real += _PI_;
+    }
+    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;
+}
\ No newline at end of file

TrackEx/BaseTrack3D.hpp

+// Copyright ©2022 Beijing JueFx Technology Co.Ltd. All rights reserved.
+// Copyright ©2022 北京觉非科技有限公司 版权所有 保留一切权利
+
+// This software is furnished under a license and may be used and copied only in accordance with the
+// terms of such license and with the inclusion of the above copyright notice. This software or any
+// other copies thereof may not be provided or otherwise made available to any other person.
+
+// The information in this software is subject to change without notice and should not be
+// constructed as a commitment by Beijing JUEFX Technology Co.Ltd. JUEFX assumes no responsibility
+// for the use or reliability of its software on equipment which is not supported by JUEFX.
+
+/**
+ * @file file_name.hpp
+ * @author Jfx
+ * @brief Jfx cxx template file
+ * @version 0.1
+ * @date 2022-10-24
+ * @copyright Copyright ©2022 Beijing JueFx Technology Co.Ltd. All rights reserved.
+ **/
+
+#ifndef _BASE_TRACK_3D_HPP_
+#define _BASE_TRACK_3D_HPP_
+
+#include "BaseTrack.h"
+
+
+/// 类ClassName的注释
+///
+/// 类ClassName的详细注释（如果没有详细注释可以连同上一行去除）
+class BaseTrack3D : public BaseTrack
+{
+public:
+	/// 构造函数BaseTrack3D
+	BaseTrack3D(unsigned int num_states, unsigned int num_obs);
+	/// 析构函数~BaseTrack3D()
+	~BaseTrack3D() default;    
+
+    virtual void Init(const std::vector<float>& data);
+    virtual void Predict();
+    virtual void Update(const std::vector<float>& data);
+    virtual void UpdateDataCheck(const std::vector<float>& data, std::vector<float>& out);
+    virtual double CalculateIou(const std::vector<float>& data);
+
+
+    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);
+
+
+
+public:
+
+    std::vector<std::vector<float>> m_points;//保存坐标值
+
+    std::vector<int> m_rot_y_diret;///< 每帧检测到是否转向
+
+};
+
+
+
+#endif  // _BASE_TRACK_3D_HPP_

TrackEx/TrackEx.cpp

+// Copyright ©2022 Beijing JueFx Technology Co.Ltd. All rights reserved.
+// Copyright ©2022 北京觉非科技有限公司 版权所有 保留一切权利
+
+// This software is furnished under a license and may be used and copied only in accordance with the
+// terms of such license and with the inclusion of the above copyright notice. This software or any
+// other copies thereof may not be provided or otherwise made available to any other person.
+
+// The information in this software is subject to change without notice and should not be
+// constructed as a commitment by Beijing JUEFX Technology Co.Ltd. JUEFX assumes no responsibility
+// for the use or reliability of its software on equipment which is not supported by JUEFX.
+
+/**
+ * @file file_name.cpp
+ * @author Jfx
+ * @brief Jfx cxx template file
+ * @version 0.1
+ * @date 2022-10-24
+ * @copyright Copyright ©2022 Beijing JueFx Technology Co.Ltd. All rights reserved.
+ **/
+
+
+#include "TrackEx.hpp"
+#include "TrackFunc.hpp"
+#include <math.h>
+
+
+/// @details 构造函数的详细注释
+TrackEx::TrackEx():BaseTrack3D(13,7)
+{
+    std::shared_ptr<mytracker::KalmanFilter> kf = std::make_shared<mytracker::KalmanFilter>(13, 7);
+    // 初始化状态向量x (10x1)
+    kf->x_.setZero();    
+    // 初始化状态转移矩阵F (13x13)
+    kf->F_ <<
+        //13个状态量：x,y,z,rot,l,w,h,vx,vy,vz,ax,ay,az
+        1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0.5, 0, 0,
+        0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0.5, 0,
+        0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0.5,
+        0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+        0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0,
+        0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0,
+        0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0,
+        0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0,
+        0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0,
+        0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1,
+        0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
+        0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0,
+        0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1;
+    // 初始化状态协方差矩阵P (13x13)        
+    kf->P_.setIdentity();
+    kf->P_ *= 10000;
+    // kf->P_ <<
+    //     10000, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    //     0, 10000, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    //     0, 0, 10000, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    //     0, 0, 0, 10000, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    //     0, 0, 0, 0, 10000, 0, 0, 0, 0, 0, 0, 0, 0,
+    //     0, 0, 0, 0, 0, 10000, 0, 0, 0, 0, 0, 0, 0,
+    //     0, 0, 0, 0, 0, 0, 10000, 0, 0, 0, 0, 0, 0,
+    //     0, 0, 0, 0, 0, 0, 0, 10000, 0, 0, 0, 0, 0,
+    //     0, 0, 0, 0, 0, 0, 0, 0, 10000, 0, 0, 0, 0,
+    //     0, 0, 0, 0, 0, 0, 0, 0, 0, 10000, 0, 0, 0,
+    //     0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10000, 0, 0,
+    //     0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10000, 0,
+    //     0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10000;
+
+    // 初始化观测矩阵H (7x13)
+    kf->H_.setZero();
+    kf->H_.block<7, 7>(0, 0).setIdentity();    
+    // kf->H_ <<
+    //     1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    //     0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    //     0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    //     0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    //     0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0,
+    //     0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0,
+    //     0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0;
+    // 初始化过程噪声协方差矩阵Q (13x13)
+    kf->Q_.setIdentity();
+    kf->Q_ *= 0.001;    
+    // kf->Q_ <<
+    //     0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    //     0, 0.01, 0, 0, 0, 0, 0, 0, 0, 0,
+    //     0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0,
+    //     0, 0, 0, 0.01, 0, 0, 0, 0, 0, 0,
+    //     0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0,
+    //     0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0,
+    //     0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0,
+    //     0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0,
+    //     0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0,
+    //     0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01;
+    // 初始化观测噪声矩阵R (7x7)
+    kf->R_.setIdentity();
+    kf->R_ *= 0.1;    
+    // kf->R_ <<
+    //     1, 0, 0, 0, 0, 0, 0,
+    //     0, 1, 0, 0, 0, 0, 0,
+    //     0, 0, 1, 0, 0, 0, 0,
+    //     0, 0, 0, 1, 0, 0, 0,
+    //     0, 0, 0, 0, 1, 0, 0,
+    //     0, 0, 0, 0, 0, 1, 0,
+    //     0, 0, 0, 0, 0, 0, 1;
+
+    kf_ = kf;    
+}
+
+void TrackEx::Predict(float dt)
+{
+    if (kf_ == nullptr)
+        return;
+    kf_->F_(0, 7) = dt;
+    kf_->F_(0, 10) = 0.5*dt*dt;
+    kf_->F_(1, 8) = dt;
+    kf_->F_(1, 11) = 0.5*dt*dt;
+    kf_->F_(2, 9) = dt;
+    kf_->F_(2, 12) = 0.5*dt*dt;
+    kf_->F_(7, 10) = dt;
+    kf_->F_(8, 11) = dt;
+    kf_->F_(9, 12) = dt;
+    BaseTrack3D::Predict();
+}

TrackEx/TrackEx.hpp

+// Copyright ©2022 Beijing JueFx Technology Co.Ltd. All rights reserved.
+// Copyright ©2022 北京觉非科技有限公司 版权所有 保留一切权利
+
+// This software is furnished under a license and may be used and copied only in accordance with the
+// terms of such license and with the inclusion of the above copyright notice. This software or any
+// other copies thereof may not be provided or otherwise made available to any other person.
+
+// The information in this software is subject to change without notice and should not be
+// constructed as a commitment by Beijing JUEFX Technology Co.Ltd. JUEFX assumes no responsibility
+// for the use or reliability of its software on equipment which is not supported by JUEFX.
+
+/**
+ * @file file_name.hpp
+ * @author Jfx
+ * @brief Jfx cxx template file
+ * @version 0.1
+ * @date 2022-10-24
+ * @copyright Copyright ©2022 Beijing JueFx Technology Co.Ltd. All rights reserved.
+ **/
+
+#ifndef _TRACK_EX_HPP_
+#define _TRACK_EX_HPP_
+
+#include "BaseTrack3D.h"
+#include <vector>
+
+
+/// 类ClassName的注释
+///
+/// 类ClassName的详细注释（如果没有详细注释可以连同上一行去除）
+class TrackEx : public BaseTrack3D
+{
+public:
+	/// 构造函数TrackEx
+	TrackEx();
+	/// 析构函数~TrackEx()
+	~TrackEx() default;
+
+    void Predict(float dt);
+
+    float m_center_rot_y = 0;//物体通过移动轨迹计算出的朝向
+    int m_isCalcRotY = 0;//是否已经计算出朝向
+    std::vector<int> m_rot_y_diret;///< 每帧检测到是否转向
+
+};
+
+#endif  // _TRACK_EX_HPP_

TrackEx/TrackFunc.cpp

+// Copyright ©2022 Beijing JueFx Technology Co.Ltd. All rights reserved.
+// Copyright ©2022 北京觉非科技有限公司 版权所有 保留一切权利
+
+// This software is furnished under a license and may be used and copied only in accordance with the
+// terms of such license and with the inclusion of the above copyright notice. This software or any
+// other copies thereof may not be provided or otherwise made available to any other person.
+
+// The information in this software is subject to change without notice and should not be
+// constructed as a commitment by Beijing JUEFX Technology Co.Ltd. JUEFX assumes no responsibility
+// for the use or reliability of its software on equipment which is not supported by JUEFX.
+
+/**
+ * @file file_name.cpp
+ * @author Jfx
+ * @brief Jfx cxx template file
+ * @version 0.1
+ * @date 2022-10-24
+ * @copyright Copyright ©2022 Beijing JueFx Technology Co.Ltd. All rights reserved.
+ **/
+
+
+#include "TrackFunc.hpp"
+
+float correct_angle_(std::vector<std::vector<float>>& points)
+{
+    if (points.size() < 5)
+        return 0;
+    Eigen::MatrixXd X(5, 2); // 输入直线 y=x 上的三个点 
+    // 坐标 (1, 1), (2, 2), (3, 3)
+    for (int i = 0; i < 5; i++)
+    {
+        X(i, 0) = points[i][0];
+        X(i, 1) = points[i][1];
+    }
+    Eigen::MatrixXd X_copy = X; // 拷贝副本，因为X取均值后会被改变
+    Eigen::MatrixXd C(2, 2); // 方差
+    Eigen::MatrixXd vec, val;
+    // 坐标去均值
+    Eigen::RowVectorXd meanvecRow = X.colwise().mean(); // 求每一列的均值，相当于在向量的每个维度上求均值
+    X.rowwise() -= meanvecRow; // 样本减去各自维度的均值
+    //计算协方差矩阵C = X^t*X / (n-1); 
+    // 其中，n是一个向量的维度，这里向量只有x,y2个维度
+    C = X.adjoint() * X;
+    C = C.array() / (X.rows() - 1);
+    // 计算特征值和特征向量
+    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eig(C); // 产生的vec和val按照特征值升序排列
+    vec = eig.eigenvectors();
+    val = eig.eigenvalues();
+    //SDK_LOG(SDK_INFO, "vec = [%f,%f,%f,%f]", vec(0, 0), vec(0, 1), vec(1, 0), vec(1, 1));
+    //SDK_LOG(SDK_INFO, "val = [%f,%f]", val(0), val(1));
+    // 打印
+    //cout << "X_copy: " << endl << X_copy
+    Eigen::Vector2d dX(points[4][0] - points[0][0], points[4][1] - points[0][1]);
+    Eigen::Vector2d orientation(vec(1, 0), vec(1, 1));
+    double alpha = dX.transpose() * orientation;
+    if (alpha < 0)
+        orientation = -orientation;
+    //SDK_LOG(SDK_INFO, "correct_angle alpha = %f, orientation[0] = %f, orientation[1] = %f", alpha, orientation[0], orientation[1]);
+    float center_rot_y = atan2(orientation[1], orientation[0]);
+    return center_rot_y;
+}
\ No newline at end of file

TrackEx/TrackFunc.hpp

+// Copyright ©2022 Beijing JueFx Technology Co.Ltd. All rights reserved.
+// Copyright ©2022 北京觉非科技有限公司 版权所有 保留一切权利
+
+// This software is furnished under a license and may be used and copied only in accordance with the
+// terms of such license and with the inclusion of the above copyright notice. This software or any
+// other copies thereof may not be provided or otherwise made available to any other person.
+
+// The information in this software is subject to change without notice and should not be
+// constructed as a commitment by Beijing JUEFX Technology Co.Ltd. JUEFX assumes no responsibility
+// for the use or reliability of its software on equipment which is not supported by JUEFX.
+
+/**
+ * @file file_name.hpp
+ * @author Jfx
+ * @brief Jfx cxx template file
+ * @version 0.1
+ * @date 2022-10-24
+ * @copyright Copyright ©2022 Beijing JueFx Technology Co.Ltd. All rights reserved.
+ **/
+
+#ifndef _TRACK_FUNCTION_HPP_
+#define _TRACK_FUNCTION_HPP_
+
+#include <vector>
+
+
+float correct_angle_(std::vector<std::vector<float>>& points);
+
+
+
+
+#endif  // _TRACK_FUNCTION_HPP_