use polygon for occlusion detection

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

planning/behavior_velocity_intersection_module/CMakeLists.txt

@@ -5,6 +5,8 @@ find_package(autoware_cmake REQUIRED)
 autoware_package()
 pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml)
 
+find_package(OpenCV REQUIRED)
+
 ament_auto_add_library(${PROJECT_NAME} SHARED
   src/debug.cpp
   src/manager.cpp
@@ -13,4 +15,8 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
   src/util.cpp
 )
 
+target_link_libraries(${PROJECT_NAME}
+  ${OpenCV_LIBRARIES}
+)
+
 ament_auto_package(INSTALL_TO_SHARE config)

planning/behavior_velocity_intersection_module/config/intersection.param.yaml

@@ -46,7 +46,7 @@
         min_vehicle_brake_for_rss: -2.5 # [m/s^2]
         max_vehicle_velocity_for_rss: 16.66 # [m/s] == 60kmph
         denoise_kernel: 1.0 # [m]
-        pub_debug_grid: false
+        possible_object_bbox: [1.0, 2.0] # [m x m]
 
       enable_rtc: # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval
         intersection: true

planning/behavior_velocity_intersection_module/package.xml

@@ -20,14 +20,9 @@
   <depend>autoware_auto_perception_msgs</depend>
   <depend>autoware_auto_planning_msgs</depend>
   <depend>behavior_velocity_planner_common</depend>
-  <depend>cv_bridge</depend>
   <depend>geometry_msgs</depend>
-  <depend>grid_map_cv</depend>
-  <depend>grid_map_ros</depend>
   <depend>interpolation</depend>
   <depend>lanelet2_extension</depend>
-  <depend>libboost-dev</depend>
-  <depend>libopencv-dev</depend>
   <depend>magic_enum</depend>
   <depend>motion_utils</depend>
   <depend>nav_msgs</depend>

planning/behavior_velocity_intersection_module/src/manager.cpp

@@ -113,7 +113,6 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
   ip.occlusion.max_vehicle_velocity_for_rss =
     node.declare_parameter<double>(ns + ".occlusion.max_vehicle_velocity_for_rss");
   ip.occlusion.denoise_kernel = node.declare_parameter<double>(ns + ".occlusion.denoise_kernel");
-  ip.occlusion.pub_debug_grid = node.declare_parameter<bool>(ns + ".occlusion.pub_debug_grid");
   ip.occlusion.possible_object_bbox =
     node.declare_parameter<std::vector<double>>(ns + ".occlusion.possible_object_bbox");
 }

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

@@ -12,34 +12,20 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include <grid_map_cv/grid_map_cv.hpp>
-#include <grid_map_ros/grid_map_ros.hpp>
-#include <lanelet2_extension/regulatory_elements/road_marking.hpp>
-#include <lanelet2_extension/utility/message_conversion.hpp>
-#include <lanelet2_extension/utility/query.hpp>
-#include <lanelet2_extension/utility/utilities.hpp>
-
-#if __has_include(<cv_bridge/cv_bridge.hpp>)
-#include <cv_bridge/cv_bridge.hpp>
-#else
-#include <cv_bridge/cv_bridge.h>
-#endif
-// #include <sensor_msgs/image_encodings.h>
-// #include <opencv2/highgui/highgui.hpp>
 #include "scene_intersection.hpp"
+
 #include "util.hpp"
 
 #include <behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp>
 #include <behavior_velocity_planner_common/utilization/path_utilization.hpp>
 #include <behavior_velocity_planner_common/utilization/util.hpp>
+#include <lanelet2_extension/regulatory_elements/road_marking.hpp>
+#include <lanelet2_extension/utility/message_conversion.hpp>
+#include <lanelet2_extension/utility/query.hpp>
+#include <lanelet2_extension/utility/utilities.hpp>
 #include <magic_enum.hpp>
 #include <opencv2/imgproc.hpp>
 
