stone-tracker-filter

src/camera_bev_infer/configs/CameraBevParam.yaml

-rgb_sub_topic_name: /front_rgb/image
+rgb_sub_topic_name: /sensor/camera/front_middle/image/bgr
 pointcloud_sub_topic_name: /total_calib_pointcloud
 detection_pub_topic_name: /perception/camera/obstacle
 # static_gridmap_pub_topic_name: /perception/camera/bev_static_gridmap
@@ -12,21 +12,17 @@ device: 0
 period_time: 5
 vis_res: true
 vis_frame_id: Lidar
-vis_project: false
-# ["pedestrian", "two_wheel", "car", "truck", "construction_machine", "fence", "stone", "dust", "cone"]
+vis_project: true
+# ["pedestrian", "vehicle", "stone"]
 pub_classes: ["stone", "cone"]
-obstacle_height_threshold: 25 # cm
+obstacle_height_threshold: 5 # cm
 camera_intrinsic: [1346.61241105747, 0, 951.014999441565,
                     0, 1344.43795511862, 530.562496955385,
                     0, 0, 1]
 distortion_coefficients: [-0.449196898930979, 0.253956763950171, -0.00139403693782871, -0.00111424348026907, -0.0870171627456232]
-# camera2lidar_extrinsic:  [-0.0423904, -0.999009, 0.0135863, 0.0656235, 
-#                           0.0313214, -0.0149206, -0.999398, -0.167858, 
-#                           0.99861, -0.0419393, 0.0319228, -0.0162872, 
-#                           0, 0, 0, 1]
-camera2lidar_extrinsic:  [  0.0029218, -0.9999551,  0.0090201, -0.339,
-                            0.0170034, -0.0089692, -0.9998152, 3.404,
-                            0.9998512,  0.0030746,  0.0169764, -0.164,
+camera2lidar_extrinsic:  [  0.0177667, -0.9994557, -0.0277972, 0.258,
+                           -0.0891004,  0.0261083, -0.9956804, 3.431,
+                            0.9958642,  0.0201667, -0.0885880, 0.212,
                             0, 0, 0, 1 ]
 src_img_width: 1920
 src_img_height: 1080

src/camera_bev_infer/configs/CameraBevParam_jd07.yaml

+rgb_sub_topic_name: /sensor/camera/front_middle/image/bgr
+pointcloud_sub_topic_name: /total_calib_pointcloud
+detection_pub_topic_name: /camera/objects
+# static_gridmap_pub_topic_name: /perception/camera/bev_static_gridmap
+creator_id: 0000
+creator_name: camera_bev_node
+sensor_frame_id: camera_01
+config_root_path: /home/nvidia/Projects/perception/ros_camera_bev/src/camera_bev_infer
+#config_root_path: /home/ubuntu/projects/ros_camera_bev/src/camera_bev_infer
+rgb_config_path: configs/weights/multi_yolov5m.yaml
+device: 0
+period_time: 5
+vis_res: true
+vis_frame_id: Lidar
+vis_project: false
+# ["pedestrian", "two_wheel", "car", "truck", "construction_machine", "fence", "stone", "dust", "cone"]
+pub_classes: ["stone", "cone"]
+obstacle_height_threshold: 25 # cm
+camera_intrinsic: [1346.61241105747, 0, 951.014999441565,
+                    0, 1344.43795511862, 530.562496955385,
+                    0, 0, 1]
+distortion_coefficients: [-0.449196898930979, 0.253956763950171, -0.00139403693782871, -0.00111424348026907, -0.0870171627456232]
+
+camera2lidar_extrinsic:  [  0.0177667, -0.9994557, -0.0277972, 0.258,
+                           -0.0891004,  0.0261083, -0.9956804, 3.431,
+                            0.9958642,  0.0201667, -0.0885880, 0.212,
+                            0, 0, 0, 1 ]
+src_img_width: 1920
+src_img_height: 1080
+dst_img_width: 960
+dst_img_height: 540
+

src/camera_bev_infer/configs/CameraBevParam_jd08.yaml

+rgb_sub_topic_name: /sensor/camera/front_middle/image/bgr
+pointcloud_sub_topic_name: /total_calib_pointcloud
+detection_pub_topic_name: /camera/objects
+# static_gridmap_pub_topic_name: /perception/camera/bev_static_gridmap
+creator_id: 0000
+creator_name: camera_bev_node
+sensor_frame_id: camera_01
+config_root_path: /home/nvidia/Projects/perception/ros_camera_bev/src/camera_bev_infer
+#config_root_path: /home/ubuntu/projects/ros_camera_bev/src/camera_bev_infer
+rgb_config_path: configs/weights/multi_yolov5m.yaml
+device: 0
+period_time: 5
+vis_res: true
+vis_frame_id: Lidar
+vis_project: false
+# ["pedestrian", "two_wheel", "car", "truck", "construction_machine", "fence", "stone", "dust", "cone"]
+pub_classes: ["stone", "cone"]
+obstacle_height_threshold: 25 # cm
+camera_intrinsic: [1346.61241105747, 0, 951.014999441565,
+                    0, 1344.43795511862, 530.562496955385,
+                    0, 0, 1]
+distortion_coefficients: [-0.449196898930979, 0.253956763950171, -0.00139403693782871, -0.00111424348026907, -0.0870171627456232]
+
+camera2lidar_extrinsic:  [  0.0357085, -0.9993340, -0.0075053, 0.168,
+                            -0.0840158,  0.0044816, -0.9964544, 3.492,
+                            0.9958244,  0.0362125, -0.0837998, 0.074,
+                            0, 0, 0, 1 ]
+src_img_width: 1920
+src_img_height: 1080
+dst_img_width: 960
+dst_img_height: 540
+
+
+
+
+

src/camera_bev_infer/configs/weights/multi_yolov5m.yaml

@@ -28,8 +28,8 @@ units:
         inputNames: [resizeNormImages]
         outputNames: [detections, seg_protos, depths, semantics]
         runMode: 1 # 0-fp32 1-fp16 2-int8 (int8 not supported)
-        weightsPath: "configs/weights/isp_m_best.onnx"
-        engineFile: "configs/weights/isp_m_best_fp16.engine"
+        weightsPath: "configs/weights/isp_x0228m_last.onnx"
+        engineFile: "configs/weights/isp_x0228m_last_fp16.engine"
         inferDynamic: false
         inferBatchSize: 1
         inputWidth: 960
@@ -47,12 +47,12 @@ units:
         scoreThreshold: 0.15 # used when inference, can be modified
         truncatedThreshold: 0.05
         maxOutputNum: 1000
-        rawDetectionLength: 32130 # 25200
+        rawDetectionLength: 42840  # 32130
         keepTopK: 100
         segProtoDim: 32
         instanceDownScale: 4
-        instanceClassNumber: 9
-        instanceClassNames: ["pedestrian", "two_wheel", "car", "truck", "construction_machine", "fence", "stone", "dust", "cone"]
+        instanceClassNumber: 3
+        instanceClassNames: ["pedestrian", "vehicle", "stone"]
         # semantic-seg
         semanticDownScale: 4
         semanticClassNumber: 2
@@ -66,7 +66,7 @@ units:
         outputNames: [tracked_instances]
         frame_rate: 10
         track_buffer: 30
-        track_thresh: 0.2
+        track_thresh: 0.15
         high_thresh: 0.2
         match_thresh: 0.6
         init_frames: 2

src/camera_bev_infer/configs/weights/multi_yolov5s.yaml → src/camera_bev_infer/configs/weights/multi_yolov5m_v0.yaml

 
 name: CameraModel
 inputNames: [cvImage]
-outputNames: [out_instances, out_semantics, out_depths, undistortVisImage]
+outputNames: [tracked_instances, out_semantics, out_depths, undistortVisImage]
 
 units:
     -
@@ -28,8 +28,8 @@ units:
         inputNames: [resizeNormImages]
         outputNames: [detections, seg_protos, depths, semantics]
         runMode: 1 # 0-fp32 1-fp16 2-int8 (int8 not supported)
-        weightsPath: "configs/weights/isp_s_best.onnx"
-        engineFile: "configs/weights/isp_s_best_fp16.engine"
+        weightsPath: "configs/weights/isp_m_best.onnx"
+        engineFile: "configs/weights/isp_m_best_fp16.engine"
         inferDynamic: false
         inferBatchSize: 1
         inputWidth: 960
@@ -44,7 +44,7 @@ units:
         fixAspectRatio: false
         # instance-seg
         nmsThreshold: 0.45
-        scoreThreshold: 0.2 # used when inference, can be modified
+        scoreThreshold: 0.15 # used when inference, can be modified
         truncatedThreshold: 0.05
         maxOutputNum: 1000
         rawDetectionLength: 32130 # 25200
@@ -60,6 +60,17 @@ units:
         # depth
         depthDownScale: 4
         depthDistanceScale: 256
+    -
+        name: ByteTracker
+        inputNames: [out_instances]
+        outputNames: [tracked_instances]
+        frame_rate: 10
+        track_buffer: 30
+        track_thresh: 0.15
+        high_thresh: 0.15
+        match_thresh: 0.6
+        init_frames: 2
+        out_lost_threshold: 0
 
         
 

src/camera_bev_infer/src/sensor_manager.h

@@ -5,7 +5,7 @@
  * @Date: 2023-09-03 01:53:20
  * @email: xin.wang@waytous.com
  * @LastEditors: wxin
- * @LastEditTime: 2023-12-14 07:06:26
+ * @LastEditTime: 2024-02-29 03:17:36
  */
 
 #ifndef SENSORMANAGER_H

src/camera_bev_infer/src/vision_transform/vision_3d.cpp

@@ -5,7 +5,7 @@
  * @Date: 2023-09-03 03:13:41
  * @email: xin.wang@waytous.com
  * @LastEditors: wxin
- * @LastEditTime: 2024-01-18 04:25:22
+ * @LastEditTime: 2024-02-28 10:32:38
  */
 
 
@@ -714,7 +714,6 @@ cv::RotatedRect  Visione_3D::getObjectAreaRect(const cv::Mat& imgBin, const cv::
             } 
         }
     }
