debug type track ok

CMakeLists.txt

 cmake_minimum_required(VERSION 3.0.2)
-project(jfx_vision)
+project(jfx_vision_nx)
 
 ## Compile as C++11, supported in ROS Kinetic and newer
 add_compile_options(-std=c++11)
@@ -7,8 +7,8 @@ set(CMAKE_BUILD_TYPE "Release")
 set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -O3")
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3")
 
-add_subdirectory(yolov5)
-add_subdirectory(track)
+#add_subdirectory(yolov5)
+#add_subdirectory(track)
 
 find_package(OpenCV 3.2.0 REQUIRED)
 find_package(CUDA REQUIRED)
@@ -96,6 +96,7 @@ file(GLOB_RECURSE TRACKING_NEW_FILES
         ${PROJECT_SOURCE_DIR}/src/BaseTracker/Track2D.cpp
         ${PROJECT_SOURCE_DIR}/src/BaseTracker/BaseTrack.cpp
         ${PROJECT_SOURCE_DIR}/src/BaseTracker/Iou.cpp
+        ${PROJECT_SOURCE_DIR}/src/BaseTracker/kalman_filter_f.cpp
 )
 
 add_executable(${PROJECT_NAME}_node
@@ -114,18 +115,6 @@ target_link_libraries(${PROJECT_NAME}_node
         yaml-cpp
         )
 
-add_executable(cv_test
-        src/test.cpp
-        ${COORDINATE_SRC_FILES}
-        ${TRACKER_SRC_FILES}
-)
-
-target_link_libraries(cv_test
-        ${catkin_LIBRARIES}
-        ${OpenCV_LIBS}
-        yaml-cpp
-        )
-
 #############
 ## Install ##
 #############

launch/vision.launch

 <launch>
    <arg name="vision_config_path" default="$(find jfx_rvconfig)/param/vision_config.yaml" />
-   <node pkg="jfx_vision" type="jfx_vision_node" name="jfx_vision_node" output="screen" respawn="true" >
+   <node pkg="jfx_vision_nx" type="jfx_vision_nx_node" name="jfx_vision_nx_node" output="screen" respawn="true" >
        <param name="vision_config_path" value="$(arg vision_config_path)" />
    </node>
 </launch>

package.xml

 <?xml version="1.0"?>
 <package format="2">
-  <name>jfx_vision</name>
+  <name>jfx_vision_nx</name>
   <version>1.0.0</version>
-  <description>The jfx_vision package</description>
+  <description>The jfx_vision_nx package</description>
 
   <!-- One maintainer tag required, multiple allowed, one person per tag -->
   <!-- Example:  -->

src/BaseTracker/Track2D.cpp

@@ -64,7 +64,7 @@ double Track2D::CalculateIou(const std::vector<float>& data)
         return 0;
     int input_type = data[0];
     int obj_type = m_obj.type;
-    if (input_type != obj_type)
+    if (!isSameType(input_type, obj_type))
         return 0.0f;
     std::vector<float> states;
     GetStateData(states);
@@ -91,3 +91,15 @@ double Track2D::CalculateIou(const std::vector<float>& data)
     auto iou = intersection_area / union_area;
 	return iou;
 }
+
+bool Track2D::isSameType(int type1, int type2) {
+    return isVehicle(type1) == isVehicle(type2);
+}
+
+bool Track2D::isVehicle(int type) {
+    if (type == 0 || type == 2 || type == 3) {
+        return false;
+    }
+
+    return true;
+}
\ No newline at end of file

src/BaseTracker/Track2D.h

@@ -30,6 +30,9 @@ public:
 
     static void MeasureIouData(const std::vector<float>& input, std::vector<float>& out, int& obj_type) {}
 
+    virtual bool isSameType(int type1, int type2);
+
+    virtual bool isVehicle(int type);
 
     TrackObj m_obj;
 };

src/detector.h

@@ -291,6 +291,10 @@ private:
                     ROS_INFO("Detector queue size=%ld, drop frame", _queue.size());
                     continue;
                 }
+                if (_queue.size() > 100) {
+                    _queue.clear();
+                    ROS_WARN("Detector queue size>100, clear...");
+                }
             }
 
             Process(framePtr);

src/track.h

@@ -138,6 +138,7 @@ public:
             for (int i = 0; i < itr->second->results.size(); i++) {
                 auto& item = itr->second->results[i];
 
+                /*
                 if (isVehicle(item.num_class)) {
                     if (item.prob < 0.35) {
                         continue;
@@ -146,7 +147,7 @@ public:
                     if (item.prob < 0.2) {
                         continue;
                     }
-                }
+                }*/
 
                 rect.x = item.location[0];
                 rect.y = item.location[1];
@@ -197,10 +198,10 @@ public:
 
         BaseTracker<Track2D> _tracker;
         _tracker.SetGPU(0);
-        _tracker.SetIouThreshold(0.1);
-        _tracker.SetMaxCoastCycles(3);
-        _tracker.SetValidUpdateCount(2);
-        float high_low_rate = 0.4;
+        _tracker.SetIouThreshold(0.3);
+        _tracker.SetMaxCoastCycles(5);
+        _tracker.SetValidUpdateCount(3);
+        float high_low_rate = 0.35;
 
         initTransUV(trackResPtr->transUV, cameraIndex);
         initCoordCVT(trackResPtr->coordinatCvt, cameraIndex);
@@ -221,6 +222,10 @@ public:
                     trackResPtr->toTrackItemList.pop_front();
                     ROS_INFO("Tracker queue size=%ld, drop frame", trackResPtr->toTrackItemList.size());
                 }
+                if (trackResPtr->toTrackItemList.size() > 100) {
+                    trackResPtr->toTrackItemList.clear();
+                    ROS_WARN("Tracker queue size>100, clear...");
+                }
             }
             trackResPtr->trackItemMutex.unlock();
 
@@ -275,6 +280,7 @@ public:
                         info = trackItem.detections_info[indexH[updateHId[iter.first]]];   //从检测结果里得到初始高分obj
                     if (updateLId.find(iter.first) != updateLId.end())
                         info = trackItem.detections_info[indexL[updateLId[iter.first]]];   //从检测结果里得到初始低分obj
+                    iter.second->m_obj = info; 
                 }
                 else
                 {

src/vision_node.cpp

@@ -17,8 +17,8 @@ int main(int argc, char*argv[]) {
     string pkgName = "jfx_rvconfig";
     string pkgPath = ros::package::getPath(pkgName);
 
-    std::string config_yaml = pkgPath + "/param/vision_config.yaml";
-    nh.param<std::string>("vision_config_path", config_yaml, pkgPath + "/param/vision_config.yaml");
+    std::string config_yaml = "/workspace/catkin_ws/src/rosrv2x/jfx_rvconfig/param/vision_config.yaml";
+    nh.param<std::string>("vision_config_path", config_yaml, "/workspace/catkin_ws/src/rosrv2x/jfx_rvconfig/param/vision_config.yaml");
 
     ROS_INFO("use config file: %s", config_yaml.c_str());