-#include <boost/geometry/algorithms/area.hpp>
-#include <boost/geometry/algorithms/simplify.hpp>
-#include <boost/geometry/core/cs.hpp>
-#include <boost/geometry/geometries/register/point.hpp>
-
 #include <lanelet2_core/geometry/Polygon.h>
 #include <lanelet2_core/primitives/BasicRegulatoryElements.h>
 
@@ -49,8 +35,6 @@
 #include <tuple>
 #include <vector>
 
-BOOST_GEOMETRY_REGISTER_POINT_2D(cv::Point, int, cs::cartesian, x, y)  // NOLINT
-
 namespace behavior_velocity_planner
 {
 namespace bg = boost::geometry;
@@ -100,10 +84,6 @@ IntersectionModule::IntersectionModule(
     planner_param_.collision_detection.state_transit_margin_time);
   before_creep_state_machine_.setMarginTime(planner_param_.occlusion.before_creep_stop_time);
   before_creep_state_machine_.setState(StateMachine::State::STOP);
-  if (enable_occlusion_detection) {
-    occlusion_grid_pub_ = node_.create_publisher<grid_map_msgs::msg::GridMap>(
-      "~/debug/intersection/occlusion_grid", rclcpp::QoS(1).transient_local());
-  }
   stuck_private_area_timeout_.setMarginTime(planner_param_.stuck_vehicle.timeout_private_area);
   stuck_private_area_timeout_.setState(StateMachine::State::STOP);
 }
@@ -1111,9 +1091,6 @@ bool IntersectionModule::checkCollision(
   return collision_detected;
 }
 
