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

BaseTracker/BaseTracker.h

 #pragma once
 
 
+#include <chrono>
 #include <iostream>
 //#include <eigen3/Eigen/Dense>
 //#include "kalman_filter.h"

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)

TrackingRosAbs.h

+#pragma once
+
+#include <vector>
+#include "BaseTracker.h"
+#include "BaseTrackerEx.h"
+#include "Track3D.h"
+#include "TrackEx.hpp"
+#ifdef _QICHECHENG_
+#include "jfxrosperceiver/det_tracking_array.h"
+#define jfx_common_msgs jfxrosperceiver
+#else
+#include "jfx_common_msgs/det_tracking_array.h"
+#endif
+#include "SafeQueue.hpp"
+//#include "TrackingMsg.h"
+#include "ros/ros.h"
+#include "coordinate_convert.h"
+#include <yaml-cpp/yaml.h>
+
+using objTrackListPtr = std::shared_ptr<jfx_common_msgs::det_tracking_array>;
+
+
+class TrackingRos
+{
+private:
+    ros::NodeHandle private_nh;
+public:
+    // Constructor
+    TrackingRos() {}
+    ~TrackingRos();
+
+    void TrackingCallBackFunc(const jfx_common_msgs::det_tracking_arrayConstPtr& msg);//接受ros发过来的消息
+
+    void Init(ros::NodeHandle& nh);
+
+    void ThreadTrackingProcess();
+    void ThreadTrackingProcessEx();
+
+
+public:
+
+    SafeQueue<objTrackListPtr> m_objsQueue;
+
+    BaseTracker<Track3D> m_tracker;
+    BaseTrackerEx<TrackEx> m_trackerEx;
+
+    bool m_isTrackingRun = false;//记录是否在运行
+    std::thread m_trackingThread;//运行事件线程
+    ros::Publisher m_pub;//发送所有物体信息的publisher
+    ros::Publisher m_pubCloud;//发送点云数据
+    ros::Publisher m_pubBoundingBoxes;//发送3D框信息
+    ros::Publisher m_pubMarkerArray;//发送marker框信息
+    ros::Publisher m_pubMarkerArrow;//发送marker框信息
+    ros::Subscriber m_subFusionRes;
+#ifdef _SEND_ABUZHABI_
+    ros::Publisher m_pubAbuzhabi;//阿布扎比发送的数据
+#endif
+
+    std::vector<std::vector<double>> m_trans;
+    std::vector<std::vector<double>> m_kitti2origin;
+#ifdef _ABUZHABI_
+    std::vector<double> m_UTM_station;
+    std::vector<double> m_wgs84_station; 
+    std::vector<std::vector<double>> m_trans2station;
+#endif
+
+    int m_frameNum = 0;
+
+    float m_lidar_x_angle = 0.0f;//修改rot_y的参数，从配置里读取
+
+    std::string m_root_dir;//存储统一的根目录路径
+    std::string m_nodeName;//点位信息，现在就是10-1，1-1等信息
+    YAML::Node m_config;//配置的yaml文件
+    int mark_type_text = 0;//0-type,1-object_id,2-object-type,3-text...
+
+    double m_cross_center_lon = 0;
+    double m_cross_center_lat = 0;
+    double m_clear_distance = 0;
+};

jfx_tracking_abs.cpp

+#include <ros/ros.h>
+//#include "image_synchronizer.h"
+//#include <yaml-cpp/yaml.h>
+
+//#include "cv_config.h"
+#include "Track3D.h"
+#include "Track2D.h"
+#include "BaseTracker.h"
+#include "Iou.h"
+#include "jfx_log.h"
+#include "LogBase.h"
+#include "TrackingRosAbs.h"
+
+//using namespace jfx_vision;
+
+//std::unique_ptr<VisionConfig>  jfx_vision::g_visionConfig;
+
+int main(int argc, char*argv[]) {
+    //ROS_INFO("START JFX_VISION_NODE");
+
+
+    InitLog(argv[0]);
+
+    SDK_LOG(SDK_INFO, "main function");
+
+
+    ros::init(argc, argv, "jfx_tracking");
+    ros::NodeHandle nh("~");
+
+
+    //BaseTracker<Track3D> tracker3d;
+    //BaseTracker<Track2D> tracker2d;
+
+    //std::map<uint64_t, std::shared_ptr<Track3D> >& a = tracker3d.GetStates();
+    //std::map<uint64_t, std::shared_ptr<Track2D> >& b = tracker2d.GetStates();
+    
+
+    //std::vector<float> truth_poses{ -0.4875, -0.798913, -1.125, 0, 0, -1.27409, 0.240477, 0.235315, 0.375 };
+    //std::vector<float> truth_poses{ -0.1875, -0.798913, -1.0, 0, 0, -1.27409, 0.240477, 0.235315, 0.375 };
+    ////std::vector<float> landmark_poses{ -0.506616, -0.796303, -1.20499, 0, 0, -0.345443, 0.22,  0.22, 0.4 };
+    //std::vector<float> landmark_poses{ -0.4875, -0.798913, -1.125, 0, 0, -1.27409, 0.240477, 0.47063, 0.375 };
+    //double iou_3d = CuboidIoU(truth_poses, landmark_poses);
+    //std::cout<<"the iou of two cuboid is: "<< iou_3d << std::endl;
+
+    //std::string config_yaml = "/data/catkin_ws_xishan/src/rosrv2x/jfx_rvconfig/param/vision_config.yaml";
+    //nh.param<std::string>("vision_config_path", config_yaml, "");
+
+    //g_visionConfig.reset(new VisionConfig(config_yaml));
+
+    //int cam_size = g_visionConfig->globalConf.camera_size;
+    //ROS_INFO("cam_size:%d", cam_size);
+
+    // ROS_INFO("CAM[0] roi: h %d,w %d,x %d,y %d", g_visionConfig->cameraParamConfigs_map[0].roiRect.height, g_visionConfig->cameraParamConfigs_map[0].roiRect.width, 
+    // g_visionConfig->cameraParamConfigs_map[0].roiRect.x, g_visionConfig->cameraParamConfigs_map[0].roiRect.y);
+
+    //MultiCamImageSynchronizer<sensor_msgs::Image> sync(nh, cam_size); 
+
+    TrackingRos jfx_tracking;
+    jfx_tracking.Init(nh);
+    // ros::Subscriber track_sub = nh.subscribe<jfx_common_msgs::det_tracking_array>("/fusion_res", 1000, &TrackingRos::TrackingCallBackFunc, &jfx_tracking);
+    //rospy.Subscriber("/detection/lidar_detector/objects", det_tracking_array, callback)
+
+    ros::spin();
+
+    return 0;
+}