Skip to content

Vehicle to Start Frame Gen #125

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

Closed
nvikramraj wants to merge 3 commits into from
Closed

Vehicle to Start Frame Gen #125

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
57811aa
integrated t vehicle to start
nvikramraj Feb 20, 2025
7e0a49f
syntax error fix
nvikramraj Feb 20, 2025
57c54b9
code refactor for start_frame
nvikramraj Feb 22, 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
35 changes: 35 additions & 0 deletions GEMstack/onboard/interface/gem_hardware.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@
import rospy
from std_msgs.msg import String, Bool, Float32, Float64
from sensor_msgs.msg import Image,PointCloud2
import message_filters
try:
from novatel_gps_msgs.msg import NovatelPosition, NovatelXYZ, Inspva
except ImportError:
Expand Down Expand Up @@ -55,6 +56,7 @@ def __init__(self):
self.front_depth_sub = None
self.top_lidar_sub = None
self.stereo_sub = None
self.sync = None
self.faults = []

# -------------------- PACMod setup --------------------
Expand Down Expand Up @@ -227,6 +229,39 @@ def callback_with_cv2(msg : Image):
cv_image = conversions.ros_Image_to_cv2(msg, desired_encoding="passthrough")
callback(cv_image)
self.front_depth_sub = rospy.Subscriber(topic, Image, callback_with_cv2)
elif name == 'sensor_fusion_Lidar_Camera_GNSS':
def callback_with_cv2_numpy_gnss(rgb_image_msg: Image, lidar_pc2_msg: PointCloud2, gnss_msg: INSNavGeod):
points = conversions.ros_PointCloud2_to_numpy(lidar_pc2_msg, want_rgb=False)
cv_image = conversions.ros_Image_to_cv2(rgb_image_msg, desired_encoding="bgr8")
pose = ObjectPose(ObjectFrameEnum.GLOBAL,
x=gnss_msg.longitude,
y=gnss_msg.latitude,
z=gnss_msg.height,
yaw=math.radians(gnss_msg.heading), #heading from north in degrees (TODO: maybe?? check this)
roll=math.radians(gnss_msg.roll),
pitch=math.radians(gnss_msg.pitch),
)
speed = np.sqrt(gnss_msg.ve**2 + gnss_msg.vn**2)
callback(cv_image,points,GNSSReading(pose,speed,('error' if gnss_msg.error else 'ok')))
topic_camera = self.ros_sensor_topics['front_camera']
topic_lidar = self.ros_sensor_topics['top_lidar']
topic_gnss = self.ros_sensor_topics['gnss']
self.front_camera_sub = message_filters.Subscriber(topic_camera, Image)
self.top_lidar_sub = message_filters.Subscriber(topic_lidar, PointCloud2)
self.gnss_sub = message_filters.Subscriber(topic_gnss, INSNavGeod)
self.sync = message_filters.ApproximateTimeSynchronizer([self.front_camera_sub, self.top_lidar_sub,self.gnss_sub], queue_size=10, slop=0.1)
self.sync.registerCallback(callback_with_cv2_numpy_gnss)
elif name == 'sensor_fusion_Lidar_Camera':
def callback_with_cv2_numpy(rgb_image_msg: Image, lidar_pc2_msg: PointCloud2):
points = conversions.ros_PointCloud2_to_numpy(lidar_pc2_msg, want_rgb=False)
cv_image = conversions.ros_Image_to_cv2(rgb_image_msg, desired_encoding="bgr8")
callback(cv_image,points)
topic_camera = self.ros_sensor_topics['front_camera']
topic_lidar = self.ros_sensor_topics['top_lidar']
self.front_camera_sub = message_filters.Subscriber(topic_camera, Image)
self.top_lidar_sub = message_filters.Subscriber(topic_lidar, PointCloud2)
self.sync = message_filters.ApproximateTimeSynchronizer([self.front_camera_sub, self.top_lidar_sub], queue_size=10, slop=0.1)
self.sync.registerCallback(callback_with_cv2_numpy)


