def inside_polygon(p, polygon): """ This functions checks if a point is inside a certain lane (or area), a.k.a. polygon. (https://jsbsan.blogspot.com/2011/01/saber-si-un-punto-esta-dentro-o-fuera.html) Takes a point and a polygon and returns if it is inside (1) or outside(0). """ counter = 0 xinters = 0 detection = False p1 = Node() p2 = Node() p1 = polygon[0] # First column = x coordinate. Second column = y coordinate for i in range(1,len(polygon)+1): p2 = polygon[i%len(polygon)] if (p.y > min(p1.y,p2.y)): if (p.y <= max(p1.y,p2.y)): if (p.x <= max(p1.x,p2.x)): if (p1.y != p2.y): xinters = (p.y-p1.y)*(p2.x-p1.x)/(p2.y-p1.y)+p1.x if (p1.x == p2.x or p.x <= xinters): counter += 1 p1 = p2 if (counter % 2 == 0): return False else: return True
def calculate_3d_corners(bbox): """ Get 3D bounding box corners bbox[0,1,2] -> LiDAR centroid bbox[3,4,5] -> l,w,h bbox[6] -> yaw bbox[7,8] -> vx,vy (global) Regarding the corners, from 0-3 lower, from 4-7 upper, 8 is the centroid (bbox[0,1,2]) 6 -------- 7 /| /| 4 -------- 5 . | | 8 | | . 2 -------- 3 |/ |/ 0 -------- 1 """ x, y, z = bbox[0:3] l, w, h = bbox[3:6] yaw = bbox[6] x_corners = [-l / 2, -l / 2, l / 2, l / 2, -l / 2, -l / 2, l / 2, l / 2] y_corners = [w / 2, -w / 2, w / 2, -w / 2, w / 2, -w / 2, w / 2, -w / 2] z_corners = [-h / 2, -h / 2, -h / 2, -h / 2, h / 2, h / 2, h / 2, h / 2] if yaw > np.pi: yaw -= np.pi R = rotz(yaw) corners_3d = np.dot(R, np.vstack([x_corners, y_corners, z_corners])) corners_3d = corners_3d + np.vstack([x, y, z]) corners_3d_list = [] for i in range(corners_3d.shape[1]): node = Node() node.x = corners_3d[0, i] node.y = corners_3d[1, i] node.z = corners_3d[2, i] corners_3d_list.append(node) return corners_3d_list
def find_closest_segment(way, point): """ This functions obtains the closest segment (two nodes) of a certain way (left or right) Returns the nodes that comprise this segment and the distance """ min_distance = 999999 closest_node_0 = Node() closest_node_1 = Node() closest = -1 for i in range(len(way)-1): node_0 = Node() node_0.x = way[i].x node_0.y = way[i].y node_1 = Node() node_1.x = way[i+1].x node_1.y = way[i+1].y distance, _ = geometric_functions.pnt2line(point, node_0, node_1) if (distance < min_distance): min_distance = distance closest = i closest_node_0 = node_0 closest_node_1 = node_1 if min_distance != 999999: if (closest > 0) and (closest < len(way)-2): return closest-1,closest+2,min_distance elif (closest > 0) and (closest == len(way)-2): return closest-1,closest+1,min_distance elif closest == 0 and (len(way) == 2): return 0,1,min_distance elif closest == 0 and (len(way) > 2): return 0,2,min_distance else: return -1, -1, -1
def calculate_aux_point(point,m,delta): aux_point = Node() if (m == 9999): aux_point.x = point.x aux_point.y = point.y + delta else: aux_point.x = point.x + delta aux_point.y = m*delta + point.y return aux_point
def calculate_index_last_waypoint(lane, predicted_distance): """ """ accumulated_distance = 0 for i in range(len(lane.left.way) - 1): cn = Node() # Current node cn.x = lane.left.way[i].x cn.y = -lane.left.way[i].y nn = Node() # Next node nn.x = lane.left.way[i + 1].x nn.y = -lane.left.way[i + 1].y diff = math.sqrt(pow(cn.x - nn.x, 2) + pow(cn.y - nn.y, 2)) accumulated_distance += diff if (accumulated_distance > predicted_distance): return i + 1 return len(lane.left.way) - 1
def callback(self, detections_rosmsg, odom_rosmsg, monitorized_lanes_rosmsg): """ """ try: # Target # Pose (translation, quaternion) = self.listener.lookupTransform( '/map', '/ego_vehicle/lidar/lidar1', rospy.Time(0)) # rospy.Time(0) get us the latest available transform rot_matrix = tf.transformations.quaternion_matrix(quaternion) self.tf_map2lidar = rot_matrix self.tf_map2lidar[:3, 3] = self.tf_map2lidar[:3, 3] + translation # This matrix transforms local to global coordinates except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): print("\033[1;33m" + "TF exception" + '\033[0;m') self.start = time.time() self.detections_subscriber.unregister() self.odom_subscriber.unregister() self.monitorized_lanes_subscriber.unregister() # Initialize the scene if not self.init_scene: self.real_height = detections_rosmsg.front - detections_rosmsg.back # Grid height (m) self.real_front = detections_rosmsg.front + self.view_ahead # To study obstacles (view_head) meters ahead self.real_width = -detections_rosmsg.left + detections_rosmsg.right # Grid width (m) self.real_left = -detections_rosmsg.left r = float(self.image_height) / self.real_height self.image_width = int(round(self.real_width * r)) # Grid width (pixels) self.image_front = int(round(self.real_front * r)) self.image_left = int(round(self.real_left * r)) print("Real width: ", self.real_width) print("Real height: ", self.real_height) print("Image width: ", self.image_width) print("Image height: ", self.image_height) self.shapes = (self.real_front, self.real_left, self.image_front, self.image_left) self.scale_factor = (self.image_height / self.real_height, self.image_width / self.real_width) # Our centroid is defined in BEV camera coordinates but centered in the ego-vehicle. # We need to traslate it to the top-left corner (required by the SORT algorithm) self.tf_bevtl2bevcenter_m = np.array( [[1.0, 0.0, 0.0, self.shapes[1]], [0.0, 1.0, 0.0, self.shapes[0]], [0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0]]) # From BEV centered to BEV top-left (in pixels) self.tf_bevtl2bevcenter_px = np.array( [[1.0, 0.0, 0.0, -self.shapes[3]], [0.0, 1.0, 0.0, -self.shapes[2]], [0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0]]) # Rotation matrix from LiDAR frame to BEV frame (to convert to global coordinates) self.tf_lidar2bev = np.array([[0.0, -1.0, 0.0, 0.0], [-1.0, 0.0, 0.0, 0.0], [0.0, 0.0, -1.0, 0.0], [0.0, 0.0, 0.0, 1.0]]) # Initialize the regression model to calculate the braking distance self.velocity_braking_distance_model = monitors_functions.fit_velocity_braking_distance_model( ) # Tracker self.mot_tracker = sort_functions.Sort(max_age, min_hits, n, self.shapes, self.trajectory_prediction, self.filter_hdmap) self.init_scene = True output_image = np.ones( (self.image_height, self.image_width, 3), dtype="uint8") # Black image to represent the scene print("----------------") # Initialize ROS and monitors variables self.trackers_marker_list = visualization_msgs.msg.MarkerArray( ) # Initialize trackers list monitorized_area_colours = [] trackers_in_route = 0 nearest_distance = std_msgs.msg.Float64() nearest_distance.data = float(self.nearest_object_in_route) world_features = [] trackers = [] dynamic_trackers = [] static_trackers = [] ndt = 0 timer_rosmsg = detections_rosmsg.header.stamp.to_sec() # Predict the ego-vehicle trajectory monitors_functions.ego_vehicle_prediction(self, odom_rosmsg, output_image) print("Braking distance ego vehicle: ", float(self.ego_braking_distance)) # Convert input data to bboxes to perform Multi-Object Tracking #print("\nNumer of total detections: ", len(detections_rosmsg.bev_detections_list)) bboxes_features, types = sort_functions.bbox_to_xywh_cls_conf( self, detections_rosmsg, odom_rosmsg, detection_threshold, output_image) #print("Number of relevant detections: ", len(bboxes_features)) # score > detection_threshold # Emergency break (Only detection) """ nearest_object_in_route = 99999 if not self.collision_flag.data: for bbox,type_object in zip(bboxes_features,types): for lane in monitorized_lanes_rosmsg.lanes: if (lane.role == "current" and len(lane.left.way) >= 2): detection = Node() bbox = bbox.reshape(1,-1) global_pos = sort_functions.store_global_coordinates(self.tf_map2lidar,bbox) detection.x = global_pos[0,0] detection.y = -global_pos[1,0] # N.B. In CARLA this coordinate is the opposite is_inside, particular_monitorized_area, dist2centroid = monitors_functions.inside_lane(lane,detection,type_object) if is_inside: #distance_to_object = monitors_functions.calculate_distance_to_nearest_object_inside_route(monitorized_lanes_rosmsg,global_pos) ego_x_global = odom_rosmsg.pose.pose.position.x ego_y_global = -odom_rosmsg.pose.pose.position.y distance_to_object = math.sqrt(pow(ego_x_global-detection.x,2)+pow(ego_y_global-detection.y,2)) print("Distance to object: ", distance_to_object) if distance_to_object < self.nearest_object_in_route: self.nearest_object_in_route = distance_to_object nearest_distance.data = float(self.nearest_object_in_route) print("Braking distance: ", self.ego_braking_distance) if distance_to_object < self.ego_braking_distance: print("Break") self.collision_flag.data = True self.pub_collision.publish(self.collision_flag) self.emergency_break_timer = time.time() self.cont = 0 break else: continue break else: dist = 99999 for bbox,type_object in zip(bboxes_features,types): for lane in monitorized_lanes_rosmsg.lanes: if (lane.role == "current" and len(lane.left.way) >= 2): detection = Node() bbox = bbox.reshape(1,-1) global_pos = sort_functions.store_global_coordinates(self.tf_map2lidar,bbox) detection.x = global_pos[0,0] detection.y = -global_pos[1,0] # N.B. In CARLA this coordinate is the opposite is_inside, particular_monitorized_area, dist2centroid = monitors_functions.inside_lane(lane,detection,type_object) print("inside: ", is_inside) if is_inside: ego_x_global = odom_rosmsg.pose.pose.position.x ego_y_global = -odom_rosmsg.pose.pose.position.y aux = math.sqrt(pow(ego_x_global-detection.x,2)+pow(ego_y_global-detection.y,2)) if aux < dist: dist = aux break if dist > 5: self.cont += 1 else: self.cont = 0 print("distance to object after collision: ", dist) print("cont: ", self.cont) if self.cont >= 3: self.collision_flag.data = False self.nearest_object_in_route = 50000 nearest_distance.data = float(self.nearest_object_in_route) print("Data: ", nearest_distance.data) print("Collision: ", self.collision_flag.data) self.pub_nearest_object_distance.publish(nearest_distance) self.pub_collision.publish(self.collision_flag) """ ## Multi-Object Tracking # TODO: Publish on tracked_obstacle message instead of visualization marker # TODO: Evaluate the predicted position to predict its influence in a certain use case if (len(bboxes_features) > 0): # At least one object was detected trackers, object_types, object_scores, object_observation_angles, dynamic_trackers, static_trackers = self.mot_tracker.update( bboxes_features, types, self.ego_vel_px, self.tf_map2lidar, self.shapes, self.scale_factor, monitorized_lanes_rosmsg, timer_rosmsg, self.angle_bb, self.geometric_monitorized_area) #print("Number of trackers: ", len(trackers)) if len(dynamic_trackers.shape) == 3: #print("Dynamic trackers", dynamic_trackers.shape[1]) ndt = dynamic_trackers.shape[1] else: #print("Dynamic trackers: ", 0) ndt = 0 #print("Static trackers: ", static_trackers.shape[0]) id_nearest = -1 if (len(trackers) > 0): # At least one object was tracked for i, tracker in enumerate(trackers): object_type = object_types[i] object_score = object_scores[i] object_rotation = np.float64(object_observation_angles[i, 0]) object_observation_angle = np.float64( object_observation_angles[i, 1]) color = self.colours[tracker[5].astype(int) % 32] #print("hago append") monitorized_area_colours.append(color) if self.ros: world_features = monitors_functions.tracker_to_topic( self, tracker, object_type, color) # world_features (w,l,h,x,y,z,id) #print("WF: ", world_features) if kitti: num_image = detections_rosmsg.header.seq - 1 # Number of image in the dataset, e.g. 0000.txt -> 0 object_properties = object_observation_angle, object_rotation, object_score monitors_functions.store_kitti( num_image, path, object_type, world_features, object_properties) if (self.display): my_thickness = -1 geometric_functions.compute_and_draw( tracker, color, my_thickness, output_image) label = 'ID %06d' % tracker[5].astype(int) cv2.putText( output_image, label, (tracker[0].astype(int), tracker[1].astype(int) - 20), cv2.FONT_HERSHEY_PLAIN, 1.5, [255, 255, 255], 2) cv2.putText( output_image, object_type, (tracker[0].astype(int), tracker[1].astype(int) - 40), cv2.FONT_HERSHEY_PLAIN, 1.5, [255, 255, 255], 2) # Evaluate if there is some obstacle in lane and calculate nearest distance if self.filter_hdmap: if tracker[-1]: # In route, last element of the array trackers_in_route += 1 obstacle_local_position = np.zeros((1, 9)) obstacle_local_position[0, 7] = world_features[3] obstacle_local_position[0, 8] = world_features[4] obstacle_global_position = sort_functions.store_global_coordinates( self.tf_map2lidar, obstacle_local_position) #distance_to_object = monitors_functions.calculate_distance_to_nearest_object_inside_route(monitorized_lanes_rosmsg,obstacle_global_position) detection = Node() detection.x = obstacle_global_position[0, 0] detection.y = -obstacle_global_position[1, 0] ego_x_global = odom_rosmsg.pose.pose.position.x ego_y_global = -odom_rosmsg.pose.pose.position.y distance_to_object = math.sqrt( pow(ego_x_global - detection.x, 2) + pow(ego_y_global - detection.y, 2)) distance_to_object -= 5 # QUITARLO, DEBERIA SER DISTANCIA CENTROIDE OBJETO A MORRO, EN VEZ DE LIDAR A LIDAR, POR ESO # LE METO ESTE OFFSET #print("Distance to object: ", distance_to_object) if distance_to_object < self.nearest_object_in_route: id_nearest = tracker[5] self.nearest_object_in_route = distance_to_object else: # Evaluate in the geometric monitorized area x = world_features[3] y = world_features[4] print("main") print("goemetric area: ", self.geometric_monitorized_area) print("x y: ", x, y) if (x < self.geometric_monitorized_area[0] and x > self.geometric_monitorized_area[1] and y < self.geometric_monitorized_area[2] and y > self.geometric_monitorized_area[3]): trackers_in_route += 1 self.cont = 0 print("\n\n\nDentro") distance_to_object = math.sqrt( pow(x, 2) + pow(y, 2)) print("Nearest: ", self.nearest_object_in_route) print("distance: ", distance_to_object) if distance_to_object < self.nearest_object_in_route: self.nearest_object_in_route = distance_to_object print("Collision: ", self.collision_flag.data) print("trackers in route: ", trackers_in_route) print("Distance nearest: ", self.nearest_object_in_route) if self.collision_flag.data and ( trackers_in_route == 0 or (self.nearest_object_in_route > 12 and self.abs_vel < 1)): print("suma A") self.cont += 1 else: self.cont == 0 nearest_distance.data = self.nearest_object_in_route if (self.trajectory_prediction): collision_id_list = [[], []] # Evaluate collision with dynamic trackers for a in range(dynamic_trackers.shape[1]): for j in range(self.n.shape[0]): e = dynamic_trackers[j][a] color = self.colours[e[5].astype(int) % 32] my_thickness = 2 geometric_functions.compute_and_draw( e, color, my_thickness, output_image) if (self.ros): object_type = "trajectory_prediction" monitors_functions.tracker_to_topic( self, e, object_type, color, j) # Predict possible collision (including the predicted bounding boxes) collision_id, index_bb = monitors_functions.predict_collision( self.ego_predicted, dynamic_trackers[:, a]) if collision_id != -1: collision_id_list[0].append(collision_id) collision_id_list[1].append(index_bb) # Evaluate collision with static trackers for b in static_trackers: if b[-1]: # In route, last element of the array collision_id, index_bb = monitors_functions.predict_collision( self.ego_predicted, b, static=True) # Predict possible collision if (collision_id != -1): collision_id_list[0].append(collision_id) collision_id_list[1].append(index_bb) #if self.nearest_object_in_route < self.ego_braking_distance: # self.collision_flag.data = True # Collision if not self.collision_flag.data: if not collision_id_list[0]: # Empty #if id_nearest == -1: collision_id_list[0].append( -1 ) # The ego-vehicle will not collide with any object collision_id_list[1].append(-1) self.collision_flag.data = False elif collision_id_list[0] and ( self.nearest_object_in_route < self.ego_braking_distance or self.nearest_object_in_route < 12): #elif id_nearest != -1 and (self.nearest_object_in_route < self.ego_braking_distance or self.nearest_object_in_route < 12): self.collision_flag.data = True self.cont = 0 #print("Collision id list: ", collision_id_list) if (len(collision_id_list) > 1): message = 'Predicted collision with objects: ' + str( collision_id_list[0]) else: message = 'Predicted collision with object: ' + str( collision_id_list[0]) cv2.putText(output_image, message, (30, 140), cv2.FONT_HERSHEY_PLAIN, 1.5, [255, 255, 255], 2) # Predicted collision message else: print("\033[1;33m" + "No object to track" + '\033[0;m') monitors_functions.empty_trackers_list(self) if self.collision_flag.data: print("suma B") self.cont += 1 else: print("\033[1;33m" + "No objects detected" + '\033[0;m') monitors_functions.empty_trackers_list(self) if self.collision_flag.data: print("suma C") self.cont += 1 print("cont: ", self.cont) if self.cont >= 3: self.collision_flag.data = False self.nearest_object_in_route = 50000 nearest_distance.data = float(self.nearest_object_in_route) self.end = time.time() fps = 1 / (self.end - self.start) self.avg_fps += fps self.frame_no += 1 print("SORT time: {}s, fps: {}, avg fps: {}".format( round(self.end - self.start, 3), round(fps, 3), round(self.avg_fps / self.frame_no, 3))) message = 'Trackers: ' + str(len(trackers)) cv2.putText(output_image, message, (30, 20), cv2.FONT_HERSHEY_PLAIN, 1.5, [255, 255, 255], 2) message = 'Dynamic trackers: ' + str(ndt) cv2.putText(output_image, message, (30, 50), cv2.FONT_HERSHEY_PLAIN, 1.5, [255, 255, 255], 2) try: message = 'Static trackers: ' + str(static_trackers.shape[0]) except: message = 'Static trackers: ' + str(0) cv2.putText(output_image, message, (30, 80), cv2.FONT_HERSHEY_PLAIN, 1.5, [255, 255, 255], 2) # Publish the list of tracked obstacles and predicted collision print("Data: ", nearest_distance.data) print("Collision: ", self.collision_flag.data) self.pub_bev_sort_tracking_markers_list.publish( self.trackers_marker_list) self.pub_collision.publish(self.collision_flag) self.pub_nearest_object_distance.publish(nearest_distance) print("moni: ", len(monitorized_area_colours)) self.particular_monitorized_area_list = self.mot_tracker.get_particular_monitorized_area_list( detections_rosmsg.header.stamp, monitorized_area_colours) self.pub_particular_monitorized_area_markers_list.publish( self.particular_monitorized_area_list) if self.write_video: if not self.video_flag: fourcc = cv2.VideoWriter_fourcc(*'MJPG') self.output = cv2.VideoWriter( "map-filtered-mot.avi", fourcc, 20, (self.image_width, self.image_height)) self.output.write(output_image) # Add a grid to appreciate the obstacles coordinates if (self.grid): gap = int( round(self.view_ahead * (self.image_width / self.real_width))) geometric_functions.draw_basic_grid(gap, output_image, pxstep=50) if (self.display): cv2.imshow("SORT tracking", output_image) cv2.waitKey(1) self.detections_subscriber = Subscriber(self.detections_topic, BEV_detections_list) self.odom_subscriber = Subscriber(self.odom_topic, nav_msgs.msg.Odometry) self.monitorized_lanes_subscriber = Subscriber( self.monitorized_lanes_topic, MonitorizedLanes) ts = TimeSynchronizer([ self.detections_subscriber, self.odom_subscriber, self.monitorized_lanes_subscriber ], header_synchro) ts.registerCallback(self.callback)
def update(self,dets,types,ego_vehicle_vel,tf_map2lidar,shapes,scale_factor,monitorized_lanes,timer_rosmsg,angle_bb,geometric_monitorized_area): """ Params: dets - a numpy array of detections (x,y,w,l,theta,beta,score), where x,y are the centroid coordinates (BEV image plane), w and l the width and length of the obstacle (BEV image plane), theta (rotation angle), beta (angle between the ego-vehicle centroid and obstacle centroid) and the detection score types - a numpy array with the corresponding type of the detections ego_vehicle_vel - a float number that represents the absolute velocity of the car in the image Requires: this method must be called once for each frame even with empty detections. Returns a similar array, where the last column is the object ID. NOTE: The number of objects returned may differ from the number of detections provided. """ # Auxiliar variables self.frame_count += 1 self.particular_monitorized_area_list = [] trks = np.zeros((len(self.trackers),6)) to_del = [] ret = [] ret_static = [] ret_dynamic = [] in_route = False for i in range(self.n.shape[0]): ret_dynamic.append([]) dyn = 0 ret_type = [] ret_score = [] ret_angles = [] # 1. Get predicted locations from existing trackers. for t,trk in enumerate(trks): pos = self.trackers[t].predict()[0] trk[:] = [pos[0], pos[1], pos[2], pos[3], pos[4], 0] if(np.any(np.isnan(pos))): to_del.append(t) trks = np.ma.compress_rows(np.ma.masked_invalid(trks)) for t in reversed(to_del): self.trackers.pop(t) #print("Detections: ", dets, len(dets)) #print("Trackers: ", trks, len(trks)) matched, unmatched_dets, unmatched_trks = tracking_functions.associate_detections_to_trackers(dets,trks) # Hungarian algorithm #print("Matched: ", matched) #print("Unmatched dets: ", unmatched_dets) #print("Unmatched trackers: ", unmatched_trks) # 2. Update matched trackers with assigned detections and evaluate its position inside the monitorized lanes num_trks = len(self.trackers) for t,trk in enumerate(self.trackers): if(t not in unmatched_trks): d = matched[np.where(matched[:,1]==t)[0],0][0] ret_type.append(types[d]) ret_score.append(dets[d,6]) #print("Det theta: ", dets[d,4]) #print("Tracker theta: ", trk.get_state()[0][4]) if (abs(dets[d,4] - trk.get_state()[0][4]) > math.pi/2): # If the difference between the matched detection and its associated tracker # in two consecutives frames is greater than pi/2 radians, probably the detection is the opposite dets[d,4] = dets[d,4] + math.pi if (dets[d,4] > 1.8*math.pi): # If the tracker orientation is close to 0, and the detection is close to pi, if we sum pi, the result # is 2*pi, but it does not make sense (huge angular velocity produced), so now substract 2*pi dets[d,4] = dets[d,4] - 2*math.pi observation_angle = dets[d,5] + dets[d,4] # Beta + Theta (According to KITTI framework) angles = np.array([[dets[d,4],observation_angle]]) ret_angles.append(angles) # Update the space state trk.update(dets[d,:]) # Get the current global position real_world_x, real_world_y, _, _ = geometric_functions.pixels2realworld(self,trk,shapes) aux_array = np.zeros((1,9)) aux_array[0,4] = trk.kf.x[4] aux_array[0,7:] = real_world_x, real_world_y current_pos = store_global_coordinates(tf_map2lidar,aux_array) trk.current_pos[:2,0] = current_pos[:2,0] # x,y global centroid trk.current_pos[2,0] = current_pos[4,0] # global orientation if self.filter_hdmap: # Check if it is inside the monitorized lanes detection = Node() detection.x = trk.current_pos[0,0] detection.y = -trk.current_pos[1,0] trk_is_inside, trk_in_route, trk_particular_monitorized_area = evaluate_detection_in_monitorized_lanes(detection, monitorized_lanes, types[d]) if trk_particular_monitorized_area: self.particular_monitorized_area_list.append(trk_particular_monitorized_area) prediction_in_route = False if trk_is_inside: # Keep the tracker if it is inside the monitorized area (not only the road) trk.off_road = 0 # Calculate the global velocities (to predict its position) trk.calculate_global_velocities_and_distance(ego_vehicle_vel,scale_factor,timer_rosmsg) trk.abs_vel = math.sqrt(pow(trk.global_velocities[0,0],2)+pow(trk.global_velocities[1,0],2)) #print("Tracker abs vel: ", trk.abs_vel) if (trk.abs_vel > 15): print("\033[1;36m"+"Huge velocity"+'\033[0;m') else: if (trk.abs_vel > 0.5): trk.trajectory_prediction(angle_bb) # Get the predicted bounding box in n seconds if not trk_in_route: # Check if the prediction in the road at this moment # Get the predicted global position predicted_bb = trk.trajectory_prediction_bb[:,-1] real_world_x, real_world_y, _, _ = geometric_functions.pixels2realworld_prediction(self,predicted_bb,shapes) aux_array = np.zeros((1,9)) aux_array[0,4] = trk.kf.x[4] aux_array[0,7:] = real_world_x, real_world_y current_pos = store_global_coordinates(tf_map2lidar,aux_array) trk.current_pos[:2,0] = current_pos[:2,0] # x,y global centroid trk.current_pos[2,0] = current_pos[4,0] # global orientation # Check if it is inside the monitorized lanes detection = Node() detection.x = trk.current_pos[0,0] detection.y = -trk.current_pos[1,0] for lane in monitorized_lanes.lanes: if len(lane.left.way) >= 2 and (lane.role == "current" or lane.role == "front"): is_inside, in_road, aux_monitorized_area, dist2centroid = monitors_functions.inside_lane(lane,detection,types[d]) if in_road: prediction_in_route = True break else: trk.trajectory_prediction_bb = np.zeros((5,self.n.shape[0])) # If it is static, the prediction buffer must be zero print("IN ROUTE 1: ", trk_in_route) print("IN ROUTE: ", prediction_in_route) if trk_in_route or prediction_in_route: trk.in_route = 1 # In the road, current lane or next lane else: trk.off_route += 1 else: trk.calculate_global_velocities_and_distance(ego_vehicle_vel,scale_factor,timer_rosmsg) x = real_world_x y = real_world_y print("goemetric area: ", geometric_monitorized_area) print("x y: ", x,y) if (x < geometric_monitorized_area[0] and x > geometric_monitorized_area[1] and y < geometric_monitorized_area[2] and y > geometric_monitorized_area[3]): print("in route geometric") trk.in_route = 1 else: trk.in_route = 0 trk.abs_vel = math.sqrt(pow(trk.global_velocities[0,0],2)+pow(trk.global_velocities[1,0],2)) #print("Tracker abs vel: ", trk.abs_vel) if (trk.abs_vel > 15): print("\033[1;36m"+"Huge velocity"+'\033[0;m') else: if (trk.abs_vel > 1.0): trk.trajectory_prediction(angle_bb) # Get the predicted bounding box in n seconds else: trk.trajectory_prediction_bb = np.zeros((5,self.n.shape[0])) # If it is static, the prediction buffer must be zero # 3. Create and initialise new trackers for unmatched detections (if these detections are inside a monitorized_lane) for i in unmatched_dets: aux = store_global_coordinates(tf_map2lidar,dets[i,:].reshape(1,9)) type_object = types[i] if self.filter_hdmap: detection = Node() detection.x = aux[0,0] detection.y = -aux[1,0] # N.B. In CARLA this coordinate is the opposite is_inside, in_route, particular_monitorized_area = evaluate_detection_in_monitorized_lanes(detection,monitorized_lanes, type_object) if is_inside: # Create new tracker trk = tracking_functions.KalmanBoxTracker(dets[i,:],timer_rosmsg) trk.current_pos[:2,0] = aux[:2,0] # x,y global centroid trk.current_pos[2,0] = aux[4,0] # global orientation trk.particular_monitorized_area = particular_monitorized_area if in_route: trk.in_route = 1 self.trackers.append(trk) if particular_monitorized_area: self.particular_monitorized_area_list.append(particular_monitorized_area) else: #print("Create new tracker") trk = tracking_functions.KalmanBoxTracker(dets[i,:],timer_rosmsg) trk.current_pos[:2,0] = aux[:2,0] # x,y global centroid trk.current_pos[2,0] = aux[4,0] # global orientation x = trk.current_pos[0,0] y = trk.current_pos[1,0] if (x < geometric_monitorized_area[0] and x > geometric_monitorized_area[1] and y < geometric_monitorized_area[2] and y > geometric_monitorized_area[3]): trk.in_route = 1 else: trk.in_route = 0 self.trackers.append(trk) i = len(self.trackers) print("Number of trackers: ", i) # 4. Store relevant trackers in lists for t,trk in enumerate(self.trackers): if(t not in unmatched_trks): d = trk.get_state()[0] # Predicted state in next frame if((trk.time_since_update < 1) and (trk.hit_streak >= self.min_hits or self.frame_count <= self.min_hits)): # id+1 as MOT benchmark requires positive ret.append(np.concatenate((d,[trk.id+1],[trk.in_route])).reshape(1,-1)) # Distinguish between dynamic and static obstacles if self.trajectory_prediction: for k in range(self.n.shape[0]): e = trk.get_trajectory_prediction_bb(k)[0] # Predicted the bounding box position all_zeros = not np.any(e) # True if all elements of the array are equal to 0 if not all_zeros: # Only append if there are predicted bbs for the current tracker ret_dynamic[k].append(np.concatenate((e,[trk.id+1])).reshape(1,-1)) else: ret_static.append(np.concatenate((d,[trk.id+1],[trk.in_route])).reshape(1,-1)) break i -= 1 # Remove dead tracklet if(trk.time_since_update > self.max_age or trk.off_route > self.max_age): self.trackers.pop(i) dyn = len(ret_dynamic[0]) if not ret_static: # The list is empty ret_static.append([]) # We need something into the list to be converted to a np.array if(len(ret)>0 and self.frame_count > 1): a = np.concatenate(ret) b = np.array(ret_type) c = np.array(ret_score) d = np.concatenate(ret_angles) try: e = np.concatenate(ret_dynamic).reshape(self.n.shape[0],dyn,6) except: e = np.empty((0,0)) try: f = np.concatenate(ret_static) except: f = np.empty((0,0)) return a,b,c,d,e,f return np.empty((0,6)),np.empty((0,6)),np.empty((0,6)),np.empty((0,6)),np.empty((0,6)),np.empty((0,6))
def SmartMOT_callback(self, detections_rosmsg, odom_rosmsg, monitorized_lanes_rosmsg): """ """ # print(">>>>>>>>>>>>>>>>>>") # print("Detections: ", detections_rosmsg.header.stamp.to_sec()) # print("Odom: ", odom_rosmsg.header.stamp.to_sec()) # print("Lanes: ", monitorized_lanes_rosmsg.header.stamp.to_sec()) try: # Target # Pose (translation, quaternion) = self.listener.lookupTransform( self.map_frame, self.lidar_frame, rospy.Time(0)) # rospy.Time(0) get us the latest available transform rot_matrix = tf.transformations.quaternion_matrix(quaternion) self.tf_map2lidar = rot_matrix self.tf_map2lidar[:3, 3] = self.tf_map2lidar[:3, 3] + translation # This matrix transforms local to global coordinates except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): print("\033[1;33m" + "TF exception" + '\033[0;m') self.ts.registerCallback(self.SmartMOT_callback) self.start = time.time() # Initialize the scene if not self.init_scene: self.real_height = detections_rosmsg.front - detections_rosmsg.back # Grid height (m) self.real_front = detections_rosmsg.front + self.view_ahead # To study obstacles (view_head) meters ahead self.real_width = -detections_rosmsg.left + detections_rosmsg.right # Grid width (m) self.real_left = -detections_rosmsg.left r = float(self.image_height) / self.real_height self.image_width = int(round(self.real_width * r)) # Grid width (pixels) self.image_front = int(round(self.real_front * r)) self.image_left = int(round(self.real_left * r)) print("Real width: ", self.real_width) print("Real height: ", self.real_height) print("Image width: ", self.image_width) print("Image height: ", self.image_height) self.shapes = (self.real_front, self.real_left, self.image_front, self.image_left) self.scale_factor = (self.image_height / self.real_height, self.image_width / self.real_width) # Our centroid is defined in BEV camera coordinates but centered in the ego-vehicle. # We need to traslate it to the top-left corner (required by the SORT algorithm) self.tf_bevtl2bevcenter_m = np.array( [[1.0, 0.0, 0.0, self.shapes[1]], [0.0, 1.0, 0.0, self.shapes[0]], [0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0]]) # From BEV centered to BEV top-left (in pixels) self.tf_bevtl2bevcenter_px = np.array( [[1.0, 0.0, 0.0, -self.shapes[3]], [0.0, 1.0, 0.0, -self.shapes[2]], [0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0]]) # Rotation matrix from LiDAR frame to BEV frame (to convert to global coordinates) self.tf_lidar2bev = np.array([[0.0, -1.0, 0.0, 0.0], [-1.0, 0.0, 0.0, 0.0], [0.0, 0.0, -1.0, 0.0], [0.0, 0.0, 0.0, 1.0]]) # Initialize the regression model to calculate the braking distance self.velocity_braking_distance_model = monitors_functions.fit_velocity_braking_distance_model( ) # Tracker self.mot_tracker = sort_functions.Sort(max_age, min_hits, n, self.shapes, self.trajectory_prediction, self.filter_hdmap) self.init_scene = True output_image = np.ones( (self.image_height, self.image_width, 3), dtype="uint8") # Black image to represent the scene # print("----------------") # Initialize ROS and monitors variables self.trackers_marker_list = visualization_msgs.msg.MarkerArray( ) # Initialize trackers list monitorized_area_colours = [] trackers_in_route = 0 nearest_distance = std_msgs.msg.Float64() nearest_distance.data = float(self.nearest_object_in_route) world_features = [] trackers = [] dynamic_trackers = [] static_trackers = [] ndt = 0 # Number of dynamic trackers timer_rosmsg = detections_rosmsg.header.stamp.to_sec() # Predict the ego-vehicle trajectory for lane in monitorized_lanes_rosmsg.lanes: if (lane.role == "current" and len(lane.left.way) >= 2): monitors_functions.new_ego_vehicle_prediction( self, odom_rosmsg, lane) #monitors_functions.ego_vehicle_prediction(self,odom_rosmsg) #print("Ego vehicle braking distance: ", float(self.ego_braking_distance)) # Convert input data to bboxes to perform Multi-Object Tracking # print("\nNumer of total detections: ", len(detections_rosmsg.bev_detections_list)) bboxes_features, types = sort_functions.bbox_to_xywh_cls_conf( self, detections_rosmsg, output_image) # print("Number of relevant detections: ", len(bboxes_features)) # score > detection_threshold # Emergency break (Only detection) BEV_LiDAR frame!! Not BEV_Camera frame object_in_route = False dist = 99999 if not self.collision_flag.data: for bbox, type_object in zip(bboxes_features, types): for lane in monitorized_lanes_rosmsg.lanes: if (lane.role == "current" and len(lane.left.way) >= 2): object_in_route = True detection = Node() bbox = bbox.reshape(1, -1) detection.x = bbox[0, 0] detection.y = -bbox[ 0, 1] # N.B. In OpenDrive this coordinate is the opposite in_polygon, in_road, particular_monitorized_area, dist2centroid = monitors_functions.inside_lane( lane, detection, type_object) if in_polygon or in_road: #distance_to_object = monitors_functions.calculate_distance_to_nearest_object_inside_route(monitorized_lanes_rosmsg,global_pos) # print("In route") ego_x_global = odom_rosmsg.pose.pose.position.x ego_y_global = -odom_rosmsg.pose.pose.position.y distance_to_object = math.sqrt( pow(ego_x_global - detection.x, 2) + pow(ego_y_global - detection.y, 2)) # print("Distance to object: ", distance_to_object) # print("Self nearest: ", self.nearest_object_in_route) if distance_to_object < self.nearest_object_in_route: # print("Update distance") self.nearest_object_in_route = distance_to_object nearest_distance.data = float( self.nearest_object_in_route) # print("Braking distance: ", self.ego_braking_distance) if distance_to_object < self.ego_braking_distance: # print("Break") self.collision_flag.data = True self.pub_collision.publish(self.collision_flag) self.cont = 0 break else: continue break if not object_in_route: self.collision_flag.data = False nearest_distance.data = float(50000) else: for bbox, type_object in zip(bboxes_features, types): for lane in monitorized_lanes_rosmsg.lanes: if (lane.role == "current" and len(lane.left.way) >= 2): detection = Node() bbox = bbox.reshape(1, -1) detection.x = bbox[0, 0] detection.y = -bbox[ 0, 1] # N.B. In OpenDrive this coordinate is the opposite in_polygon, in_road, particular_monitorized_area, dist2centroid = monitors_functions.inside_lane( lane, detection, type_object) if in_polygon or in_road: # print("In route after") ego_x_global = odom_rosmsg.pose.pose.position.x ego_y_global = -odom_rosmsg.pose.pose.position.y aux = math.sqrt( pow(ego_x_global - detection.x, 2) + pow(ego_y_global - detection.y, 2)) if aux < dist: dist = aux break nearest_distance.data = dist if dist > 5: self.cont += 1 else: self.cont = 0 # print("distance to object after collision: ", dist) # print("cont: ", self.cont) if self.cont >= 3: self.collision_flag.data = False # print("Nearest object distance: ", nearest_distance.data) # print("Collision: ", self.collision_flag.data) self.pub_nearest_object_distance.publish(nearest_distance) self.pub_collision.publish(self.collision_flag) """