-
     if( vPoints.size() > 3)
     {
         rRect = minAreaRect(vPoints);
@@ -963,10 +962,13 @@ int Visione_3D::Process_Instance_Objects(object_Seg_info& obj, const cv::Mat& in
     }
     // std::cout<<" instance find table vision "<<double(clock()-start_)/CLOCKS_PER_SEC<<"s"<<std::endl;  //输出时间（单位：ｓ）
     float project_ratio = project_count / (mask_count + 1e-5);
-    bool valid = validTracks.find(obj.track_id) != validTracks.end();
-    // std::cout<<"obj: "<< obj.name <<" with project ratio: "<< project_ratio << ", " << valid << std::endl;
+    if(Tracks.find(obj.track_id) == Tracks.end()){
+        Tracks[obj.track_id] = STrack(); // init
+    }
+    bool valid = Tracks[obj.track_id].valid;
+    // std::cout<<"obj: "<< obj.track_id<< ","<< obj.name <<" with project ratio: "<< project_ratio << ", " << project_count << std::endl;
     if(valid){
-        validTracks[obj.track_id] = 0; // update
+        Tracks[obj.track_id].last_update_age = 0; // update
     }
     bool pointEnough = false;
     cv::Rect objRc = cv::Rect(objX1, objY1, MIN(objBin.cols, objX2 + 1) - objX1,  MIN(objBin.rows, objY2 + 1) - objY1);
@@ -989,82 +991,57 @@ int Visione_3D::Process_Instance_Objects(object_Seg_info& obj, const cv::Mat& in
             insobj.fConfidence  = obj.fConfidence;
             vGrid.vInsObjs.push_back(insobj);
             obj.valid = obj.valid + 1;
+            // std::cout<< obj.track_id<<"valid----------------"<<std::endl;
         }else{
+            // std::cout<< obj.track_id<<"depth----------------"<<std::endl;
             return -1; // depth estimination
         }
     }else{
         if(pointEnough)
         {
             // 静态目标根据高度差过滤误检
-            bool filtered = false;
             // if(std::find(perception::camera::StaticTypes.begin(), perception::camera::StaticTypes.end(), obj.name)
             //     != perception::camera::StaticTypes.end())
-            if(obj.name == "stone")
-            {
-                // 过滤多余点
-                auto bundingRect = insobj.rrc.boundingRect2f();
-                for(auto iter = points3d.begin(); iter != points3d.end();){
-                    bool in_rect = ((iter->x <= bundingRect.x+bundingRect.width) && (iter->x >= bundingRect.x) && 
-                                    (iter->y <= bundingRect.y+bundingRect.height) && (iter->y >= bundingRect.y));
-                    if(!in_rect){
-                        iter = points3d.erase(iter);
-                    }else{
-                        iter++;
-                    }
+            // 过滤多余点
+            auto bundingRect = insobj.rrc.boundingRect2f();
+            for(auto iter = points3d.begin(); iter != points3d.end();){
+                bool in_rect = ((iter->x <= bundingRect.x+bundingRect.width) && (iter->x >= bundingRect.x) && 
+                                (iter->y <= bundingRect.y+bundingRect.height) && (iter->y >= bundingRect.y));
+                if(!in_rect){
+                    iter = points3d.erase(iter);
+                }else{
+                    iter++;
                 }
-                // 获得高低差
-                float min_z = 1e5, max_z = -1e5;
-                for(auto& p3d_: points3d){
-                    if(p3d_.z > max_z){
-                        max_z = p3d_.z;
-                    }
-                    if(p3d_.z < min_z){
-                        min_z = p3d_.z;
-                    }
+            }
+            // 获得高低差
+            float min_z = 1e5, max_z = -1e5;
+            for(auto& p3d_: points3d){
+                if(p3d_.z > max_z){
+                    max_z = p3d_.z;
                 }
-                filtered = (max_z - min_z < obstacle_height_threshold * 0.01);
-                if(!filtered){
-                    validTracks[obj.track_id] = 0; // init
-                    std::cout << "------------------------ "<< obj.track_id <<" stone with "<<points3d.size() << " points, height diff: " << max_z - min_z << "m." << std::endl;
+                if(p3d_.z < min_z){
+                    min_z = p3d_.z;
                 }
-                /*else{// 扬尘遮挡，点少
-                    if(unvalidTracks.find(obj.track_id) != unvalidTracks.end()){
-                        if(project_ratio < 0.05 && obj.fConfidence > 0.5){
-                            unvalidTracks[obj.track_id].second++;
-                        }
-                        // 长时间跟踪上，但是未被激活，用深度估计激活
-                        if(unvalidTracks[obj.track_id].second >= 10){
-                            unvalidTracks.erase(obj.track_id);
-                            validTracks[obj.track_id] = 0; // init
-                            return -1; // using depth estimination
-                        }
-                    }else if(project_ratio < 0.05 && obj.fConfidence > 0.5){ // 当遮挡比例大时
-                        unvalidTracks[obj.track_id] = std::make_pair(0, 0);
-                    }
-                }*/
             }
-            // 高差大于阈值输出
+
+            bool filtered = (max_z - min_z < obstacle_height_threshold * 0.01) && (max_z > obstacle_height_threshold * 0.01 * 0.5);
             if(!filtered){
-                insobj.rc = obj.rc;
-                insobj.color = uColor;
-                insobj.names = obj.name;
-                insobj.fConfidence  = obj.fConfidence;
-                vGrid.vInsObjs.push_back(insobj);
-                obj.valid = obj.valid + 1;
-            }
-        }else{
-            if(unvalidTracks.find(obj.track_id) != unvalidTracks.end()){
-                unvalidTracks[obj.track_id].second++;
-                // 长时间跟踪上，但是未被激活，用深度估计激活
-                // if(unvalidTracks[obj.track_id].second >= 10){
-                //     unvalidTracks.erase(obj.track_id);
-                //     validTracks[obj.track_id] = 0; // init
-                //     return -1; // using depth estimination
-                // }
-            }else{
-                unvalidTracks[obj.track_id] = std::make_pair(0, 0);
+                Tracks[obj.track_id].valid_frame_num++; // add
+                std::cout << "------------------------ "<< obj.track_id <<": " << obj.name <<" with "<<points3d.size() << 
+                    " points, height diff: " << max_z - min_z << "m, max-z: " << max_z << "m." << std::endl;
+                if(Tracks[obj.track_id].valid_frame_num >= 2){
+                    Tracks[obj.track_id].valid = true;
+                    insobj.rc = obj.rc;
+                    insobj.color = uColor;
+                    insobj.names = obj.name;
+                    insobj.fConfidence  = obj.fConfidence;
+                    vGrid.vInsObjs.push_back(insobj);
+                    obj.valid = obj.valid + 1;
+                }
             }
+            // point enough, filtered by 
         }
+        // todo, point not enough
     }
     return 0;
     
@@ -1139,29 +1116,17 @@ int Visione_3D::Process_objects(const cv::Mat& image, const cv::Mat& depth, Clou
             Process_Segmantic_Objects(obj, point_index_in_camera, pcl_cloud, vGrid);
         }
     }
-    // update and delete valid stones 
-    for(auto t = validTracks.begin(); t != validTracks.end();){
-        t->second++;
-        // std::cout<<t->first<<","<<t->second<<std::endl;
-        if(t->second > 30){
-            validTracks.erase(t++);
+    // update and delete tracks 
+    for(auto t=Tracks.begin(); t != Tracks.end();){
+        t->second.last_update_age++;
+        t->second.tracked_age++;
+        if(t->second.last_update_age > 30){
+            Tracks.erase(t++);
         }else{
             t++;
         }
     }
-    // std::cout<<"-----valid stones:" << validTracks.size() << std::endl;
-
-    // update and delete unvalid stones
-    for(auto t = unvalidTracks.begin(); t != unvalidTracks.end();){
-        t->second.first++;
-        // std::cout<<t->first<<","<<t->second.first<<","<<t->second.second<<std::endl;
-        if(t->second.first > 30){
-            unvalidTracks.erase(t++);
-        }else{
-            t++;
-        }
-    }
-    // std::cout<<"-----unvalid stones:" << unvalidTracks.size() << std::endl;
+
     // 将静态目标投影到栅格地图中
     pcl::PointXYZI bev_point;
     cv::Point2f pt2f;

src/camera_bev_infer/src/vision_transform/vision_3d.h

@@ -5,7 +5,7 @@
  * @Date: 2023-04-13 14:37:39
  * @emal: qingshui.cheng@waytous.com
  * @LastEditors: wxin
- * @LastEditTime: 2023-12-04 06:11:33
+ * @LastEditTime: 2024-02-28 09:14:51
  */
 
 #ifndef VISION_3D_H
@@ -214,10 +214,17 @@ private:
 
     std::string current_frame_id = "Lidar";
     bool vis_project = false;
+
+    struct STrack{
+        bool valid = false;
+        int valid_frame_num = 0;
+        int tracked_age = 0;
+        int last_update_age = 0;
+    };
     // 保存跟踪后高度够的落石, id: last_update_age
-    std::map<int, int> validTracks; 
-     // 保存跟踪的，但是高度不够，未被激活的落石, id: (tracked_age, last_update_age)
-    std::map<int, std::pair<int, int>> unvalidTracks;
+    std::map<int, STrack> Tracks; 
+    //  保存跟踪的，但是高度不够，未被激活的落石, id: (tracked_age, last_update_age)
+    // std::map<int, std::pair<int, int>> unvalidTracks;
     cv::Mat m_matrixQ;
     Eigen::Matrix4d m_lidar2camera;
 	// Eigen::Matrix4d m_lidar2odom;