# PACMod enable callback function
Expand Down
200 changes: 125 additions & 75 deletions GEMstack/onboard/perception/pedestrian_detection.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -47,9 +47,10 @@
# GEMStack
from ...state import AllState,VehicleState,ObjectPose,ObjectFrameEnum,AgentState,AgentEnum,AgentActivityEnum,ObjectFrameEnum
from .pedestrian_detection_utils import *
from ..interface.gem import GEMInterface
from ..interface.gem import GEMInterface, GNSSReading
from ..component import Component
from .IdTracker import IdTracker
from scipy.spatial.transform import Rotation as R

def box_to_fake_agent(box):
"""Creates a fake agent state from an (x,y,w,h) bounding box.
Expand Down Expand Up @@ -80,6 +81,7 @@ def __init__(self, vehicle_interface : GEMInterface) -> None:
# track_id: AgentState
self.prev_agents = dict()
self.current_agents = dict()
self.current_agent_obj_dims = dict()
# TODO Implement time
self.confidence = 0.7
self.classes_to_detect = 0
Expand All @@ -98,11 +100,23 @@ def __init__(self, vehicle_interface : GEMInterface) -> None:
[-0.02289521, 0.00333035, 0.9997323]])
self.t_lidar_to_vehicle = np.array([[0.0], [0.0], [0.35]])

# Subscribers and sychronizers
self.rgb_rosbag = message_filters.Subscriber('/oak/rgb/image_raw', Image)
self.top_lidar_rosbag = message_filters.Subscriber('/ouster/points', PointCloud2)
self.sync = message_filters.ApproximateTimeSynchronizer([self.rgb_rosbag, self.top_lidar_rosbag], queue_size=10, slop=0.1)
self.sync.registerCallback(self.ouster_oak_callback)
# To calculate vehicle to start frame data
self.t_start_to_world = None
self.t_vehicle_to_world = None
self.t_vehicle_to_start = None

# Rospy Subscribers and sychronizers
# self.rgb_rosbag = message_filters.Subscriber('/oak/rgb/image_raw', Image)
# self.top_lidar_rosbag = message_filters.Subscriber('/ouster/points', PointCloud2)
# self.sync = message_filters.ApproximateTimeSynchronizer([self.rgb_rosbag, self.top_lidar_rosbag], queue_size=10, slop=0.1)
# self.sync.registerCallback(self.ouster_oak_callback)

# GEMStack Subscribers and sychronizers
# LIDAR Camera fusion
self.vehicle_interface.subscribe_sensor('sensor_fusion_Lidar_Camera_GNSS',self.ouster_oak_callback)

# LIDAR Camera GNSS fusion
# self.vehicle_interface.subscribe_sensor('sensor_fusion_Lidar_Camera_GNSS',self.ouster_oak_callback)

# TF listener to get transformation from LiDAR to Camera
self.tf_listener = tf.TransformListener()
Expand All @@ -115,14 +129,8 @@ def __init__(self, vehicle_interface : GEMInterface) -> None:

# Update function variables
self.t_start = datetime.now() # Estimated start frame time
# self.start_pose_abs = None
# self.start_pose_abs = ObjectPose(
# frame=ObjectFrameEnum.GLOBAL,
# t=0,
# x=0,
# y=0,
# z=0
# )
self.start_pose_abs = None # Get this from GNSS (GLOBAL frame)
self.current_vehicle_state = None # Current vehicle state from GNSS in GLOBAL frame

def init_debug(self,) -> None:
# Debug Publishers
Expand All @@ -132,10 +140,23 @@ def init_debug(self,) -> None:
self.pub_image = rospy.Publisher("/camera/image_detection", Image, queue_size=1)

def update(self, vehicle : VehicleState) -> Dict[str,AgentState]:
# if self.start_pose_abs is None:
# 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

# Convert to start state.
# self.current_vehicle_state | data from GNSS of vehicle pose in GLOBAL Frame
# self.start_pose_abs | data from GNSS of the first vehicle pose in GLOBAL Frame
vehicle.pose.to_frame(ObjectFrameEnum.START,self.current_vehicle_state.pose,self.start_pose_abs)

# To be checked with Lucas
# self.prev_agents = self.current_agents.copy()
self.current_agents.clear()
Copy link
Collaborator

@LucasEby LucasEby Feb 22, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This little bit here is good. I'll modify find_vels_and_ids to accept start frame data instead of current frame data


# Converting to start frame
obj_centers = vehicle.pose.apply(self.current_agent_obj_dims["pose"])
obj_dims = vehicle.pose.apply(self.current_agent_obj_dims["dims"])

# Creating agent state and trackinig ids
self.find_vels_and_ids(obj_centers, obj_dims)

return self.current_agents

# TODO: Improve Algo Knn, ransac, etc.
Expand All @@ -154,29 +175,6 @@ def find_dims(self, clusters: List[List[np.ndarray]]) -> List[np.ndarray]:
clusters = [np.array(clust) for clust in clusters]
dims = [np.array(()) if clust.size == 0 else np.max(clust, axis= 0) - np.min(clust, axis= 0) for clust in clusters]
return dims

# TODO: Slower but cleaner to input self.current_agents dict
# TODO: Moving Average across last N iterations pos/vel? Less spurious vals
# TODO Akul: Fix velocity calculation to calculate in ObjectFrameEnum.START
# Work towards own tracking class instead of simple YOLO track?
# Fix 1: division by time
# Fix 2: Put centers and dims in start frame for velocity calc + final agentstate in update_object_states
# ret: Dict[track_id: vel[x, y, z]]
# def find_vels(self, track_ids: List[int], obj_centers: List[np.ndarray], obj_dims: List[np.ndarray]) -> Dict[int, np.ndarray]:
# # Object not seen -> velocity = None
# track_id_center_map = dict(zip(track_ids, obj_centers))
# vels = defaultdict(lambda: np.array(())) # None is faster, np.array matches other find_ methods.

# for prev_track_id, prev_agent in self.prev_agents.items():
# if prev_track_id in track_ids:
# # TODO: Add prev_agents to memory to avoid None velocity
# # We should only be missing prev pose on first sight of track_id Agent.
# # print("shape 1: ", track_id_center_map[prev_agent.track_id])
# # print("shape 2: ", np.array([prev_agent.pose.x, prev_agent.pose.y, prev_agent.pose.z]))
# # prev can be 3 separate Nones, current is just empty array... make this symmetrical
# if prev_agent.pose.x and prev_agent.pose.y and prev_agent.pose.z and track_id_center_map[prev_agent.track_id].shape == 3:
# vels[prev_track_id] = (track_id_center_map[prev_track_id] - np.array([prev_agent.pose.x, prev_agent.pose.y, prev_agent.pose.z])) / (self.curr_time - self.prev_time)
# return vels

# TODO: Separate debug/viz class, bbox and 2d 3d points funcs
def viz_object_states(self, cv_image, boxes, extracted_pts_all):
Expand Down Expand Up @@ -238,9 +236,9 @@ 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()


self.current_agent_obj_dims.clear()
# Return if no track results available
if track_result[0].boxes.id == None:
return
Expand All @@ -264,25 +262,25 @@ def update_object_states(self, track_result: List[Results], extracted_pts_all: L
obj_centers = self.find_centers(pedestrians_3d_pts) # Centers are calculated in lidar frame here
obj_dims = self.find_dims(pedestrians_3d_pts)

# If in vehicle frame, transform centers from top_lidar frame to vehicle frame
# Need to transform the center point one by one since matrix op can't deal with empty points
if self.vehicle_frame:
obj_centers_vehicle = []
for obj_center in obj_centers:
if len(obj_center) > 0:
obj_center = np.array([obj_center])
obj_center_vehicle = transform_lidar_points(obj_center, self.R_lidar_to_vehicle, self.t_lidar_to_vehicle)[0]
obj_centers_vehicle.append(obj_center_vehicle)
else:
obj_centers_vehicle.append(np.array(()))
obj_centers = obj_centers_vehicle
# Pose is stored in vehicle frame and converted to start frame in the update function
obj_centers_vehicle = []
for obj_center in obj_centers:
if len(obj_center) > 0:
obj_center = np.array([obj_center])
obj_center_vehicle = transform_lidar_points(obj_center, self.R_lidar_to_vehicle, self.t_lidar_to_vehicle)[0]
obj_centers_vehicle.append(obj_center_vehicle)
else:
obj_centers_vehicle.append(np.array(()))
obj_centers = obj_centers_vehicle

self.current_agent_obj_dims["pose"] = obj_center
self.current_agent_obj_dims["dims"] = obj_dims

self.find_vels_and_ids(obj_centers, obj_dims)

# 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):
Expand Down Expand Up @@ -421,36 +419,87 @@ def create_agent_states_in_start_frame(self, obj_centers: List[np.ndarray], obj_
)

# 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
# )
)
agents.append(state)

# Return the agent states converted to start frame:
return agents


# TODO : Generate Transformation matrix from vehicle to start
# Just keeping this here incase we need it, at this moment this function will not be used.
# def generate_vehicle_start_frame(self) -> np.ndarray:
# '''
# Creates T matrix for WORLD to START Frame and T matrix for WORLD to VEHICLE Frame
# Return T VEHICLE to START
# '''

# # T World to Start
# start_x, start_y, start_z = self.current_vehicle_state.pose.x, self.current_vehicle_state.pose.y, self.current_vehicle_state.pose.z
# start_yaw, start_roll, start_pitch = self.current_vehicle_state.pose.yaw, self.current_vehicle_state.pose.pitch, self.current_vehicle_state.pose.roll
# rotation_world_start = R.from_euler('xyz',[start_roll, start_pitch,start_yaw],degrees=False).as_matrix()
# self.t_start_to_world = np.eye(4)
# self.t_start_to_world[:3,:3] = rotation_world_start
# self.t_start_to_world[:3,3] = [start_x, start_y, start_z]


# # Converting vehicle_state from START to CURRENT
# self.current_vehicle_state.to_frame(frame=ObjectFrameEnum.CURRENT, current_pose=self.current_vehicle_state.pose, start_pose_abs=self.start_pose_abs)

# # T World to Vehicle
# vehicle_x, vehicle_y, vehicle_z = self.current_vehicle_state.pose.x, self.current_vehicle_state.pose.y, self.current_vehicle_state.pose.z
# vehicle_yaw, vehicle_roll, vehicle_pitch = self.current_vehicle_state.pose.yaw, self.current_vehicle_state.pose.pitch, self.current_vehicle_state.pose.roll
# rotation_world_vehicle = R.from_euler('xyz',[vehicle_roll, vehicle_pitch, vehicle_yaw],degrees=False).as_matrix()
# self.t_vehicle_to_world = np.eye(4)
# self.t_vehicle_to_world[:3,:3] = rotation_world_vehicle
# self.t_vehicle_to_world[:3,3] = [vehicle_x, vehicle_y, vehicle_z]

# return np.linalg.inv(self.t_start_to_world @ np.linalg.inv(self.t_vehicle_to_world))


# TODO: Slower but cleaner to input self.current_agents dict
# TODO: Moving Average across last N iterations pos/vel? Less spurious vals
# TODO Akul: Fix velocity calculation to calculate in ObjectFrameEnum.START
# Work towards own tracking class instead of simple YOLO track?
# Fix 1: division by time
# Fix 2: Put centers and dims in start frame for velocity calc + final agentstate in update_object_states
# ret: Dict[track_id: vel[x, y, z]]
# def find_vels(self, track_ids: List[int], obj_centers: List[np.ndarray], obj_dims: List[np.ndarray]) -> Dict[int, np.ndarray]:
# # Object not seen -> velocity = None
# track_id_center_map = dict(zip(track_ids, obj_centers))
# vels = defaultdict(lambda: np.array(())) # None is faster, np.array matches other find_ methods.

# for prev_track_id, prev_agent in self.prev_agents.items():
# if prev_track_id in track_ids:
# # TODO: Add prev_agents to memory to avoid None velocity
# # We should only be missing prev pose on first sight of track_id Agent.
# # print("shape 1: ", track_id_center_map[prev_agent.track_id])
# # print("shape 2: ", np.array([prev_agent.pose.x, prev_agent.pose.y, prev_agent.pose.z]))
# # prev can be 3 separate Nones, current is just empty array... make this symmetrical
# if prev_agent.pose.x and prev_agent.pose.y and prev_agent.pose.z and track_id_center_map[prev_agent.track_id].shape == 3:
# vels[prev_track_id] = (track_id_center_map[prev_track_id] - np.array([prev_agent.pose.x, prev_agent.pose.y, prev_agent.pose.z])) / (self.curr_time - self.prev_time)
# return vels

def ouster_oak_callback(self, rgb_image_msg: Image, lidar_pc2_msg: PointCloud2):
def ouster_oak_callback(self, cv_image: cv2.Mat, lidar_points: np.ndarray, vehicle_state: GNSSReading):
Copy link
Collaborator

@LucasEby LucasEby Feb 22, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We should rename the variable vehicle_state here because it's confusing. VehicleState is the name of a class so if I were reading the code and working with it I'd assume this is an object of that class.

# Update times for basic velocity calculation
self.prev_time = self.curr_time
self.curr_time = datetime.now()

# Getting start pose in GLOBAL Frame
if self.start_pose_abs == None and vehicle_state.status == 'ok':
self.start_pose_abs = vehicle_state.pose

# Update current vehicle state and convert to START Frame
if (vehicle_state.status == 'ok'):
self.current_vehicle_state = vehicle_state
else:
raise Exception('Unable to get current pose of vehicle, lost GPS')
Copy link
Collaborator

@LucasEby LucasEby Feb 22, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I love it that you used an exception here. I think there are a lot of places where we should be using them to check for edge cases. This might be a confusing exception for the other teams though.

It may potentially be a better idea to log this, print it to the console, and throw out the currently scanned in pedestrians because we can't calculate their positions relative to start frame. This might be a good question to ask your roommate what he thinks?


# Convert to cv2 image and run detector
cv_image = self.bridge.imgmsg_to_cv2(rgb_image_msg, "bgr8")
# cv_image = self.bridge.imgmsg_to_cv2(rgb_image_msg, "bgr8")
track_result = self.detector.track(source=cv_image, classes=self.classes_to_detect, persist=True, conf=self.confidence)

# Convert 1D PointCloud2 data to x, y, z coords
lidar_points = convert_pointcloud2_to_xyz(lidar_pc2_msg)
# lidar_points = convert_pointcloud2_to_xyz(lidar_pc2_msg)
# print("len lidar_points", len(lidar_points))

# Downsample xyz point clouds
Expand Down Expand Up @@ -495,6 +544,7 @@ def ouster_oak_callback(self, rgb_image_msg: Image, lidar_pc2_msg: PointCloud2):
extracted_pts_all.append(extracted_pts)
else: extracted_pts_all.append(np.array(()))

# Generate Transformation matrix for vehicle to start
self.update_object_states(track_result, extracted_pts_all)
if self.debug: self.viz_object_states(cv_image, boxes, extracted_pts_all)

Expand Down
3 changes: 2 additions & 1 deletion requirements.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,4 +10,5 @@ dacite

# Perception
ultralytics
lap==0.5.12
lap==0.5.12
open3d