添加电梯拥挤度筛选代码，roi和阈值筛选

src/guangpo_yolo_lidar/include/yolo_lidar_help.hpp

@@ -54,6 +54,7 @@ struct mybox {
         lidar_cluster(new pcl::PointCloud<pcl::PointXYZ>) {}
 
   std::string label;
+  float prop;
   cv::Rect rect;
   cv::Rect small_rect;
   pcl::PointCloud<pcl::PointXYZ>::Ptr lidar_points;
@@ -66,7 +67,7 @@ struct mybox {
 
   // 拷贝构造函数
   mybox(const mybox &other)
-      : label(other.label), rect(other.rect), small_rect(other.small_rect),
+      : label(other.label), prop(other.prop), rect(other.rect), small_rect(other.small_rect),
         position(other.position),
         lidar_points(new pcl::PointCloud<pcl::PointXYZ>(*other.lidar_points)),
         lidar_cluster(new pcl::PointCloud<pcl::PointXYZ>(*other.lidar_cluster)),
@@ -78,6 +79,7 @@ struct mybox {
   mybox &operator=(const mybox &other) {
     if (this != &other) {
       label = other.label;
+      prop = other.prop;
       rect = other.rect;
       small_rect = other.small_rect;
 
@@ -95,7 +97,7 @@ struct mybox {
 
   // 移动构造函数
   mybox(mybox &&other) noexcept
-      : label(std::move(other.label)), rect(std::move(other.rect)),
+      : label(std::move(other.label)),prop(std::move(other.prop)), rect(std::move(other.rect)),
 
         position(std::move(other.position)),
         small_rect(std::move(other.small_rect)),
@@ -109,6 +111,7 @@ struct mybox {
   mybox &operator=(mybox &&other) noexcept {
     if (this != &other) {
       label = std::move(other.label);
+      prop = std::move(other.prop);
       rect = std::move(other.rect);
       small_rect = std::move(other.small_rect);
       position = std::move(other.position),

src/guangpo_yolo_lidar/src/main.cpp

@@ -106,6 +106,8 @@ public:
 
     person_density_pub_ = nh.advertise<std_msgs::Int8>(
         yaml_config["person_density_topic"].as<std::string>(), 10);
+    person_density_withroi_pub_ = nh.advertise<std_msgs::Int8>(
+        yaml_config["person_density_withroi_topic"].as<std::string>(), 10);
 
     sub_detect_congest_enable_ = nh.subscribe(
         yaml_config["detect_congest_enable_topic"].as<std::string>(), 10,
@@ -492,6 +494,7 @@ public:
           class_name == "car" || class_name == "motorcycle" ||
           class_name == "cat" || class_name == "dog") {
         box.label = class_name;
+        box.prop = det_result->prop;
         box3ds.push_back(box);
       }
     }
@@ -629,6 +632,46 @@ public:
     auto obstacles_msg = PackageMessage(box3ds, true);
     pub_obstacles_.publish(obstacles_msg);
 
+    // 添加电梯拥挤度使用roi检测的代码
+    static double person_density_roi_x_min = yaml_config["person_density_roi"]["x_min"].as<double>();
+    static double person_density_roi_x_max = yaml_config["person_density_roi"]["x_max"].as<double>();
+    static double person_density_roi_y_min = yaml_config["person_density_roi"]["y_min"].as<double>();
+    static double person_density_roi_y_max = yaml_config["person_density_roi"]["y_max"].as<double>();
+    static double person_density_roi_confidence = yaml_config["person_density_roi_confidence"].as<double>();
+    // gDebugWarn(person_density_roi_x_min);
+    // gDebugWarn(person_density_roi_x_max);
+    // gDebugWarn(person_density_roi_y_min);
+    // gDebugWarn(person_density_roi_y_max);
+    int person_withroi_cnt=0;
+    for (int i = 0; i < box3ds.size(); i++) {
+      auto &box = box3ds[i];
+      if(box.label != "person") {
+        continue;
+      }
+      // 如果聚类失败了(点太少或者不满足要求)
+      if (box.lidar_cluster->points.empty()) {
+        continue;
+      }
+
+      auto& center= box.position.center;
+      gDebugWarn() << VAR(i,box.prop,center.x,center.y);
+      if(box.prop < person_density_roi_confidence) {
+        continue;
+      }
+      if(center.x < person_density_roi_x_min || center.x > person_density_roi_x_max) {
+        continue;
+      }
+      if(center.y < person_density_roi_y_min || center.y > person_density_roi_y_max) {
+        continue;
+      }
+      person_withroi_cnt++;
+    }
+    gDebugWarn(person_withroi_cnt);
+    std_msgs::Int8 person_density_roi_msg;
+    person_density_roi_msg.data = person_withroi_cnt;
+    person_density_withroi_pub_.publish(person_density_roi_msg);
+
+
     // 发布可视化内容
     auto packages_marker = Package2dBoxMarker(box3ds);
     Publish2dObstacles(packages_marker);
@@ -703,6 +746,7 @@ private:
   ros::Subscriber sub_detect_congest_enable_;
   bool enable_detect_congest = true;
   ros::Publisher person_density_pub_;
+  ros::Publisher person_density_withroi_pub_;
   ros::Publisher pub_obstacles_;
 
   ros::Publisher image_pub_;

src/guangpo_yolo_lidar/yaml_config/yaml_config.yaml

@@ -10,7 +10,7 @@ pub_box3d_markers_topic : "/box3d_markers"
 # 使用仿真还是激光雷达
 # 使用仿真就是图像数据从功能包接收
 # 不使用仿真就是图像数据从/dev驱动中读取
-use_bag : false
+use_bag : true
 camera_image_dev : "/dev/video11"
 
 # 大于一定距离就不计算到电梯拥挤度里面 ,单位 m
@@ -19,6 +19,16 @@ person_distance : 5
 detect_congest_enable_topic : "/detect_congest_enable"
 # 发布的电梯拥挤度话题
 person_density_topic : "/person_density"
+# 发布的电梯拥挤度话题（指定roi排除掉镜像人员)
+person_density_withroi_topic : "/person_density_withroi"
+# 电梯拥挤度roi筛选（大于这个置信度才进行后续的roi筛选） (设置成1.0就认为不筛选置信度了)
+person_density_roi_confidence : 0.6
+# 电梯拥挤度检测，排除掉镜像人员，指定roi区域 （长方形区域，基于车体坐标系）
+person_density_roi:
+  x_min: 0 
+  x_max: 2.8 
+  y_min: -0.5
+  y_max: 0.5
 # 发布的障碍物信息话题
 obstacles_topic_ : "/obstacles"