| |
| |
| |
| import cv2 |
| import numpy as np |
| import math |
|
|
| PIXELS_PER_METER = 8 |
| MAX_DISTANCE = 32 |
| IMG_SIZE = MAX_DISTANCE * PIXELS_PER_METER * 2 |
| EGO_CAR_X = IMG_SIZE // 2 |
| EGO_CAR_Y = IMG_SIZE - (4.0 * PIXELS_PER_METER) |
|
|
| def process_camera_image(tensor_image): |
| image_np = tensor_image.permute(1, 2, 0).cpu().numpy() |
| image_np = (image_np * np.array([0.229, 0.224, 0.225])) + np.array([0.485, 0.456, 0.406]) |
| image_np = np.clip(image_np, 0, 1) |
| return (image_np * 255).astype(np.uint8)[:, :, ::-1] |
|
|
| def ensure_rgb(image): |
| if len(image.shape) == 2 or image.shape[2] == 1: |
| return cv2.cvtColor(image, cv2.COLOR_GRAY2BGR) |
| return image |
|
|
| def convert_grid_to_xy(i, j): |
| return (j - 9.5) * 1.6, (19.5 - i) * 1.6 |
|
|
| def render_waypoints(waypoints, last_valid_waypoints=None): |
| img = np.zeros((IMG_SIZE, IMG_SIZE, 3), np.uint8) |
| current_waypoints = waypoints |
| if waypoints is not None and len(waypoints) > 2: |
| last_valid_waypoints = waypoints |
| elif last_valid_waypoints is not None: |
| current_waypoints = last_valid_waypoints |
| else: |
| return img, last_valid_waypoints |
| for i, point in enumerate(current_waypoints): |
| px = int(EGO_CAR_X + point[1] * PIXELS_PER_METER) |
| py = int(EGO_CAR_Y - point[0] * PIXELS_PER_METER) |
| color = (0, 0, 255) if i == len(current_waypoints) - 1 else (0, 255, 0) |
| cv2.circle(img, (px, py), 4, color, -1) |
| return img, last_valid_waypoints |
|
|
| def render_self_car(theta, last_valid_theta=0.0): |
| img = np.zeros((IMG_SIZE, IMG_SIZE, 3), np.uint8) |
| if abs(theta - last_valid_theta) < np.pi / 2: last_valid_theta = theta |
| else: theta = last_valid_theta |
| rect_size_px = (int(4.0 * PIXELS_PER_METER), int(2.0 * PIXELS_PER_METER)) |
| angle_deg = -np.degrees(theta) |
| box_points = cv2.boxPoints(((EGO_CAR_X, EGO_CAR_Y), rect_size_px, angle_deg)) |
| cv2.fillConvexPoly(img, np.int32(box_points), (0, 255, 255)) |
| return img, last_valid_theta |
| |
| def add_rect(img, loc, ori, box, value, color): |
| center_x=EGO_CAR_X+int(loc[0]*PIXELS_PER_METER);center_y=EGO_CAR_Y-int(loc[1]*PIXELS_PER_METER);size_px=(int(box[0]*PIXELS_PER_METER),int(box[1]*PIXELS_PER_METER));angle_deg=-np.degrees(math.atan2(ori[1],ori[0]));display_color=tuple([int(c*value) for c in color]);box_points=cv2.boxPoints(((center_x,center_y),size_px,angle_deg));cv2.fillConvexPoly(img,np.int32(box_points),display_color) |
|
|
| def find_peak_box_improved(data): |
| OBJECT_CLASSES={'car':(2.0,8.0),'bike':(1.0,2.5),'pedestrian':(0.3,1.5)};CONF_THRESHOLD=0.25;det_padded=np.zeros((22,22,7));det_padded[1:21,1:21]=data;detected_objects=[] |
| for i in range(1,21): |
| for j in range(1,21): |
| conf=det_padded[i,j,0];is_peak=(conf>CONF_THRESHOLD and conf>det_padded[i-1,j,0] and conf>det_padded[i+1,j,0] and conf>det_padded[i,j-1,0] and conf>det_padded[i,j+1,0]) |
| if is_peak: |
| coords=(i-1,j-1);raw_data=data[coords];length,width=raw_data[4],max(raw_data[5],0.1);obj_type='unknown' |
| if OBJECT_CLASSES['pedestrian'][0]<=length<OBJECT_CLASSES['pedestrian'][1]:obj_type='pedestrian' |
| elif OBJECT_CLASSES['bike'][0]<=length<OBJECT_CLASSES['bike'][1]:obj_type='bike' |
| elif OBJECT_CLASSES['car'][0]<=length<OBJECT_CLASSES['car'][1]:obj_type='car' |
| if obj_type!='unknown':detected_objects.append({'coords':coords,'type':obj_type,'confidence':conf,'raw_data':raw_data}) |
| return detected_objects |
|
|
| def render_improved(det_data,t=0): |
| CLASS_COLORS={'car':(0,0,255),'bike':(0,255,0),'pedestrian':(255,0,0),'unknown':(128,128,128)};detected_objects=find_peak_box_improved(det_data) |
| counts={cls:0 for cls in CLASS_COLORS.keys()};[counts.update({obj['type']:counts.get(obj['type'],0)+1}) for obj in detected_objects];img=np.zeros((IMG_SIZE,IMG_SIZE,3),np.uint8) |
| for obj in detected_objects: |
| i,j=obj['coords'];obj_data=obj['raw_data'];speed=obj_data[6];center_x,center_y=convert_grid_to_xy(i,j);theta=obj_data[3]*np.pi;ori=np.array([math.cos(theta),math.sin(theta)]) |
| loc_x=center_x+obj_data[1]+t*speed*ori[0];loc_y=center_y+obj_data[2]-t*speed*ori[1];box=np.array([obj_data[4],obj_data[5]]) |
| if obj['type']=='pedestrian':box*=1.5 |
| add_rect(img,np.array([loc_x,loc_y]),ori,box,obj['confidence'],CLASS_COLORS[obj['type']]) |
| return img,counts |
|
|
| class DisplayInterface: |
| def __init__(self,width=1200,height=600):self._width=width;self._height=height |
| def run_interface(self,data): |
| dashboard=np.zeros((self._height,self._width,3),dtype=np.uint8);font=cv2.FONT_HERSHEY_SIMPLEX;dashboard[:,:800]=cv2.resize(data.get('camera_view'),(800,600));dashboard[:400,800:1200]=cv2.resize(ensure_rgb(data['map_t0']),(400,400));dashboard[400:600,800:1000]=cv2.resize(ensure_rgb(data['map_t1']),(200,200));dashboard[400:600,1000:1200]=cv2.resize(ensure_rgb(data['map_t2']),(200,200));cv2.line(dashboard,(800,0),(800,600),(255,255,255),2);cv2.line(dashboard,(800,400),(1200,400),(255,255,255),2);cv2.line(dashboard,(1000,400),(1000,600),(255,255,255),2) |
| y_pos=40 |
| for key,text in data['text_info'].items():cv2.putText(dashboard,text,(820,y_pos),font,0.6,(255,255,255),1);y_pos+=30 |
| y_pos+=10 |
| for t,counts in data['object_counts'].items():count_str=f"{t}: C={counts['car']} B={counts['bike']} P={counts['pedestrian']}";cv2.putText(dashboard,count_str,(820,y_pos),font,0.5,(255,255,255),1);y_pos+=20 |
| cv2.putText(dashboard,"t0",(1160,30),font,0.8,(0,255,255),2);cv2.putText(dashboard,"t1",(960,430),font,0.8,(0,255,255),2);cv2.putText(dashboard,"t2",(1160,430),font,0.8,(0,255,255),2) |
| return dashboard |
|
|
