From bc16a40380eda813d29ff8097cad2ba86dbb6e0d Mon Sep 17 00:00:00 2001 From: LucasEby Date: Sat, 22 Feb 2025 16:56:15 -0500 Subject: [PATCH 1/4] Putting this here for now not sure what the issue is --- .../perception/pedestrian_detection.py | 120 +++++------------- 1 file changed, 34 insertions(+), 86 deletions(-) diff --git a/GEMstack/onboard/perception/pedestrian_detection.py b/GEMstack/onboard/perception/pedestrian_detection.py index e145bcabf..7be481386 100644 --- a/GEMstack/onboard/perception/pedestrian_detection.py +++ b/GEMstack/onboard/perception/pedestrian_detection.py @@ -277,41 +277,32 @@ def update_object_states(self, track_result: List[Results], extracted_pts_all: L obj_centers_vehicle.append(np.array(())) obj_centers = obj_centers_vehicle - self.find_vels_and_ids(obj_centers, obj_dims) + agents = self.create_agent_states(obj_centers=obj_centers, obj_dims=obj_dims) + + self.find_vels_and_ids(agents=agents) # TODO: Refactor to make more efficient # TODO: Moving Average across last N iterations pos/vel? Less spurious vals # TODO Fix velocity calculation to calculate in ObjectFrameEnum.START - def find_vels_and_ids(self, obj_centers: List[np.ndarray], obj_dims: List[np.ndarray]): - # 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] + def find_vels_and_ids(self, agents: List[AgentState]): + # self.prev_agents was assigned a deepcopy of self.current_agents and then self.current_agents + # was cleared before this function was called # Nothing was scanned, erase current (for current output) and previous list (for next time through because nothing was scanned) - if (len(obj_dims) == 0 or len(obj_centers) == 0): + if (len(agents) == 0): self.current_agents = {} - self.prev_agents = {} return - - new_prev_agents = {} # Stores current agents in START frame for next time through (since - # planning wants us to send them agents in CURRENT frame) assigned = False - num_pairings = len(obj_centers) + num_agents = len(agents) - # Create a list of agent states in the start frame - agents_sf = self.create_agent_states_in_start_frame(obj_centers=obj_centers, obj_dims=obj_dims) # Create agents in start frame - - # Loop through the indexes of the obj_center and obj_dim pairings - for idx in range(num_pairings): + for idx in range(num_agents): assigned = False # Loop through previous agents backwards for prev_id, prev_state in reversed(self.prev_agents.items()): - # If an obj_center and obj_dim pair overlaps with a previous agent, assume that they're the same agent - if self.agents_overlap(agents_sf[idx], prev_state): + # If a scanned agent overlaps with a previous agent, assume that they're the same agent + if self.agents_overlap(agents[idx], prev_state): assigned = True if self.prev_time == None: @@ -319,60 +310,29 @@ def find_vels_and_ids(self, obj_centers: List[np.ndarray], obj_dims: List[np.nda vel = np.array([0, 0, 0]) else: delta_t = self.curr_time - self.prev_time - vel = (np.array([agents_sf[idx].pose.x, agents_sf[idx].pose.y, agents_sf[idx].pose.z]) - np.array([prev_state.pose.x, prev_state.pose.y, prev_state.pose.z])) / delta_t.total_seconds() + vel = (np.array([agents[idx].pose.x, agents[idx].pose.y, agents[idx].pose.z]) - np.array([prev_state.pose.x, prev_state.pose.y, prev_state.pose.z])) / delta_t.total_seconds() print("VELOCITY:") print(vel) - - # Create current frame agent at planning group's request: - self.current_agents[prev_id] = ( - AgentState( - track_id = prev_id, - pose=ObjectPose(t=(self.curr_time-self.t_start).total_seconds(), x=obj_centers[idx][0], y=obj_centers[idx][1], z=obj_centers[idx][2], yaw=0,pitch=0,roll=0,frame=ObjectFrameEnum.CURRENT), - # (l, w, h) - # TODO: confirm (z -> l, x -> w, y -> h) - dimensions=(obj_dims[idx][0], obj_dims[idx][1], obj_dims[idx][2]), - outline=None, - type=AgentEnum.PEDESTRIAN, - activity=AgentActivityEnum.STOPPED if np.all(vel == 0) else AgentActivityEnum.MOVING, - velocity=(0, 0, 0) if vel.size == 0 else tuple(vel), - yaw_rate=0 - )) # Fix start frame agent and store in this dict: - agents_sf[idx].track_id = prev_id - agents_sf[idx].activity = AgentActivityEnum.STOPPED if (np.all(vel == 0) or vel.size == 0) else AgentActivityEnum.MOVING - agents_sf[idx].velocity = (0, 0, 0) if vel.size == 0 else tuple(vel) + agents[idx].track_id = prev_id + agents[idx].activity = AgentActivityEnum.STOPPED if (np.all(vel == 0) or vel.size == 0) else AgentActivityEnum.MOVING + agents[idx].velocity = (0, 0, 0) if vel.size == 0 else tuple(vel) - new_prev_agents[prev_id] = agents_sf[idx] - del self.prev_agents[prev_id] # Remove previous agent from previous agents + self.current_agents[prev_id] = agents[idx] + del self.prev_agents[prev_id] # Remove previous agent from previous agents so it doesn't get assigned multiple times on accident break - - # If not assigned: + if not assigned: + print("not assigned") # Set velocity to 0 and assign the new agent a new id with IdTracker id = self.id_tracker.get_new_id() - # Create current frame agent at planning group's request: - self.current_agents[id] = ( - AgentState( - track_id = id, - pose=ObjectPose(t=(self.curr_time-self.t_start).total_seconds(), x=obj_centers[idx][0], y=obj_centers[idx][1], z=obj_centers[idx][2] ,yaw=0,pitch=0,roll=0,frame=ObjectFrameEnum.CURRENT), - # (l, w, h) - # TODO: confirm (z -> l, x -> w, y -> h) - dimensions=(obj_dims[idx][0], obj_dims[idx][1], obj_dims[idx][2]), - outline=None, - type=AgentEnum.PEDESTRIAN, - activity=AgentActivityEnum.UNDETERMINED, - velocity=(0, 0, 0), - yaw_rate=0 - )) - # Fix start frame agent and store in this dict: - agents_sf[idx].track_id = id - agents_sf[idx].activity = AgentActivityEnum.UNDETERMINED - agents_sf[idx].velocity = (0, 0, 0) - new_prev_agents[id] = agents_sf[idx] - self.prev_agents = new_prev_agents + agents[idx].track_id = id + agents[idx].activity = AgentActivityEnum.UNDETERMINED + agents[idx].velocity = (0, 0, 0) + self.current_agents[id] = agents[idx] # Calculates whether 2 agents overlap in START frame. True if they do, false if not def agents_overlap(self, curr_agent: AgentState, prev_agent: AgentState) -> bool: @@ -398,13 +358,19 @@ def agents_overlap(self, curr_agent: AgentState, prev_agent: AgentState) -> bool ( (z1_min <= z2_min and z2_min <= z1_max) or (z2_min <= z1_min and z1_min <= z2_max) ) ) - def create_agent_states_in_start_frame(self, obj_centers: List[np.ndarray], obj_dims: List[np.ndarray]) -> List[AgentState]: + def create_agent_states(self, obj_centers: List[np.ndarray], obj_dims: List[np.ndarray]) -> List[AgentState]: + # 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[idx].size == 0): + del obj_centers[idx] + del obj_dims[idx] + # Create list of agent states in current vehicle frame: agents = [] num_pairings = len(obj_centers) for idx in range(num_pairings): # Create agent in current frame: - state = AgentState( + agents.append(AgentState( track_id=0, # Temporary pose=ObjectPose(t=(self.curr_time - self.t_start).total_seconds(), x=obj_centers[idx][0], y=obj_centers[idx][1], z=obj_centers[idx][2], yaw=0,pitch=0,roll=0,frame=ObjectFrameEnum.CURRENT), # Beware: AgentState(PhysicalObject) builds bbox from @@ -412,32 +378,14 @@ def create_agent_states_in_start_frame(self, obj_centers: List[np.ndarray], obj_ # [-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][2], obj_dims[idx][0], obj_centers[idx][1] + obj_dims[idx][1]), + dimensions=(obj_dims[idx][0], obj_centers[idx][1] + obj_dims[idx][1], obj_dims[idx][2]), outline=None, type=AgentEnum.PEDESTRIAN, activity=AgentActivityEnum.UNDETERMINED, # Temporary velocity=None, # Temporary yaw_rate=0 - ) - - # Convert agent to start frame and add to agents list: - agents.append( - state - # state.to_frame( - # frame=ObjectFrameEnum.START, - # current_pose=self.current_vehicle_pose_sf, - # # current_pose=ObjectPose( - # # frame=ObjectFrameEnum.CURRENT, - # # t=(self.curr_time - self.t_start).total_seconds(), - # # x=state.pose.x, - # # y=state.pose.y, - # # z=state.pose.z - # # ), - # start_pose_abs=self.start_pose_abs - # ) - ) - - # Return the agent states converted to start frame: + )) + return agents def ouster_oak_callback(self, rgb_image_msg: Image, lidar_pc2_msg: PointCloud2): From 0de5980fbfa15dde2fc05b347b7b49f4555cbf61 Mon Sep 17 00:00:00 2001 From: LucasEby Date: Sat, 22 Feb 2025 17:48:37 -0500 Subject: [PATCH 2/4] Simplified velocity and id function --- .../onboard/perception/pedestrian_detection.py | 15 ++++++--------- 1 file changed, 6 insertions(+), 9 deletions(-) diff --git a/GEMstack/onboard/perception/pedestrian_detection.py b/GEMstack/onboard/perception/pedestrian_detection.py index 7be481386..f8d14fde3 100644 --- a/GEMstack/onboard/perception/pedestrian_detection.py +++ b/GEMstack/onboard/perception/pedestrian_detection.py @@ -34,6 +34,7 @@ from typing import List, Dict from collections import defaultdict from datetime import datetime +import copy # ROS, CV import rospy import message_filters @@ -238,9 +239,6 @@ def viz_object_states(self, cv_image, boxes, extracted_pts_all): def update_object_states(self, track_result: List[Results], extracted_pts_all: List[np.ndarray]) -> None: - # self.prev_agents = self.current_agents.copy() - self.current_agents.clear() - # Return if no track results available if track_result[0].boxes.id == None: return @@ -285,8 +283,7 @@ def update_object_states(self, track_result: List[Results], extracted_pts_all: L # TODO: Moving Average across last N iterations pos/vel? Less spurious vals # TODO Fix velocity calculation to calculate in ObjectFrameEnum.START def find_vels_and_ids(self, agents: List[AgentState]): - # self.prev_agents was assigned a deepcopy of self.current_agents and then self.current_agents - # was cleared before this function was called + self.current_agents.clear() # Nothing was scanned, erase current (for current output) and previous list (for next time through because nothing was scanned) if (len(agents) == 0): @@ -319,12 +316,11 @@ def find_vels_and_ids(self, agents: List[AgentState]): agents[idx].activity = AgentActivityEnum.STOPPED if (np.all(vel == 0) or vel.size == 0) else AgentActivityEnum.MOVING agents[idx].velocity = (0, 0, 0) if vel.size == 0 else tuple(vel) - self.current_agents[prev_id] = agents[idx] + self.current_agents[prev_id] = copy.deepcopy(agents[idx]) del self.prev_agents[prev_id] # Remove previous agent from previous agents so it doesn't get assigned multiple times on accident break if not assigned: - print("not assigned") # Set velocity to 0 and assign the new agent a new id with IdTracker id = self.id_tracker.get_new_id() @@ -332,7 +328,8 @@ def find_vels_and_ids(self, agents: List[AgentState]): agents[idx].track_id = id agents[idx].activity = AgentActivityEnum.UNDETERMINED agents[idx].velocity = (0, 0, 0) - self.current_agents[id] = agents[idx] + self.current_agents[id] = copy.deepcopy(agents[idx]) + self.prev_agents = copy.deepcopy(self.current_agents) # Calculates whether 2 agents overlap in START frame. True if they do, false if not def agents_overlap(self, curr_agent: AgentState, prev_agent: AgentState) -> bool: @@ -350,7 +347,7 @@ def agents_overlap(self, curr_agent: AgentState, prev_agent: AgentState) -> bool x2_min, x2_max = prev_agent.pose.x - prev_agent.dimensions[0] / 2.0, prev_agent.pose.x + prev_agent.dimensions[0] / 2.0 y2_min, y2_max = prev_agent.pose.y, prev_agent.pose.y + prev_agent.dimensions[1] # AGENT STATE CALCULATION z2_min, z2_max = prev_agent.pose.z - prev_agent.dimensions[2] / 2.0, prev_agent.pose.z + prev_agent.dimensions[2] / 2.0 - + # True if they overlap, false if not return ( ( (x1_min <= x2_min and x2_min <= x1_max) or (x2_min <= x1_min and x1_min <= x2_max) ) and From 410e13e6120c0ec7a2a853310f50a048d294f73a Mon Sep 17 00:00:00 2001 From: LucasEby Date: Sun, 23 Feb 2025 12:02:44 -0500 Subject: [PATCH 3/4] Simplified and fixed agent state dimensions --- .../onboard/perception/pedestrian_detection.py | 18 ++++++------------ 1 file changed, 6 insertions(+), 12 deletions(-) diff --git a/GEMstack/onboard/perception/pedestrian_detection.py b/GEMstack/onboard/perception/pedestrian_detection.py index f8d14fde3..1f6c1eb9c 100644 --- a/GEMstack/onboard/perception/pedestrian_detection.py +++ b/GEMstack/onboard/perception/pedestrian_detection.py @@ -137,6 +137,7 @@ def update(self, vehicle : VehicleState) -> Dict[str,AgentState]: # self.start_pose_abs = vehicle.pose # Store first pose which is start frame # self.start_pose_abs.to_frame(frame=ObjectFrameEnum.GLOBAL, current_pose=) # self.current_vehicle_pose_sf = vehicle.pose # Vehicle pose in start frame + self.current_agents.clear() return self.current_agents # TODO: Improve Algo Knn, ransac, etc. @@ -283,8 +284,6 @@ def update_object_states(self, track_result: List[Results], extracted_pts_all: L # TODO: Moving Average across last N iterations pos/vel? Less spurious vals # TODO Fix velocity calculation to calculate in ObjectFrameEnum.START def find_vels_and_ids(self, agents: List[AgentState]): - self.current_agents.clear() - # Nothing was scanned, erase current (for current output) and previous list (for next time through because nothing was scanned) if (len(agents) == 0): self.current_agents = {} @@ -339,14 +338,9 @@ def agents_overlap(self, curr_agent: AgentState, prev_agent: AgentState) -> bool # [-l/2,l/2] x [-w/2,w/2] x [-h/2,h/2] # TODO: confirm (z -> l, x -> w, y -> h) - # Calculate corners of obj_center and obj_dim pairing - x1_min, x1_max = curr_agent.pose.x - curr_agent.dimensions[0] / 2.0, curr_agent.pose.x + curr_agent.dimensions[0] / 2.0 - y1_min, y1_max = curr_agent.pose.y, curr_agent.pose.y + curr_agent.dimensions[1] # AGENT STATE CALCULATION - z1_min, z1_max = curr_agent.pose.z - curr_agent.dimensions[2] / 2.0, curr_agent.pose.z + curr_agent.dimensions[2] / 2.0 - - x2_min, x2_max = prev_agent.pose.x - prev_agent.dimensions[0] / 2.0, prev_agent.pose.x + prev_agent.dimensions[0] / 2.0 - y2_min, y2_max = prev_agent.pose.y, prev_agent.pose.y + prev_agent.dimensions[1] # AGENT STATE CALCULATION - z2_min, z2_max = prev_agent.pose.z - prev_agent.dimensions[2] / 2.0, prev_agent.pose.z + prev_agent.dimensions[2] / 2.0 + # Extract the corners of the agents: + (x1_min, x1_max), (y1_min, y1_max), (z1_min, z1_max) = curr_agent.bounds() # Bounds of current agent + (x2_min, x2_max), (y2_min, y2_max), (z2_min, z2_max) = prev_agent.bounds() # Bounds of previous agent # True if they overlap, false if not return ( @@ -369,13 +363,13 @@ def create_agent_states(self, obj_centers: List[np.ndarray], obj_dims: List[np.n # Create agent in current frame: agents.append(AgentState( track_id=0, # Temporary - pose=ObjectPose(t=(self.curr_time - self.t_start).total_seconds(), x=obj_centers[idx][0], y=obj_centers[idx][1], z=obj_centers[idx][2], yaw=0,pitch=0,roll=0,frame=ObjectFrameEnum.CURRENT), + pose=ObjectPose(t=(self.curr_time - self.t_start).total_seconds(), x=obj_centers[idx][0], y=obj_centers[idx][1], z=obj_centers[idx][2] - obj_dims[idx][2], yaw=0,pitch=0,roll=0,frame=ObjectFrameEnum.CURRENT), # Beware: AgentState(PhysicalObject) builds bbox from # dims [-l/2,l/2] x [-w/2,w/2] x [0,h], not # [-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_centers[idx][1] + obj_dims[idx][1], obj_dims[idx][2]), + dimensions=(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 From ed5db7875f0b7e8f637d9d4cde4d553470527b5d Mon Sep 17 00:00:00 2001 From: LucasEby Date: Sun, 23 Feb 2025 12:19:30 -0500 Subject: [PATCH 4/4] Fixed a dict clearing issue --- GEMstack/onboard/perception/pedestrian_detection.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/GEMstack/onboard/perception/pedestrian_detection.py b/GEMstack/onboard/perception/pedestrian_detection.py index 1f6c1eb9c..ee041d3a2 100644 --- a/GEMstack/onboard/perception/pedestrian_detection.py +++ b/GEMstack/onboard/perception/pedestrian_detection.py @@ -137,7 +137,6 @@ def update(self, vehicle : VehicleState) -> Dict[str,AgentState]: # self.start_pose_abs = vehicle.pose # Store first pose which is start frame # self.start_pose_abs.to_frame(frame=ObjectFrameEnum.GLOBAL, current_pose=) # self.current_vehicle_pose_sf = vehicle.pose # Vehicle pose in start frame - self.current_agents.clear() return self.current_agents # TODO: Improve Algo Knn, ransac, etc. @@ -284,6 +283,7 @@ def update_object_states(self, track_result: List[Results], extracted_pts_all: L # TODO: Moving Average across last N iterations pos/vel? Less spurious vals # TODO Fix velocity calculation to calculate in ObjectFrameEnum.START def find_vels_and_ids(self, agents: List[AgentState]): + self.current_agents.clear() # Nothing was scanned, erase current (for current output) and previous list (for next time through because nothing was scanned) if (len(agents) == 0): self.current_agents = {}