feat: filter only lanelets close to ego (#366)

Author: hayato-m126

docs/use_case/images/search_range.jpg

@@ -28,6 +28,20 @@ Launching the file executes the following steps:
 4. The evaluation node passes the point cloud and the polygon of the non-detected area to perception_eval for evaluation. The result is dumped into a file.
 5. When the playback of the rosbag is finished, Autoware's launch is automatically terminated, and the evaluation is completed.
 
+### How to calculate polygon for non-detectable area
+
+The non-detect area is calculated as the area where the polygon given in the scenario overlaps with the road_lanelet.
+It is calculated according to the following steps:
+
+1. get the transform of map_to_base_link at the time of the header.stamp in pointcloud, and transform the polygon to the map coordinate system.
+2. get the road_lanelet in the range of search_range (see the figure below) from the point where the vehicle is.
+3. take the intersection of the road_lanelet and polygon obtained in step 2.
+4. return the array of polygons obtained in step 3 to the base_link coordinate system (to match the coordinate system to filter the pointcloud).
+
+![search_range](./images/search_range.jpg)
+
+In step 2, by narrowing down to the lanelets in the range where polygons can exist, the intersection process with lanelets that are obvious to return empty polygons in step 3 is omitted.
+
 ## Evaluation Result
 
 The results are calculated for each subscription. The format and available states are described below.

docs/use_case/obstacle_segmentation.en.md

@@ -27,6 +27,20 @@ launch を立ち上げると以下のことが実行され、評価される。
 4. 評価ノードが点群と非検知エリアのpolygonをperception_eval に渡して評価する。結果をファイルに記録する
 5. bag の再生が終了すると自動で launch が終了して評価が終了する
 
+### 非検知エリアのpolygonの計算方法
+
+非検知エリアは、シナリオで与えられたpolygonと、走行路(road_lanelet)の重なり合ったエリアとして算出される。
+以下のステップに従って計算される。
+
+1. pointcloudのheader.stampの時刻でのmap_to_base_linkのtransformを取得して、polygonをmap座標系に変換する
+2. 車両のいるpointから、search_range(下図参照)の範囲のroad_laneletを取得する
+3. 2で取得したroad_laneletとpolygonのintersectionを取る
+4. 3で取得したpolygonの配列をbase_link座標系へ戻す(pointcloudをフィルタするために座標系を一致させる)
+
+![search_range](./images/search_range.jpg)
+
+ステップ2で、polygonが存在し得る範囲のlaneletに絞ることで、ステップ3で空のpolygonが返ってくるとわかりきっているlaneletとのintersection処理を省いている。
+
 ## 評価結果
 
 topic の subscribe 1 回につき、以下に記述する判定結果が出力される。

docs/use_case/obstacle_segmentation.ja.md

@@ -88,6 +88,16 @@ def is_clockwise(cls, v: list[list[number]]) -> list | None:
     def to_float(cls, v: number) -> float:
         return float(v)
 
+    def search_range(self) -> float:
+        coord: list = []
+        for p in self.polygon_2d:
+            coord.append([p[0], p[1]])
+        poly = Polygon(coord)
+        min_x, min_y, max_x, max_y = poly.bounds
+        abs_max_x = max(abs(min_x), abs(max_x))
+        abs_max_y = max(abs(min_y), abs(max_y))
+        return np.sqrt(abs_max_x**2 + abs_max_y**2)
+
 
 class BoundingBoxCondition(BaseModel):
     Start: number | None = None
@@ -375,6 +385,11 @@ def get_non_detection_area_in_base_link(
     return line_strip, list_intersection_area
 
 
+def set_ego_point(map_to_baselink: TransformStamped) -> Point:
+    t = map_to_baselink.transform.translation
+    return Point(x=t.x, y=t.y, z=t.z)
+
+
 @dataclass
 class Detection(EvaluationItem):
     name: str = "Detection"

driving_log_replayer/driving_log_replayer/obstacle_segmentation.py

@@ -23,6 +23,8 @@
 from diagnostic_msgs.msg import DiagnosticStatus
 from geometry_msgs.msg import PoseStamped
 from geometry_msgs.msg import TransformStamped
+import lanelet2  # noqa
+from lanelet2_extension_python.utility.query import getLaneletsWithinRange
 import numpy as np
 from perception_eval.config import SensingEvaluationConfig
 from perception_eval.manager import SensingEvaluationManager
@@ -48,6 +50,7 @@
 from driving_log_replayer.obstacle_segmentation import get_sensing_frame_config
 from driving_log_replayer.obstacle_segmentation import ObstacleSegmentationResult
 from driving_log_replayer.obstacle_segmentation import ObstacleSegmentationScenario
+from driving_log_replayer.obstacle_segmentation import set_ego_point
 from driving_log_replayer.obstacle_segmentation import transform_proposed_area
 import driving_log_replayer.perception_eval_conversions as eval_conversions
 from driving_log_replayer_analyzer.data import convert_str_to_dist_type
@@ -80,7 +83,12 @@ def __init__(self, name: str) -> None:
             "lanelet2_map.osm",
         ).as_posix()
         self.__road_lanelets = road_lanelets_from_file(map_path)
-
+        if self._scenario.Evaluation.Conditions.NonDetection is not None:
+            self.__search_range = (
+                self._scenario.Evaluation.Conditions.NonDetection.ProposedArea.search_range()
+            )
+        else:
+            self.__search_range = 0.0
         self.declare_parameter("vehicle_model", "")
         self.__vehicle_model = (
             self.get_parameter("vehicle_model").get_parameter_value().string_value
@@ -320,7 +328,13 @@ def get_non_detection_area(
         )
         # get intersection
         marker_id = 0
-        for road in self.__road_lanelets:
+        ego_point = set_ego_point(map_to_baselink)
+        near_road_lanelets = getLaneletsWithinRange(
+            self.__road_lanelets,
+            ego_point,
+            self.__search_range,
+        )
+        for road in near_road_lanelets:
             poly_lanelet = to_shapely_polygon(road)
             i_area = poly_lanelet.intersection(proposed_area_in_map)
             if not i_area.is_empty:

driving_log_replayer/scripts/obstacle_segmentation_evaluator_node.py

@@ -12,10 +12,13 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+from geometry_msgs.msg import Point
+import lanelet2  # noqa
 from lanelet2.core import getId
 from lanelet2.core import Lanelet
 from lanelet2.core import LineString3d
 from lanelet2.core import Point3d
+from lanelet2_extension_python.utility.query import getLaneletsWithinRange
 from shapely.geometry import Polygon
 
 from driving_log_replayer.lanelet2_util import to_shapely_polygon
@@ -46,3 +49,15 @@ def test_intersection_no_overlapping() -> None:
     s_poly = Polygon(((-1.0, -1.0), (-1.0, -2.0), (-2.0, -2.0), (-2.0, -1.0)))
     i_area = l_poly.intersection(s_poly)
     assert i_area.is_empty
+
+
+def test_get_lanelets_within_range() -> None:
+    lanes = [get_a_lanelet(), get_a_lanelet(index=4)]
+    near_lanelets = getLaneletsWithinRange(lanes, Point(x=1.0, y=3.0, z=0.0), 1.0)
+    assert len(near_lanelets) == 2  # noqa
+
+
+def test_get_lanelets_within_range_no_lane() -> None:
+    lanes = [get_a_lanelet(), get_a_lanelet(index=4)]
+    near_lanelets = getLaneletsWithinRange(lanes, Point(x=1.0, y=3.0, z=0.0), 0.5)
+    assert len(near_lanelets) == 0

driving_log_replayer/test/unittest/test_lanelet2.py

@@ -623,3 +623,30 @@ def test_intersection_polygon_orientation() -> None:
     b = ShapelyPoint(2, 1).buffer(1.5)
     i_poly: Polygon = a.intersection(b)
     assert i_poly.exterior.is_ccw is False  # clockwise
+
+
+def test_search_range() -> None:
+    proposed_area = ProposedAreaCondition(
+        polygon_2d=[[1.0, 1.0], [2.0, -2.0], [-3.0, -3.0], [-2.0, 4.0]],
+        z_min=0.0,
+        z_max=2.0,
+    )
+    assert proposed_area.search_range() == 5.0  # noqa
+
+
+def draw_search_range() -> None:
+    from matplotlib import patches
+    from matplotlib import pyplot as plt
+
+    points = [[1.0, 1.0], [2.0, -2.0], [-3.0, -3.0], [-2.0, 4.0], [1.0, 1.0]]
+    poly = patches.Polygon(xy=points, closed=True)
+    bound = patches.Rectangle(xy=(-3, -3), width=5, height=7, ec="g", linewidth="2.0", fill=False)
+    search_range = patches.Circle(xy=(0, 0), radius=5, fill=False, ec="r")
+
+    _, ax = plt.subplots(figsize=(7, 7))
+    ax.add_patch(poly)
+    ax.add_patch(bound)
+    ax.add_patch(search_range)
+    ax.autoscale()
+    ax.grid()
+    plt.show()