Skip to content

G8 simplified velocity and ids #128

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
LucasEby merged 4 commits into from
Feb 23, 2025
Merged

G8 simplified velocity and ids #128

Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
bc16a40
Putting this here for now not sure what the issue is
LucasEby Feb 22, 2025
0de5980
Simplified velocity and id function
LucasEby Feb 22, 2025
410e13e
Simplified and fixed agent state dimensions
LucasEby Feb 23, 2025
ed5db78
Fixed a dict clearing issue
LucasEby Feb 23, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
137 changes: 38 additions & 99 deletions GEMstack/onboard/perception/pedestrian_detection.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -277,102 +275,60 @@ 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.current_agents.clear()
# 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)

# 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
num_agents = len(agents)

# 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:
# This will be triggered if the very first message has pedestrians in it
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] = 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:

if 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] = 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:
Expand All @@ -382,62 +338,45 @@ 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
# 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

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
( (y1_min <= y2_min and y2_min <= y1_max) or (y2_min <= y1_min and y1_min <= y2_max) ) and
( (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),
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][2], obj_dims[idx][0], obj_centers[idx][1] + obj_dims[idx][1]),
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
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):
Expand Down