From 3c19b465cee69100621a243e917c9644a0126cf4 Mon Sep 17 00:00:00 2001 From: Kris Hauser Date: Mon, 3 Feb 2025 12:31:56 -0500 Subject: [PATCH 1/9] Better docstring --- GEMstack/utils/config.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/GEMstack/utils/config.py b/GEMstack/utils/config.py index 9112107e9..ac7fbbaa7 100644 --- a/GEMstack/utils/config.py +++ b/GEMstack/utils/config.py @@ -18,7 +18,7 @@ def save_config(fn : str, config : dict) -> None: def load_config_recursive(fn : str) -> dict: - """Loads a configuration file with !include directives.""" + """Loads a configuration file with !include and !relative_path directives.""" if fn.endswith('yaml') or fn.endswith('yml'): with open(fn,'r') as f: res = yaml.load(f,_Loader) @@ -35,7 +35,7 @@ def load_config_recursive(fn : str) -> dict: class _Loader(yaml.SafeLoader): - """YAML Loader with `!include` constructor.""" + """YAML Loader with `!include` and `!relative_path` directives.""" def __init__(self, stream: IO) -> None: """Initialise Loader.""" @@ -60,7 +60,7 @@ def _construct_relative_path(loader: _Loader, node: yaml.Node) -> Any: yaml.add_constructor('!relative_path', _construct_relative_path, _Loader) def _load_config_or_text_recursive(fn : str) -> dict: - """Loads a configuration file with !include directives.""" + """Loads a configuration file with !include and !relative_path directives.""" if fn.endswith('yaml') or fn.endswith('yml'): with open(fn,'r') as f: res = yaml.load(f,_Loader) From 26eb65019b9c6a1f9a0fb844429edb1db0c74f07 Mon Sep 17 00:00:00 2001 From: Kris Hauser Date: Mon, 3 Feb 2025 12:56:54 -0500 Subject: [PATCH 2/9] Fixed to work with Klampt 0.10 --- .../visualization/klampt_visualization.py | 40 +++++++++++++------ 1 file changed, 28 insertions(+), 12 deletions(-) diff --git a/GEMstack/onboard/visualization/klampt_visualization.py b/GEMstack/onboard/visualization/klampt_visualization.py index 3e07aed23..762a747b4 100644 --- a/GEMstack/onboard/visualization/klampt_visualization.py +++ b/GEMstack/onboard/visualization/klampt_visualization.py @@ -1,4 +1,5 @@ from ..component import Component +from klampt import __version__ as klampt_version from klampt import vis from klampt.math import se3 from klampt import * @@ -11,14 +12,16 @@ import numpy as np class KlamptVisualization(Component): - """Runs a matplotlib visualization at 10Hz. - - If save_as is not None, saves the visualization to a file. + """Runs a Klampt visualization. + + Runs at 20Hz by default. """ def __init__(self, vehicle_interface, rate : float = 20.0, save_as : str = None): self.vehicle_interface = vehicle_interface self._rate = rate self.save_as = save_as + if save_as is not None: + print("WARNING: automatic saving of KlamptVisualization to movie is not supported yet. You can use Ctrl+M to start / stop saving the movie") self.num_updates = 0 self.last_yaw = None self.plot_values = {} @@ -57,12 +60,20 @@ def state_inputs(self): def initialize(self): vis.setWindowTitle("GEMstack visualization") vp = vis.getViewport() - vp.camera.rot[1] = -0.15 - vp.camera.rot[2] = -math.pi/2 - vp.camera.dist = 30.0 - vp.w = 1280 - vp.h = 720 - vp.clippingplanes = (0.1,1000) + if klampt_version == '0.10.0': + vp.controller.rot[1] = -0.15 + vp.controller.rot[2] = -math.pi/2 + vp.controller.dist = 30.0 + vp.resize(1280,720) + vp.n = 0.1 + vp.f = 1000 + else: + vp.camera.rot[1] = -0.15 + vp.camera.rot[2] = -math.pi/2 + vp.camera.dist = 30.0 + vp.w = 1280 + vp.h = 720 + vp.clippingplanes = (0.1,1000) vis.setViewport(vp) vis.add("vehicle_plane",self.world.terrain(0),hide_label=True) #note: show() takes over the interrupt handler and sets it to default, so we restore it @@ -121,9 +132,14 @@ def update(self, state): center_offset = 1.0 lookahead = 4.0*v dx,dy = math.cos(tracked_vehicle.pose.yaw)*(lookahead+center_offset),math.sin(tracked_vehicle.pose.yaw)*(lookahead+center_offset) - vp.camera.tgt = [tracked_vehicle.pose.x+dx,tracked_vehicle.pose.y+dy,1.5] - vp.camera.rot[2] += tracked_vehicle.pose.yaw - self.last_yaw - vp.camera.dist += 5.0*(v - self.last_v) + if klampt_version == '0.10.0': + vp.controller.tgt = [tracked_vehicle.pose.x+dx,tracked_vehicle.pose.y+dy,1.5] + vp.controller.rot[2] += tracked_vehicle.pose.yaw - self.last_yaw + vp.controller.dist += 5.0*(v - self.last_v) + else: + vp.camera.tgt = [tracked_vehicle.pose.x+dx,tracked_vehicle.pose.y+dy,1.5] + vp.camera.rot[2] += tracked_vehicle.pose.yaw - self.last_yaw + vp.camera.dist += 5.0*(v - self.last_v) self.last_v = v vis.setViewport(vp) From 503a403e05e46eb1d09706918ce06c9a146b2bf6 Mon Sep 17 00:00:00 2001 From: Kris Hauser Date: Mon, 17 Feb 2025 09:39:38 -0600 Subject: [PATCH 3/9] Fixed ROS type typo --- GEMstack/onboard/interface/gem_hardware.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/GEMstack/onboard/interface/gem_hardware.py b/GEMstack/onboard/interface/gem_hardware.py index 3f2f26994..e13ff817e 100644 --- a/GEMstack/onboard/interface/gem_hardware.py +++ b/GEMstack/onboard/interface/gem_hardware.py @@ -186,7 +186,7 @@ def callback_with_gnss_reading(msg: INSNavGeod): ) speed = np.sqrt(msg.ve**2 + msg.vn**2) callback(GNSSReading(pose,speed,('error' if msg.error else 'ok'))) - self.gnss_sub = rospy.Subscriber(topic, Inspva, callback_with_gnss_reading) + self.gnss_sub = rospy.Subscriber(topic, INSNavGeod, callback_with_gnss_reading) elif name == 'top_lidar': topic = self.ros_sensor_topics[name] if type is not None and (type is not PointCloud2 and type is not np.ndarray): From dc21e7e9a367bf0db7f66d1277bb87bdb8547a31 Mon Sep 17 00:00:00 2001 From: Kris Hauser Date: Mon, 17 Feb 2025 09:39:38 -0600 Subject: [PATCH 4/9] Fixed ROS type typo --- GEMstack/onboard/interface/gem_hardware.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/GEMstack/onboard/interface/gem_hardware.py b/GEMstack/onboard/interface/gem_hardware.py index 3f2f26994..e13ff817e 100644 --- a/GEMstack/onboard/interface/gem_hardware.py +++ b/GEMstack/onboard/interface/gem_hardware.py @@ -186,7 +186,7 @@ def callback_with_gnss_reading(msg: INSNavGeod): ) speed = np.sqrt(msg.ve**2 + msg.vn**2) callback(GNSSReading(pose,speed,('error' if msg.error else 'ok'))) - self.gnss_sub = rospy.Subscriber(topic, Inspva, callback_with_gnss_reading) + self.gnss_sub = rospy.Subscriber(topic, INSNavGeod, callback_with_gnss_reading) elif name == 'top_lidar': topic = self.ros_sensor_topics[name] if type is not None and (type is not PointCloud2 and type is not np.ndarray): From 8031e3295eedcff795a2767a1d141f566ffa12d1 Mon Sep 17 00:00:00 2001 From: Kris Hauser Date: Mon, 17 Feb 2025 22:22:08 -0500 Subject: [PATCH 5/9] Typo --- GEMstack/knowledge/calibration/gem_e4.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/GEMstack/knowledge/calibration/gem_e4.yaml b/GEMstack/knowledge/calibration/gem_e4.yaml index da971c253..d0954dc06 100644 --- a/GEMstack/knowledge/calibration/gem_e4.yaml +++ b/GEMstack/knowledge/calibration/gem_e4.yaml @@ -1,7 +1,7 @@ calibration_date: "2024-03-05" # Date of calibration YYYY-MM-DD reference: rear_axle_center # rear axle center rear_axle_height: 0.33 # height of rear axle center above flat ground -gnss_location: [1.10,0.1.62] # meters, taken from https://github.com/hangcui1201/POLARIS_GEM_e2_Real/blob/main/vehicle_drivers/gem_gnss_control/scripts/gem_gnss_tracker_stanley_rtk.py. Note conflict with pure pursuit location? +gnss_location: [1.10,0,1.62] # meters, taken from https://github.com/hangcui1201/POLARIS_GEM_e2_Real/blob/main/vehicle_drivers/gem_gnss_control/scripts/gem_gnss_tracker_stanley_rtk.py. Note conflict with pure pursuit location? gnss_yaw: 0.0 # radians top_lidar: !include "gem_e4_ouster.yaml" front_camera: !include "gem_e4_oak.yaml" From bc16a40380eda813d29ff8097cad2ba86dbb6e0d Mon Sep 17 00:00:00 2001 From: LucasEby Date: Sat, 22 Feb 2025 16:56:15 -0500 Subject: [PATCH 6/9] 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 7/9] 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 8/9] 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 9/9] 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 = {}