pedestrian_detection.py Convert obj_dims to start frame aligned bbox

GEMstack/onboard/perception/pedestrian_detection.py

@@ -167,14 +167,11 @@ def update(self, vehicle : VehicleState) -> Dict[str,AgentState]:
         if(self.current_agent_obj_dims == {}):
             return self.current_agents
 
-        print(f"Global state : {vehicle}")
+
+        (f"Global state : {vehicle}")
 
         # Convert pose to start state. Need to use previous_vehicle state as pedestrian info is delayed
         vehicle_start_pose = vehicle.pose.to_frame(ObjectFrameEnum.START,self.previous_vehicle_state.pose,self.start_pose_abs)
-        print(f"Start state : {vehicle_start_pose}")
-
-        print(f"ped pose vehicle state = {self.current_agent_obj_dims['pose']}")
-        print(f"ped dimenstions vehicle state = {self.current_agent_obj_dims['dims']}")
 
         # converting to start frame
         for i,pose in enumerate(self.current_agent_obj_dims['pose']):
@@ -183,10 +180,6 @@ def update(self, vehicle : VehicleState) -> Dict[str,AgentState]:
         for i,dims in enumerate(self.current_agent_obj_dims['dims']):
             self.current_agent_obj_dims['dims'][i] = np.array(vehicle_start_pose.apply(dims))
 
-
-        print(f"ped pose start state = {self.current_agent_obj_dims['pose']}")
-        print(f"ped dimenstions start state = {self.current_agent_obj_dims['dims']}")
-
         # Prepare variables for find_vels_and_ids
         self.prev_agents = self.current_agents.copy()
         self.current_agents.clear()
@@ -219,9 +212,18 @@ def find_dims(self, clusters: List[List[np.ndarray]]) -> List[np.ndarray]:
         # objects distance to ground we filtered from lidar, 
         # other heuristics to imrpove stability for find_ funcs ?
         clusters = [np.array(clust) for clust in clusters]
+        # x, y, z 1 value 
         dims = [np.array(()) if clust.size == 0 else np.max(clust, axis= 0) - np.min(clust, axis= 0) for clust in clusters]
+        # x, y, z convert to 2 values around origin
+        for i in range(len(dims)):
+            if dims[i].size() == 0: continue
+            else:
+                dims[i] = np.vstack(0-dims[i]/2, dims[i]/2)
+
+        # Dims are 2 points: [ [min_x, min_y, min_z], [max_x, max_y, max_z] ]
+        # Need that for transform
         return dims
-    
+
     # TODO: Separate debug/viz class, bbox and 2d 3d points funcs 
     def viz_object_states(self, cv_image, boxes, extracted_pts_all):
         # Extract 3D pedestrians points in lidar frame
@@ -305,6 +307,7 @@ def update_object_states(self, track_result: List[Results], extracted_pts_all: L
         # TODO: Combine funcs for efficiency in C.
         #       Separate numpy prob still faster for now
         obj_centers = self.find_centers(pedestrians_3d_pts) # Centers are calculated in lidar frame here
+        # Dims are 2 points: [ [min_x, min_y, min_z], [max_x, max_y, max_z] ]
         obj_dims = self.find_dims(pedestrians_3d_pts)
         # Pose is stored in vehicle frame and converted to start frame in the update function 
         obj_centers_vehicle = []
@@ -318,8 +321,19 @@ def update_object_states(self, track_result: List[Results], extracted_pts_all: L
         obj_centers = obj_centers_vehicle
 
         if(len(obj_center) != 0) and (len(obj_dims) != 0):
-            self.current_agent_obj_dims["pose"] = obj_center
-            self.current_agent_obj_dims["dims"] = obj_dims
+          obj_dims_vehicle = []
+          for obj_dim in obj_dims:
+              if len(obj_dim) > 0:
+                  obj_dim_min = np.array([obj_dim[0]])
+                  obj_dim_max = np.array([obj_dim[1]])
+                  obj_dim_min_vehicle = transform_lidar_points(obj_dim_min, self.R_lidar_to_vehicle, self.t_lidar_to_vehicle)[0]
+                  obj_dim_max_vehicle = transform_lidar_points(obj_dim_max, self.R_lidar_to_vehicle, self.t_lidar_to_vehicle)[0]
+              else: obj_dims_vehicle.append(np.array(()))
+          obj_dims = obj_dims_vehicle
+
+          self.current_agent_obj_dims["pose"] = obj_centers
+          self.current_agent_obj_dims["dims"] = obj_dims
+
 
     # TODO: Refactor to make more efficient
     # TODO: Moving Average across last N iterations pos/vel? Less spurious vals
@@ -392,14 +406,19 @@ def agents_overlap(self, curr_agent: AgentState, prev_agent: AgentState) -> bool
         )
 
     def create_agent_states(self, obj_centers: List[np.ndarray], obj_dims: List[np.ndarray]) -> List[AgentState]:
+        # Dims are 2 points: [ [min_x, min_y, min_z], [max_x, max_y, max_z] ]
+
         # Gate to check whether dims and centers are empty (will happen if no pedestrians are scanned):
         for idx in range(len(obj_dims) -1, -1, -1):
             if (obj_centers[idx].size == 0) or (obj_dims[0].size == 0):
                 del obj_centers[idx]
                 del obj_dims[idx]
-            elif (obj_centers[idx].size != obj_dims[0].size):
-                del obj_centers[idx]
-                del obj_dims[idx]
+
+        # Convert from 2 point to 1 point dims
+        obj_dims = [np.abs( dim[0] - dim[1])  for dim in obj_dims]   
+       #     elif (obj_centers[idx].size != obj_dims[0].size):
+       #         del obj_centers[idx]
+       #         del obj_dims[idx]
         # Create list of agent states in current vehicle frame:
         agents = []
         num_pairings = len(obj_centers)
@@ -413,7 +432,7 @@ def create_agent_states(self, obj_centers: List[np.ndarray], obj_dims: List[np.n
                             # [-l/2,l/2] x [-w/2,w/2] x [-h/2,h/2]
                             # (l, w, h)
                             # TODO: confirm (z -> l, x -> w, y -> h)
-                            dimensions=(obj_dims[idx][0], obj_dims[idx][1], obj_centers[idx][2] + obj_dims[idx][2]),  
+                            dimensions=(obj_dims[idx][2], obj_dims[idx][0], obj_centers[idx][1]),   # obj_dims[idx][0], obj_dims[idx][1], obj_centers[idx][2] + obj_dims[idx][2]
                             outline=None,
                             type=AgentEnum.PEDESTRIAN,
                             activity=AgentActivityEnum.UNDETERMINED, # Temporary