update

BaseTracker/BaseTrack.cpp

@@ -50,6 +50,34 @@ void BaseTrack::Update(const std::vector<float>& data)
         observation(i) = data[i];
     kf_->Update(observation);
 }
+void BaseTrack::UpdateTryConfirm(const std::vector<float>& data)
+{
+    if (kf_ == nullptr)
+        return;
+    // get measurement update, reset coast cycle count
+    coast_cycles_ = 0;
+    // accumulate hit streak count
+    hit_streak_++;
+    m_update_count++;
+
+    // observation - center_x, center_y, area, ratio
+    int size = data.size();
+    Eigen::VectorXf observation = Eigen::VectorXf::Zero(size);
+    for (int i = 0; i < data.size(); i++)
+        observation(i) = data[i];
+    kf_->UpdateTry(observation);
+    std::vector<float> _x,_x_update;
+    for (int i = 0; i < m_num_states; i++)
+        _x.push_back(kf_->x_[i]); 
+    for (int i = 0; i < m_num_states; i++)
+        _x_update.push_back(kf_->x_update_[i]);
+    kf_->UpdateConfirm(IsUpdateConfirm(_x,_x_update));
+}
+bool BaseTrack::IsUpdateConfirm(const std::vector<float>& _x,const std::vector<float>& _x_update)
+{
+    return true;
+}
+
 void BaseTrack::UpdateDataCheck(const std::vector<float>& data, std::vector<float>& out)
 {
 

BaseTracker/BaseTrack.h

@@ -30,6 +30,10 @@ public:
     virtual bool IsLost();//数据是否丢失，如果不更新就会丢失
     virtual bool IsValid();//数据是否有效
     virtual bool IsValid(int& updateNum);//数据是否有效，返回更新次数
+    virtual void UpdateTryConfirm(const std::vector<float>& data);
+    virtual bool IsUpdateConfirm(const std::vector<float>& _x,const std::vector<float>& _x_update);
+
+
 
     virtual int GetIouData(std::vector<float>& data,int& obj_type);
     

Tracking/TrackingObj.cpp

@@ -48,7 +48,8 @@ namespace juefx_tracking
         m_tracker.SetValues(values);
         if(m_config["update_invalid_num"])
             m_update_invalid_num = m_config["update_invalid_num"].as<int32_t>();
-
+        if(m_config["speed_step"])
+            m_speed_step = m_config["speed_step"].as<float>();
         if (config["TRANS"])
             m_trans = config["TRANS"].as<std::vector<std::vector<double>>>();
         else

Tracking/TrackingObj.h

@@ -49,6 +49,7 @@ namespace juefx_tracking
         YAML::Node m_config;
         TrackingProcessCallback m_cb= nullptr;
         int m_update_invalid_num = 10;//开始产生的时候速度不可信的帧数
+        float m_speed_step = 1;//每帧速度变化最大值。
     };
 
 

ros2/RosMsg.h

@@ -17,6 +17,7 @@ struct TrackStructObj
     int dataSource;
     int cameraId;
     int trackingId;
+    uint64_t id;
     std::shared_ptr<autoware_auto_perception_msgs::msg::TrackedObject>  obj;
 };
 

ros2/Track3DEx.cpp

 
+#include <rclcpp/rclcpp.hpp>
 #include "Track3DEx.h"
 #include <eigen3/Eigen/Dense>
 #include "Iou.h"
@@ -38,6 +39,55 @@ void Track3DEx::Predict()
     }
     Track3D::Predict();
 }
