added the alternative implementation for occlusions

nuplan/planning/simulation/occlusion/complete_shadow_occlusion_manager.py

@@ -9,6 +9,8 @@ from shapely.ops import unary_union
 from typing import List
 from collections import deque 
 import math
+import numpy as np
+
 import time
 
 
@@ -22,19 +24,20 @@ class CompleteShadowOcclusionManager(AbstractOcclusionManager):
         self,
         scenario: AbstractScenario,
         horizon_threshold: float = 4800, # meters since that is how far a standing human can see unblocked before the curvature of the earth cuts your line of sight
-        min_rad: float = 0.035 # minimum radians that the vehicle must take up to be observed (0.035 = aprox 2 degrees)
+        min_rad: float = 0.035, # minimum radians that the vehicle must take up to be observed (0.035 = aprox 2 degrees)
+        num_wedges: float = 180 # gives wedge width of roughly 2 degrees
+
     ):
         super().__init__(scenario)
         self.horizon_threshold = horizon_threshold
         self.min_rad = min_rad
+        self.num_wedges = num_wedges
     def _compute_visible_agents(self, ego_state: EgoState, observations: DetectionsTracks) -> set:
         """
         Returns set of track tokens that represents the observations visible to the ego
         at this time step.
         """
-
         # Visible track token set
-
         return self._determine_occlusions(ego_state.agent, observations.tracked_objects.tracked_objects)
     
     def _determine_occlusions(self, observer: AgentState, targets:List[AgentState]) -> set:
@@ -96,4 +99,43 @@ class CompleteShadowOcclusionManager(AbstractOcclusionManager):
             two_neighbors[1] = points[(ndx+2)%len(points)]
         two_neighbors[0] = Point(two_neighbors[0])
         two_neighbors[1] = Point(two_neighbors[1])
-        return two_neighbors
\ No newline at end of file
+        return two_neighbors
+
+    #alternative occlusion implementation. measured to be an order of magnitude slower
+    def _determine_occlusions_alt(self, observer: AgentState, targets:List[AgentState]) -> set:
+        start = time.time()
+        not_occluded = set() # Visible track token set
+        rads = np.linspace(0,2*math.pi,self.num_wedges+1)
+
+        for i in range(len(rads)-1):
+            build_wedge = time.time()
+
+            d1 = rads[i]#create wedge
+            d2 = rads[i+1]
+            p1 = (self.horizon_threshold * math.cos(d1), self.horizon_threshold * math.sin(d1))
+            p2 = (self.horizon_threshold * math.cos(d2), self.horizon_threshold * math.sin(d2))
+            wedge = Polygon([(0,0), p1, p2])
+            print('build_wedge', time.time() - build_wedge)
+
+
+            wedge_occupiers = []
+            target_looping = time.time()
+
+            collect_targets = time.time()
+            for target in targets:
+                corners_list = target.box.all_corners() #Return 4 corners of oriented box (FL, RL, RR, FR) Point2D
+                corners = []
+                for corner in corners_list:
+                    corners.append((corner.x - observer.center.x, corner.y - observer.center.y)) #we shift the corners and move them to a different data structure we can play with
+                target_poly = Polygon(corners)
+                if wedge.intersects(target_poly):
+                    wedge_occupiers.append(target)
+            print('collect_targets', time.time() - collect_targets)
+            
+            sort_time = time.time()
+            if len(wedge_occupiers) > 0:
+                wedge_occupiers.sort(key=lambda x: (x.center.x - observer.center.x)**2 + (x.center.y - observer.center.y)**2)
+                not_occluded.add(wedge_occupiers[0].metadata.track_token)            
+            print('sort_time', time.time() - sort_time)
+        print('elapsed time:', time.time() - start)
+        return not_occluded
\ No newline at end of file