-using CvPolygon = boost::geometry::model::polygon<cv::Point>;
-using CvLineString = boost::geometry::model::linestring<cv::Point>;
-
 bool IntersectionModule::isOcclusionCleared(
   const nav_msgs::msg::OccupancyGrid & occ_grid,
   const std::vector<lanelet::CompoundPolygon3d> & attention_areas,
@@ -1355,10 +1332,20 @@ bool IntersectionModule::isOcclusionCleared(
   cv::findContours(occlusion_mask, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);
   std::vector<std::vector<cv::Point>> valid_contours;
   for (const auto & contour : contours) {
+    std::vector<cv::Point> valid_contour;
+    for (const auto & p : contour) {
+      if (distance_grid.at<unsigned char>(p.y, p.x) == undef_pixel) {
+        continue;
+      }
+      valid_contour.push_back(p);
+    }
+    if (valid_contour.size() <= 2) {
+      continue;
+    }
     std::vector<cv::Point> approx_contour;
     cv::approxPolyDP(
-      contour, approx_contour,
-      std::round(std::min(possible_object_bbox_x, possible_object_bbox_y) / resolution), true);
+      valid_contour, approx_contour,
+      std::round(std::min(possible_object_bbox_x, possible_object_bbox_y) / std::sqrt(2.0)), true);
     if (approx_contour.size() <= 1) continue;
     // check area
     const double poly_area = cv::contourArea(approx_contour);
@@ -1378,59 +1365,29 @@ bool IntersectionModule::isOcclusionCleared(
     debug_data_.occlusion_polygons.push_back(polygon_msg);
   }
 
-  // clean-up and find nearest risk
   const int min_cost_thr = dist2pixel(occlusion_dist_thr);
   int min_cost = undef_pixel - 1;
-  int max_cost = 0;
-  std::optional<int> min_cost_projection_ind = std::nullopt;
   geometry_msgs::msg::Point nearest_occlusion_point;
-  for (int i = 0; i < width; ++i) {
-    for (int j = 0; j < height; ++j) {
-      const int pixel = static_cast<int>(distance_grid.at<unsigned char>(height - 1 - j, i));
-      const bool occluded = (occlusion_mask.at<unsigned char>(height - 1 - j, i) == 255);
+  for (const auto & occlusion_contour : valid_contours) {
+    for (const auto & p : occlusion_contour) {
+      const int pixel = static_cast<int>(distance_grid.at<unsigned char>(p.y, p.x));
+      const bool occluded = (occlusion_mask.at<unsigned char>(p.y, p.x) == 255);
       if (pixel == undef_pixel || !occluded) {
-        // ignore outside of cropped
-        // some cell maybe undef still
-        distance_grid.at<unsigned char>(height - 1 - j, i) = 0;
         continue;
       }
-      if (max_cost < pixel) {
-        max_cost = pixel;
-      }
-      const int projection_ind = projection_ind_grid.at<int>(height - 1 - j, i);
       if (pixel < min_cost) {
-        assert(projection_ind >= 0);
         min_cost = pixel;
-        min_cost_projection_ind = projection_ind;
-        nearest_occlusion_point.x = origin.x + i * resolution;
-        nearest_occlusion_point.y = origin.y + j * resolution;
-        nearest_occlusion_point.z = origin.z + distance_max * pixel / max_cost_pixel;
+        nearest_occlusion_point.x = origin.x + p.x * resolution;
+        nearest_occlusion_point.y = origin.y + (height - 1 - p.y) * resolution;
+        nearest_occlusion_point.z = origin.z;
       }
     }
   }
-  debug_data_.nearest_occlusion_point = nearest_occlusion_point;
-
-  if (planner_param_.occlusion.pub_debug_grid) {
-    cv::Mat distance_grid_heatmap;
-    cv::applyColorMap(distance_grid, distance_grid_heatmap, cv::COLORMAP_JET);
-    grid_map::GridMap occlusion_grid({"elevation"});
-    occlusion_grid.setFrameId("map");
-    occlusion_grid.setGeometry(
-      grid_map::Length(width * resolution, height * resolution), resolution,
-      grid_map::Position(origin.x + width * resolution / 2, origin.y + height * resolution / 2));
-    cv::rotate(distance_grid, distance_grid, cv::ROTATE_90_COUNTERCLOCKWISE);
-    cv::rotate(distance_grid_heatmap, distance_grid_heatmap, cv::ROTATE_90_COUNTERCLOCKWISE);
-    grid_map::GridMapCvConverter::addLayerFromImage<unsigned char, 1>(
-      distance_grid, "elevation", occlusion_grid, origin.z /* elevation for 0 */,
-      origin.z + distance_max /* elevation for 255 */);
-    grid_map::GridMapCvConverter::addColorLayerFromImage<unsigned char, 3>(
-      distance_grid_heatmap, "color", occlusion_grid);
-    occlusion_grid_pub_->publish(grid_map::GridMapRosConverter::toMessage(occlusion_grid));
-  }
 
-  if (min_cost > min_cost_thr || !min_cost_projection_ind.has_value()) {
+  if (min_cost > min_cost_thr) {
     return true;
   } else {
+    debug_data_.nearest_occlusion_point = nearest_occlusion_point;
     return false;
   }
 }

planning/behavior_velocity_intersection_module/src/scene_intersection.hpp

@@ -20,13 +20,11 @@
 #include <behavior_velocity_planner_common/scene_module_interface.hpp>
 #include <behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp>
 #include <behavior_velocity_planner_common/utilization/state_machine.hpp>
-#include <grid_map_core/grid_map_core.hpp>
 #include <motion_utils/motion_utils.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <tier4_autoware_utils/tier4_autoware_utils.hpp>
 
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
-#include <grid_map_msgs/msg/grid_map.hpp>
 
 #include <lanelet2_core/LaneletMap.h>
 #include <lanelet2_routing/RoutingGraph.h>
@@ -108,7 +106,6 @@ class IntersectionModule : public SceneModuleInterface
       double min_vehicle_brake_for_rss;
       double max_vehicle_velocity_for_rss;
       double denoise_kernel;
-      bool pub_debug_grid;
       std::vector<double> possible_object_bbox;
     } occlusion;
   };
@@ -275,7 +272,6 @@ class IntersectionModule : public SceneModuleInterface
   */
 
   util::DebugData debug_data_;
-  rclcpp::Publisher<grid_map_msgs::msg::GridMap>::SharedPtr occlusion_grid_pub_;
 };
 
 }  // namespace behavior_velocity_planner