From 053abaf79999f00cdd275bb245cb3953318ad197 Mon Sep 17 00:00:00 2001 From: tanaka3 Date: Thu, 10 Feb 2022 11:36:34 +0900 Subject: [PATCH] chore(behavior_velocity): refactor and use planner param Signed-off-by: tanaka3 --- .../config/occlusion_spot.param.yaml | 12 ++--- .../scene_module/occlusion_spot/geometry.hpp | 24 ++-------- .../occlusion_spot/occlusion_spot_utils.hpp | 37 +++++++++++---- .../scene_occlusion_spot_in_private_road.hpp | 2 +- .../scene_occlusion_spot_in_public_road.hpp | 2 +- .../occlusion-spot-design.md | 20 ++++----- .../src/scene_module/occlusion_spot/debug.cpp | 5 ++- .../scene_module/occlusion_spot/geometry.cpp | 25 +++++------ .../scene_module/occlusion_spot/manager.cpp | 11 ++--- .../occlusion_spot/occlusion_spot_utils.cpp | 44 +++++++++--------- .../scene_occlusion_spot_in_private_road.cpp | 4 +- .../test/src/test_grid_utils.cpp | 45 ------------------- 12 files changed, 96 insertions(+), 135 deletions(-) diff --git a/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml b/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml index 3397c6479a5be..fe6728a56ea11 100644 --- a/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml +++ b/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml @@ -9,13 +9,13 @@ motion: safety_ratio: 0.8 # [-] jerk/acceleration ratio for safety max_slow_down_acceleration: -1.5 # [m/s^2] minimum deceleration for safe deceleration. - min_allowed_velocity: 1.0 # [m/s] minimum velocity allowed + min_allowed_velocity: 1.0 # [m/s] minimum velocity allowed delay_time: 0.1 # [s] safety time buffer for delay system response safe_margin: 1.0 # [m] maximum safety distance for any error - sidewalk: + detection_area: min_occlusion_spot_size: 1.0 # [m] occupancy grid must contain an UNKNOWN area of at least size NxN to be considered a hidden obstacle. - slice_size: 1.5 # [m] size of sidewalk slices in both length and distance relative to the ego trajectory. - focus_range: 3.0 # [m] buffer around the ego path used to build the sidewalk area. + slice_length: 1.5 # [m] size of slices in both length and distance relative to the ego path. + max_lateral_distance: 3.0 # [m] buffer around the ego path used to build the detection area. grid: - free_space_max: 40 # maximum value of a free space cell in the occupancy grid - occupied_min: 60 # minimum value of an occupied cell in the occupancy grid + free_space_max: 40 # [-] maximum value of a free space cell in the occupancy grid + occupied_min: 60 # [-] minimum value of an occupied cell in the occupancy grid diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/geometry.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/geometry.hpp index 9f00951f2c3c0..4d7130a628b37 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/geometry.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/geometry.hpp @@ -30,33 +30,15 @@ namespace occlusion_spot_utils { namespace bg = boost::geometry; -// @brief represent the range of a slice -// (either by index on the lanelet centerline or by arc coordinates) -struct SliceRange -{ - double min_length{}; - double max_length{}; - double min_distance{}; - double max_distance{}; -}; - -// @brief representation of a slice along a path -struct Slice -{ - SliceRange range{}; - lanelet::BasicPolygon2d polygon{}; -}; - //!< @brief build slices all along the trajectory // using the given range and desired slice length and width void buildSlices( std::vector & slices, const lanelet::ConstLanelet & path_lanelet, const SliceRange & range, const double slice_length, const double resolution); -//!< @brief build sidewalk slice from path +//!< @brief build detection_area slice from path void buildDetectionAreaPolygon( - std::vector & slice, const lanelet::ConstLanelet & path_lanelet, - const double longitudinal_offset, const double lateral_offset, const double min_size, - const double lateral_max_dist); + std::vector & slice, const lanelet::ConstLanelet & path_lanelet, const double offset, + const PlannerParam & param); //!< @brief calculate interpolation between a and b at distance ratio t template T lerp(T a, T b, double t) diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp index e03ec0d154597..08e03b7a315ff 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp @@ -18,7 +18,6 @@ #include #include #include -#include #include #include #include @@ -65,10 +64,10 @@ namespace occlusion_spot_utils { enum ROAD_TYPE { PRIVATE, PUBLIC, HIGHWAY, UNKNOWN }; -struct Sidewalk +struct DetectionArea { - double focus_range; // [m] distance to care about occlusion spot - double slice_size; // [m] size of each slice + double max_lateral_distance; // [m] distance to care about occlusion spot + double slice_length; // [m] size of each slice double min_occlusion_spot_size; // [m] minumum size to care about the occlusion spot }; struct Velocity @@ -84,6 +83,12 @@ struct Velocity double safe_margin; // [m] maximum safety distance for any error }; +struct LatLon +{ + double lateral_distance; // [m] lateral distance + double longitudinal_distance; // [m] longitudinal distance +}; + struct PlannerParam { // parameters in yaml @@ -101,7 +106,7 @@ struct PlannerParam double baselink_to_front; // [m] wheel_base + front_overhang Velocity v; - Sidewalk sidewalk; + DetectionArea detection_area; grid_utils::GridParam grid; }; @@ -111,6 +116,22 @@ struct SafeMotion double safe_velocity; }; +// @brief represent the range of a each polygon +struct SliceRange +{ + double min_length{}; + double max_length{}; + double min_distance{}; + double max_distance{}; +}; + +// @brief representation of a polygon along a path +struct Slice +{ + SliceRange range{}; + lanelet::BasicPolygon2d polygon{}; +}; + struct ObstacleInfo { SafeMotion safe_motion; // safe motion of velocity and stop point @@ -228,14 +249,14 @@ void calcSlowDownPointsForPossibleCollision( DetectionAreaIdx extractTargetRoadArcLength( const LaneletMapPtr lanelet_map_ptr, const double max_range, const PathWithLaneId & path, const ROAD_TYPE & target_road_type); -//!< @brief convert a set of occlusion spots found on sidewalk slice -void generateSidewalkPossibleCollisionFromOcclusionSpot( +//!< @brief convert a set of occlusion spots found on detection_area slice +void generateDetectionAreaPossibleCollisionFromOcclusionSpot( std::vector & possible_collisions, const grid_map::GridMap & grid, const std::vector & occlusion_spot_positions, const double offset_from_start_to_ego, const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param); //!< @brief generate possible collisions coming from occlusion spots on the side of the path -void generateSidewalkPossibleCollisions( +void generateDetectionAreaPossibleCollisions( std::vector & possible_collisions, const grid_map::GridMap & grid, const PathWithLaneId & path, const double offset_from_start_to_ego, const PlannerParam & param, std::vector & debug); diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.hpp index 93d382576c29e..90f10d81f2e03 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.hpp @@ -46,7 +46,7 @@ class OcclusionSpotInPrivateModule : public SceneModuleInterface { double z; std::string road_type = "private"; - std::vector sidewalks; + std::vector detection_areas; std::vector possible_collisions; PathWithLaneId path_raw; PathWithLaneId interp_path; diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.hpp index 250972dea8342..91c77f5997b37 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.hpp @@ -45,7 +45,7 @@ class OcclusionSpotInPublicModule : public SceneModuleInterface { std::string road_type = "public"; double z; - std::vector sidewalks; + std::vector detection_areas; std::vector parked_vehicle_point; std::vector possible_collisions; }; diff --git a/planning/behavior_velocity_planner/occlusion-spot-design.md b/planning/behavior_velocity_planner/occlusion-spot-design.md index 2cbc0d2368fd0..3516f6958c530 100644 --- a/planning/behavior_velocity_planner/occlusion-spot-design.md +++ b/planning/behavior_velocity_planner/occlusion-spot-design.md @@ -93,16 +93,16 @@ while ego is cruising from safe margin to collision path point ego keeps same ve | `delay_time` | double | [m/s] time buffer for the system delay | | `safe_margin` | double | [m] maximum error to stop with emergency braking system. | -| Parameter /sidewalk | Type | Description | -| ------------------------- | ------ | --------------------------------------------------------------- | -| `min_occlusion_spot_size` | double | [m] the length of path to consider occlusion spot | -| `slice_size` | double | [m] the distance of divided detection area | -| `focus_range` | double | [m] buffer around the ego path used to build the sidewalk area. | - -| Parameter /grid | Type | Description | -| ---------------- | ------ | --------------------------------------------------------------- | -| `free_space_max` | double | [-] maximum value of a free space cell in the occupancy grid | -| `occupied_min` | double | [-] buffer around the ego path used to build the sidewalk area. | +| Parameter /detection_area | Type | Description | +| ------------------------- | ------ | --------------------------------------------------------------------- | +| `min_occlusion_spot_size` | double | [m] the length of path to consider occlusion spot | +| `slice_length` | double | [m] the distance of divided detection area | +| `max_lateral_distance` | double | [m] buffer around the ego path used to build the detection_area area. | + +| Parameter /grid | Type | Description | +| ---------------- | ------ | --------------------------------------------------------------------- | +| `free_space_max` | double | [-] maximum value of a free space cell in the occupancy grid | +| `occupied_min` | double | [-] buffer around the ego path used to build the detection_area area. | #### Flowchart diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp index 4af5a95ce4413..22be533080647 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp @@ -152,7 +152,7 @@ visualization_msgs::msg::MarkerArray makePolygonMarker( debug_marker.scale = tier4_autoware_utils::createMarkerScale(0.1, 0.1, 0.1); debug_marker.color = tier4_autoware_utils::createMarkerColor(1.0, 0.0, 1.0, 0.3); debug_marker.lifetime = rclcpp::Duration::from_seconds(0.1); - debug_marker.ns = "sidewalk"; + debug_marker.ns = "detection_area"; for (const auto & poly : polygons) { for (const auto & p : poly) { geometry_msgs::msg::Point point = @@ -252,7 +252,8 @@ visualization_msgs::msg::MarkerArray OcclusionSpotInPrivateModule::createDebugMa visualization_msgs::msg::MarkerArray debug_marker_array; appendMarkerArray(createMarkers(debug_data_, module_id_), current_time, &debug_marker_array); appendMarkerArray( - makePolygonMarker(debug_data_.sidewalks, debug_data_.z), current_time, &debug_marker_array); + makePolygonMarker(debug_data_.detection_areas, debug_data_.z), current_time, + &debug_marker_array); appendMarkerArray( createPathMarkerArray(debug_data_.path_raw, "path_raw", 0, 0.0, 1.0, 1.0), current_time, &debug_marker_array); diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/geometry.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/geometry.cpp index 51cc5bf391c51..b4f36dd42e4da 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/geometry.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/geometry.cpp @@ -16,6 +16,7 @@ #include #include #include +#include #include #include @@ -57,7 +58,7 @@ void createOffsetLineString( void buildSlices( std::vector & slices, const lanelet::ConstLanelet & path_lanelet, const SliceRange & range, - const double slice_length, [[maybe_unused]] const double slice_width, const double resolution) + const double slice_length, const double resolution) { /** * @brief bounds @@ -105,24 +106,22 @@ void buildSlices( } void buildDetectionAreaPolygon( - std::vector & slices, const lanelet::ConstLanelet & path_lanelet, - const double longitudinal_offset, const double lateral_offset, const double slice_size, - const double lateral_max_dist) + std::vector & slices, const lanelet::ConstLanelet & path_lanelet, const double offset, + const PlannerParam & param) { std::vector left_slices; std::vector right_slices; - const double longitudinal_max_dist = lg::length2d(path_lanelet); + const double d_min = param.half_vehicle_width; + const double d_max = param.detection_area.max_lateral_distance; SliceRange left_slice_range = { - longitudinal_offset, longitudinal_max_dist, lateral_offset, lateral_offset + lateral_max_dist}; + offset + param.baselink_to_front, param.detection_area_length, d_min, d_max}; // in most case lateral distance is much more effective for velocity planning - const double slice_length = 4.0 * slice_size; - const double slice_width = slice_size; - const double resolution = 1.0; - buildSlices(left_slices, path_lanelet, left_slice_range, slice_length, slice_width, resolution); + const double slice_length = param.detection_area.slice_length; + const double resolution = 1.0; // interpolation interval TODO parametrize this + buildSlices(left_slices, path_lanelet, left_slice_range, slice_length, resolution); SliceRange right_slice_range = { - longitudinal_offset, longitudinal_max_dist, -lateral_offset, - -lateral_offset - lateral_max_dist}; - buildSlices(right_slices, path_lanelet, right_slice_range, slice_length, slice_width, resolution); + offset + param.baselink_to_front, param.detection_area_length, -d_min, -d_max}; + buildSlices(right_slices, path_lanelet, right_slice_range, slice_length, resolution); // Properly order lanelets from closest to furthest slices = left_slices; slices.insert(slices.end(), right_slices.begin(), right_slices.end()); diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/manager.cpp index 316873d80d751..7cffddef109f4 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/manager.cpp @@ -95,11 +95,12 @@ OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node) pp.v.safe_margin = node.declare_parameter(ns + ".motion.safe_margin", 1.0); pp.v.max_slow_down_accel = node.declare_parameter(ns + ".motion.max_slow_down_accel", -1.5); pp.v.min_allowed_velocity = node.declare_parameter(ns + ".motion.min_allowed_velocity", 1.0); - // sidewalk param - pp.sidewalk.min_occlusion_spot_size = - node.declare_parameter(ns + ".sidewalk.min_occlusion_spot_size", 2.0); - pp.sidewalk.focus_range = node.declare_parameter(ns + ".sidewalk.focus_range", 4.0); - pp.sidewalk.slice_size = node.declare_parameter(ns + ".sidewalk.slice_size", 1.5); + // detection_area param + pp.detection_area.min_occlusion_spot_size = + node.declare_parameter(ns + ".detection_area.min_occlusion_spot_size", 2.0); + pp.detection_area.max_lateral_distance = + node.declare_parameter(ns + ".detection_area.max_lateral_distance", 4.0); + pp.detection_area.slice_length = node.declare_parameter(ns + ".detection_area.slice_length", 1.5); // occupancy grid param pp.grid.free_space_max = node.declare_parameter(ns + ".grid.free_space_max", 10); pp.grid.occupied_min = node.declare_parameter(ns + ".grid.occupied_min", 51); diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp index 0ff465b424ae9..c4c3e9b000407 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp @@ -14,6 +14,7 @@ #include #include +#include #include #include #include @@ -415,7 +416,7 @@ void filterCollisionByRoadType( } } -void generateSidewalkPossibleCollisions( +void generateDetectionAreaPossibleCollisions( std::vector & possible_collisions, const grid_map::GridMap & grid, const PathWithLaneId & path, const double offset_from_start_to_ego, const PlannerParam & param, std::vector & debug) @@ -425,44 +426,45 @@ void generateSidewalkPossibleCollisions( return; } using Slice = occlusion_spot_utils::Slice; - std::vector sidewalk_slices; + std::vector detection_area_polygons; occlusion_spot_utils::buildDetectionAreaPolygon( - sidewalk_slices, path_lanelet, 0.0, param.half_vehicle_width, param.sidewalk.slice_size, - param.sidewalk.focus_range); + detection_area_polygons, path_lanelet, offset_from_start_to_ego, param); double length_lower_bound = std::numeric_limits::max(); double distance_lower_bound = std::numeric_limits::max(); // sort distance closest first to skip inferior collision - std::sort(sidewalk_slices.begin(), sidewalk_slices.end(), [](const Slice & s1, const Slice s2) { - return std::abs(s1.range.min_distance) < std::abs(s2.range.min_distance); - }); + std::sort( + detection_area_polygons.begin(), detection_area_polygons.end(), + [](const Slice & s1, const Slice s2) { + return std::abs(s1.range.min_distance) < std::abs(s2.range.min_distance); + }); - std::sort(sidewalk_slices.begin(), sidewalk_slices.end(), [](const Slice s1, const Slice s2) { - return s1.range.min_length < s2.range.min_length; - }); + std::sort( + detection_area_polygons.begin(), detection_area_polygons.end(), + [](const Slice s1, const Slice s2) { return s1.range.min_length < s2.range.min_length; }); - for (const Slice sidewalk_slice : sidewalk_slices) { - debug.push_back(sidewalk_slice.polygon); - if ((sidewalk_slice.range.min_length < length_lower_bound || - std::abs(sidewalk_slice.range.min_distance) < distance_lower_bound)) { + for (const Slice detection_area_slice : detection_area_polygons) { + debug.push_back(detection_area_slice.polygon); + if ((detection_area_slice.range.min_length < length_lower_bound || + std::abs(detection_area_slice.range.min_distance) < distance_lower_bound)) { std::vector occlusion_spot_positions; grid_utils::findOcclusionSpots( - occlusion_spot_positions, grid, sidewalk_slice.polygon, - param.sidewalk.min_occlusion_spot_size); - generateSidewalkPossibleCollisionFromOcclusionSpot( + occlusion_spot_positions, grid, detection_area_slice.polygon, + param.detection_area.min_occlusion_spot_size); + generateDetectionAreaPossibleCollisionFromOcclusionSpot( possible_collisions, grid, occlusion_spot_positions, offset_from_start_to_ego, path_lanelet, param); if (!possible_collisions.empty()) { - length_lower_bound = sidewalk_slice.range.min_length; - distance_lower_bound = std::abs(sidewalk_slice.range.min_distance); + length_lower_bound = detection_area_slice.range.min_length; + distance_lower_bound = std::abs(detection_area_slice.range.min_distance); possible_collisions.insert( possible_collisions.end(), possible_collisions.begin(), possible_collisions.end()); - debug.push_back(sidewalk_slice.polygon); + debug.push_back(detection_area_slice.polygon); } } } } -void generateSidewalkPossibleCollisionFromOcclusionSpot( +void generateDetectionAreaPossibleCollisionFromOcclusionSpot( std::vector & possible_collisions, const grid_map::GridMap & grid, const std::vector & occlusion_spot_positions, const double offset_from_start_to_ego, const lanelet::ConstLanelet & path_lanelet, diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.cpp index 2bd245a91f76b..e1cabf549d47d 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.cpp @@ -91,9 +91,9 @@ bool OcclusionSpotInPrivateModule::modifyPathVelocity( logger_, *clock_, 3000, "offset_from_start_to_ego : " << offset_from_start_to_ego); std::vector possible_collisions; // Note: Don't consider offset from path start to ego here - utils::generateSidewalkPossibleCollisions( + utils::generateDetectionAreaPossibleCollisions( possible_collisions, grid_map, interp_path, offset_from_start_to_ego, param_, - debug_data_.sidewalks); + debug_data_.detection_areas); utils::filterCollisionByRoadType(possible_collisions, focus_area); RCLCPP_DEBUG_STREAM_THROTTLE( logger_, *clock_, 3000, "num possible collision:" << possible_collisions.size()); diff --git a/planning/behavior_velocity_planner/test/src/test_grid_utils.cpp b/planning/behavior_velocity_planner/test/src/test_grid_utils.cpp index d0c169b2b1b7f..7fcd790190a49 100644 --- a/planning/behavior_velocity_planner/test/src/test_grid_utils.cpp +++ b/planning/behavior_velocity_planner/test/src/test_grid_utils.cpp @@ -206,48 +206,3 @@ TEST(isOcclusionSpotSquare, many_cells_occlusion_spot) } } } - -TEST(buildSlices, test_buffer_offset) -{ - using behavior_velocity_planner::occlusion_spot_utils::buildSlices; - using behavior_velocity_planner::occlusion_spot_utils::Slice; - using behavior_velocity_planner::occlusion_spot_utils::SliceRange; - - // Curving lanelet - // lanelet::Lanelet l; - // lanelet::Points3d line; - boost::geometry::model::linestring line; - boost::geometry::model::multi_polygon> - poly; - line.emplace_back(0.0, 1.0); - line.emplace_back(1.0, 4.0); - line.emplace_back(2.0, 6.0); - line.emplace_back(3.0, 7.0); - line.emplace_back(4.0, 6.0); - line.emplace_back(5.0, 4.0); - line.emplace_back(6.0, 1.0); - - boost::geometry::strategy::buffer::distance_asymmetric distance_strategy(0.0, 1.1); - boost::geometry::strategy::buffer::end_flat end_strategy; - boost::geometry::strategy::buffer::side_straight side_strategy; - boost::geometry::strategy::buffer::join_miter join_strategy; - boost::geometry::strategy::buffer::point_square point_strategy; - boost::geometry::buffer( - line, poly, distance_strategy, side_strategy, join_strategy, end_strategy, point_strategy); - - std::cout << boost::geometry::wkt(line) << "\n"; - std::cout << boost::geometry::wkt(poly[0]) << "\n"; - - boost::geometry::model::multi_point in1; - for (const auto & p : line) { - in1.push_back(p); - } - boost::geometry::model::multi_point in2; - for (const auto & p : poly[0].outer()) { - in2.push_back(p); - } - boost::geometry::model::multi_point output; - boost::geometry::difference(in2, in1, output); - - std::cout << boost::geometry::wkt(output) << "\n"; -}