+void Track3DEx::Update(const std::vector<float>& data)
+{
+    std::vector<float> out;
+    UpdateDataCheck(data, out);
+    BaseTrack::UpdateTryConfirm(out);
+}
+bool Track3DEx::IsUpdateConfirm(const std::vector<float>& _x,const std::vector<float>& _x_update)
+{
+    if(_x.size() != 10 || _x_update.size() != 10)
+        return false;
+    if(_x[0] > 80)//物体在80外，不做逻辑。
+        return true;
+    if(m_speedXLists.size() < 10)
+    {
+        m_speedXLists.push_back(_x_update[7]);
+        m_speedYLists.push_back(_x_update[8]);
+        return true;
+    }
+    float totelX = 0,totelY = 0;
+    for(int i = 0; i < m_speedXLists.size(); i++)
+    {
+        totelX += m_speedXLists[i];
+        totelY += m_speedYLists[i];
+    }
+    float averageX = totelX/m_speedXLists.size();
+    float averageY = totelY/m_speedYLists.size();
+    float x_u = _x_update[7];
+    float y_u = _x_update[8];
+    if(fabs(averageX - x_u) < m_speedStep*2 && fabs(averageY - y_u) < m_speedStep*2)
+    {
+        m_speedXLists.push_back(_x_update[7]);
+        m_speedYLists.push_back(_x_update[8]);
+    }
+    if(m_speedXLists.size() > 10)
+        m_speedXLists.erase(m_speedXLists.begin());
+    if(m_speedYLists.size() > 10)
+        m_speedYLists.erase(m_speedYLists.begin());                
+    if(fabs(averageX - x_u) < m_speedStep && fabs(averageY - y_u) < m_speedStep)
+        return true;
+    else
+    {
+        if(m_obj)
+            RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "IsUpdateConfirm id = %d, _x = [%f,%f,%f][%f,%f], _x_update = [%f,%f,%f][%f,%f],averageX = %f,averageY = %f",
+                m_obj->id,_x[0],_x[1],_x[2],_x[7],_x[8],_x_update[0],_x_update[1],_x_update[2],_x_update[7],_x_update[8],averageX,averageY);
+        return false;
+    }
+
+    return true;
+}
 void Track3DEx::UpdateDataCheck(const std::vector<float>& data, std::vector<float>& out)
 {
     if (kf_ == nullptr)
@@ -156,4 +206,32 @@ double Track3DEx::CalculateIou(const std::vector<float>& data)
         }
     }
     return iou_3d;
+}
+void Track3DEx::SetSpeedStep(float step)
+{
+    m_speedStep = step/10;//step是每帧之间速度变动值，但到了算法需要除以10.
+}
+void Track3DEx::GetSpeed(float& x, float& y)//获取速度值
+{
+    if(m_obj == nullptr)
+        return;
+    if(m_obj->dataSource < 10)
+    {
+        float totelX = 0,totelY = 0;
+        for(int i = 0; i < m_speedXLists.size(); i++)
+        {
+            totelX += m_speedXLists[i];
+            totelY += m_speedYLists[i];
+        }
+        x = totelX/m_speedXLists.size()*10;
+        y = totelY/m_speedYLists.size()*10;    
+    }
+    else
+    {
+        if(m_obj->obj)
+        {
+            x = m_obj->obj->kinematics.twist_with_covariance.twist.linear.x;
+            y = m_obj->obj->kinematics.twist_with_covariance.twist.linear.y;
+        }
+    }
 }
\ No newline at end of file

ros2/Track3DEx.h

@@ -12,9 +12,19 @@ public:
 
     virtual void Init(const std::vector<float>& data);
     virtual void Predict();
+    virtual void Update(const std::vector<float>& data);
+    virtual bool IsUpdateConfirm(const std::vector<float>& _x,const std::vector<float>& _x_update);
 
     virtual void UpdateDataCheck(const std::vector<float>& data, std::vector<float>& out);//对于输入数据进行修正
 
     virtual double CalculateIou(const std::vector<float>& data);
 
+    void SetSpeedStep(float step);
+    float m_speedStep = 0.1f;//速度跳变的最大值
+
+    void GetSpeed(float& x, float& y);//获取速度值
+
+    std::vector<float> m_speedXLists;//记录之前的速度值x
+    std::vector<float> m_speedYLists;//记录之前的速度值y
+
 };
\ No newline at end of file

ros2/TrackingRos2.cpp

@@ -238,6 +238,7 @@ void TrackingRos2::TrackingPorcess(objTrackListPtr objsPtr) {
         else
         {
             obj.object_id = generate_uuid();
+            iter.second->SetSpeedStep(m_tracking.m_speed_step);
             if(input_obj.data_source > 1 && input_obj.camera_id > 0 && input_obj.tracking_id > 0)//这个是相机的检测结果
             {
                 if(m_yoloTrackingObj.find(input_obj.tracking_id) == m_yoloTrackingObj.end())
@@ -294,6 +295,7 @@ void TrackingRos2::TrackingPorcess(objTrackListPtr objsPtr) {
         strObj.cameraId = input_obj.camera_id == 0 ? camera_id: input_obj.camera_id;
         strObj.trackingId = input_obj.tracking_id == 0 ? tracking_id: input_obj.tracking_id;
         strObj.obj = std::make_shared<autoware_auto_perception_msgs::msg::TrackedObject>(obj);
+        strObj.id = iter.first;
         iter.second->m_obj = std::make_shared<TrackStructObj>(strObj);  //不仅状态变量还有类型时间戳的m_obj数据更新（Tracker3D.h的智能指针）
     }
     else
@@ -339,8 +341,11 @@ void TrackingRos2::TrackingPorcess(objTrackListPtr objsPtr) {
         obj.existence_probability = 0.0f;
     geometry_msgs::msg::Vector3& linear =
         obj.kinematics.twist_with_covariance.twist.linear;  
-    linear.x *= 10;  
-    linear.y *= 10;  
+    float speed_x = linear.x*10;
+    float speed_y = linear.y*10; 
+    iter.second->GetSpeed(speed_x,speed_y);
+    linear.x = speed_x;
+    linear.y = speed_y;
     geometry_msgs::msg::Point& poss =
         obj.kinematics.pose_with_covariance.pose.position;    
     if (is_need_send == 1 && orient.y < 2 && poss.y > -15 && poss.y < 15) {
@@ -411,11 +416,13 @@ void TrackingRos2::TrackingPorcess(objTrackListPtr objsPtr) {
         markerText.color.b = 0.0f;
         markerText.color.a = 1.0;
         char str[512] = {};
-        sprintf(str, "%d:%.2f",iter.first,linear.x);
+        sprintf(str, "%lld:%.2f",iter.first,linear.x);
         // sprintf(str, "%.1f:%d:%d",cof,iter.first,obj.classification[0].label);
         // sprintf(str, "%.2f:%d:%.2f:%.2f",cof,obj.classification[0].label,poss.x,linear.x);
         markerText.text = str;        
-        makerTextArray.markers.emplace_back(markerText);        
+        makerTextArray.markers.emplace_back(markerText); 
+        RCLCPP_INFO(this->get_logger(),"tracking obj info id = %lld, label = %d,cof = %.2f,pos = [%.2f,%.2f,%.2f],linear = [%.2f,%.2f]",
+            iter.first,obj.classification[0].label,cof,poss.x,poss.y,poss.z,linear.x,linear.y);       
         sendObjs.objects.emplace_back(obj);
       }
     }

tracker/kalman_filter.cpp

@@ -96,6 +96,49 @@ void KalmanFilter::Update(const Eigen::VectorXf& z) {
     // Optimal gain
     P_ = (I - K * H_) * P_predict_;
 }
+void KalmanFilter::UpdateTry(const Eigen::VectorXf &z)
+{
+    Eigen::VectorXf z_predict = PredictionToObservation(x_predict_);
+
+    // y - innovation, z - real observation, z_predict - predicted observation
+    Eigen::VectorXf y = z - z_predict;
+
+    Eigen::MatrixXf Ht = H_.transpose();
+
+    // S - innovation covariance
+    Eigen::MatrixXf S = H_ * P_predict_ * Ht + R_;
+
+    // NIS_ = y.transpose() * S.inverse() * y;   
+
+    Eigen::MatrixXf K = P_predict_ * Ht * S.inverse();
+
+    // Updated state estimation
+    x_update_ = x_predict_ + K * y;
+    K_ = K;
+
+    // Eigen::MatrixXf I = Eigen::MatrixXf::Identity(num_states_, num_states_);
+    // // Joseph form
+    // //P_ = (I - K * H_) * P_predict_ * (I - K * H_).transpose() + K * R_ * K.transpose();
+    // // Optimal gain
+    // P_ = (I - K * H_) * P_predict_;     
+}
+void KalmanFilter::UpdateConfirm(bool confirm)
+{
+    if(confirm)
+    {
+        x_ = x_update_;
+        Eigen::MatrixXf I = Eigen::MatrixXf::Identity(num_states_, num_states_);
+        // Joseph form
+        //P_ = (I - K * H_) * P_predict_ * (I - K * H_).transpose() + K * R_ * K.transpose();
+        // Optimal gain
+        P_ = (I - K_ * H_) * P_predict_;        
+    }
+    // else
+    // {
+    //     x_ = x_predict_;
+    //     P_ = P_predict_;        
+    // }
+}
 
 
 float KalmanFilter::CalculateLogLikelihood(const Eigen::VectorXf& y, const Eigen::MatrixXf& S) {

tracker/kalman_filter.h

@@ -44,6 +44,13 @@ public:
      * @param z The measurement at k+1
      */
     virtual void Update(const Eigen::VectorXf &z);
+    /**
+     * try to updates the state by using Extended Kalman Filter equations
+     * @param z The measurement at k+1
+     */    
+    virtual void UpdateTry(const Eigen::VectorXf &z);
+
+    virtual void UpdateConfirm(bool confirm);
 
     /**
      * Calculate marginal log-likelihood to evaluate different parameter choices
@@ -68,6 +75,9 @@ public:
     // covariance matrix of observation noise
     Eigen::MatrixXf R_;
 
+    Eigen::VectorXf x_update_;
+    Eigen::MatrixXf K_;
+
     unsigned int num_states_, num_obs_;
 
     float log_likelihood_delta_;