def check_obstacles(self, vehicles): minimumDistance = 1000 for vehicle in vehicles: if self.vehicle_type == "Pedrestian": # and vehicle.vehicle_type == "Pedrestian": continue elif vehicle.RightOfPassage == 0 and self.RightOfPassage == 1 and self.vehicle_type != "Pedrestian": # The vehicle doesn't have right of passage (not in roundabout) continue elif self.position == vehicle.position: # It's our vehicle continue else: distance = utils.calc_distance(self.position, vehicle.position) angle = abs(utils.calc_angle(self.position, vehicle.position) - self.direction) current_vision = self.vision_angle if ( self.RightOfPassage == 0 and self.velocity < 0.75 * self.max_velocity and vehicle.RightOfPassage == 1 ): current_vision = self.vision_angle_entrance if distance <= self.range_of_sight and angle < current_vision: stopDistance = ( utils.calc_stopDistance(distance, angle) - self.length * 2 - vehicle.length ) # 3 times radius if stopDistance < minimumDistance: minimumDistance = stopDistance return minimumDistance
def GetDistanceToBusStop(self,position,direction): deltaX=self.roads[2][0]-self.roads[1][0] deltaY=self.roads[2][1]-self.roads[1][1] busStopX=self.roads[1][0]+deltaX*0.5 busStopY=self.roads[1][1]+deltaY*0.5 dist=utils.calc_distance((busStopX,busStopY),position) if (abs(utils.calc_angle(position, (busStopX,busStopY)) - direction)>math.pi/2): dist=-dist return dist
def test_works_as_expected(self): '''utils.calc_angle correctly calculates the angle between two points''' input_output_pairs = [[[[0, 0], [1, 0]], 0], [[[1, 0], [0, 0]], np.pi], [[[0, 0], [1, 1]], np.pi / 4], [[[0, 0], [0, 1]], np.pi / 2], [[[1, 1], [0, 0]], 5 * np.pi / 4], [[[0, 0], [1, 2]], math.atan(2)], [[[0, 1], [1, 0]], 7 * np.pi / 4]] for coord_pair, expected_angle in input_output_pairs: angle = utils.calc_angle(*coord_pair) self.assertEqual(round(angle, 10), round(expected_angle, 10))
def detect_known_objects(self, img): print("HELLLOOOOOO") #img = self.image_resize(img, height=int(img.shape[0]/3.0)) #img_yuv = cv2.cvtColor(img, cv2.COLOR_BGR2YUV) #img_yuv[:,:,0] = cv2.equalizeHist(img_yuv[:,:,0]) #img=cv2.cvtColor(img_yuv,cv2.COLOR_YUV2BGR) X, ratio = self.format_img(img, self.C) if K.image_dim_ordering() == 'tf': X = np.transpose(X, (0, 2, 3, 1)) # get the feature maps and output from the RPN [Y1, Y2, F] = self.model_rpn.predict(X) #print Y1, Y2, F a = datetime.datetime.now() R = roi_helpers.rpn_to_roi(Y1, Y2, self.C, K.image_dim_ordering(), overlap_thresh=0.7) b = datetime.datetime.now() delta = b - a #print("roi_helpers.rpn_to_roi took:", int(delta.total_seconds() * 1000)) # milliseconds #print R #for i in R: # cv2.rectangle(img,(i[0],i[1]),(i[2],i[3]),(0,255,0),3) # convert from (x1,y1,x2,y2) to (x,y,w,h) R[:, 2] -= R[:, 0] R[:, 3] -= R[:, 1] # apply the spatial pyramid pooling to the proposed regions bboxes = {} probs = {} for idx, jk in enumerate(range(R.shape[0] // self.C.num_rois + 1)): ROIs = np.expand_dims(R[self.C.num_rois * jk:self.C.num_rois * (jk + 1), :], axis=0) if ROIs.shape[1] == 0: break if jk == R.shape[0] // self.C.num_rois: #pad R curr_shape = ROIs.shape target_shape = (curr_shape[0], self.C.num_rois, curr_shape[2]) ROIs_padded = np.zeros(target_shape).astype(ROIs.dtype) ROIs_padded[:, :curr_shape[1], :] = ROIs ROIs_padded[0, curr_shape[1]:, :] = ROIs[0, 0, :] ROIs = ROIs_padded #print("ROIs shape", np.array(ROIs).shape) #print("F", np.array(F).shape) a = datetime.datetime.now() [P_cls, P_regr, P_clust] = self.model_classifier_only.predict([F, ROIs]) b = datetime.datetime.now() delta = b - a #print("prediction of roi took: :", int(delta.total_seconds() * 1000)) # milliseconds #print P_cls, P_regr #print P_cls.shape, P_regr.shape for ii in range(P_cls.shape[1]): #print P_cls[0,ii,:] if np.max(P_cls[0, ii, :]) < self.bbox_threshold or np.argmax( P_cls[0, ii, :]) == (P_cls.shape[2] - 1): continue cls_name = self.class_mapping[np.argmax(P_cls[0, ii, :])] if cls_name not in bboxes: bboxes[cls_name] = [] probs[cls_name] = [] (x, y, w, h) = ROIs[0, ii, :] #print x, y, w, h cls_num = np.argmax(P_cls[0, ii, :]) #print "something", cls_num try: (tx, ty, tw, th) = P_regr[0, ii, 4 * cls_num:4 * (cls_num + 1)] tx /= self.C.classifier_regr_std[0] ty /= self.C.classifier_regr_std[1] tw /= self.C.classifier_regr_std[2] th /= self.C.classifier_regr_std[3] x, y, w, h = roi_helpers.apply_regr( x, y, w, h, tx, ty, tw, th) except: print("exception") pass bboxes[cls_name].append([ self.C.rpn_stride * x, self.C.rpn_stride * y, self.C.rpn_stride * (x + w), self.C.rpn_stride * (y + h) ]) probs[cls_name].append(np.max(P_cls[0, ii, :])) detected_objects = [] for key in bboxes: bbox = np.array(bboxes[key]) new_boxes, new_probs = roi_helpers.non_max_suppression_fast( bbox, np.array(probs[key]), overlap_thresh=0.5) for jk in range(new_boxes.shape[0]): (x1, y1, x2, y2) = new_boxes[jk, :] (real_x1, real_y1, real_x2, real_y2) = self.get_real_coordinates(ratio, x1, y1, x2, y2) #print "drawing detected rect at:", (real_x1, real_y1), (real_x2, real_y2) #cv2.rectangle(img,(real_x1, real_y1), (real_x2, real_y2), (int(class_to_color[key][0]), int(class_to_color[key][1]), int(class_to_color[key][2])),5) #textLabel = '{}: {}'.format(key,int(100*new_probs[jk])) #all_dets.append((key,100*new_probs[jk])) #(retval,baseLine) = cv2.getTextSize(textLabel,cv2.FONT_HERSHEY_COMPLEX,1,1) #textOrg = (real_x1-20, real_y1-20) #cv2.rectangle(img, (textOrg[0] - 5, textOrg[1]+baseLine - 5), (textOrg[0]+retval[0] +5, textOrg[1]-retval[1] +5), (0, 0, 0), 2) #cv2.rectangle(img, (textOrg[0] -5,textOrg[1]+baseLine - 5), (textOrg[0]+retval[0] +5, textOrg[1]-retval[1] +5), (255, 255, 255), -1) #cv2.putText(img, textLabel, textOrg, cv2.FONT_HERSHEY_DUPLEX, 0.3, (0, 0, 0), 1) height, width, channels = img.shape #FOV horizontal = 62 degrees (from 90 on right to 33 on left) angle_between_robot_centre_and_detected_object = self.angle_between( (real_x1 + self.distance([real_x1, real_y1], [real_x2, real_y1]) / 2.0, (real_y1 + self.distance([real_x1, real_y1], [real_x1, real_y2]) / 2.0)), (width / 2.0, 0)) - 52.0 angle_between_robot_centre_and_detected_object = -angle_between_robot_centre_and_detected_object angle_between_robot_centre_and_detected_object = calc_angle([ int((real_x1 + real_x2) / 2.0), int((real_y1 + real_y2) / 2.0) ]) angle, distance = calculate_angle_and_distance(img, real_x1, real_x2, real_y1, real_y2, obj_width=16) angle_between_robot_centre_and_detected_object = angle focal_length_mm = 1.0 average_real_object_height_mm = 1.0 image_height_px = height object_height_px = self.distance([real_x1, real_y1], [real_x1, real_y2]) sensor_height_mm = 314.2 distance_between_robot_centre_and_detected_object = (15.0 / ( (min(self.distance([real_x1, real_y1], [real_x2, real_y1]), self.distance([real_x1, real_y1], [real_x1, real_y2])) / max(self.distance([real_x1, real_y1], [real_x2, real_y1]), self.distance([real_x1, real_y1], [real_x1, real_y2])) )) * 123) / self.distance([real_x1, real_y1], [real_x2, real_y1]) distance_between_robot_centre_and_detected_object = distance_between_robot_centre_and_detected_object * 5.0 distance_between_robot_centre_and_detected_object = distance detected_objects.append( (key, "", real_x1, real_y1, real_x2, real_y2, distance_between_robot_centre_and_detected_object, angle_between_robot_centre_and_detected_object)) print("detected objects", len(detected_objects)) temporary_memory = [] for image_item in detected_objects: seen_item_centroid = (image_item[2] + self.distance( (image_item[2], image_item[3]), (image_item[4], image_item[3])) / 2.0, image_item[4] + self.distance( (image_item[2], image_item[3]), (image_item[2], image_item[5])) / 2.0) tracking_uuid = None #print ("items in memory", len(self.SHORT_TERM_MEMORY)) for memory_item in self.SHORT_TERM_MEMORY: memory_centroid = (memory_item[2] + self.distance( (memory_item[2], memory_item[3]), (memory_item[4], memory_item[3])) / 2.0, memory_item[4] + self.distance( (memory_item[2], memory_item[3]), (memory_item[2], memory_item[5])) / 2.0) #print ("distance", self.distance(seen_item_centroid, memory_centroid)) if self.distance(seen_item_centroid, memory_centroid) < self.distance( [image_item[2], image_item[3]], [image_item[4], image_item[5] ]) and image_item[0] == memory_item[0]: tracking_uuid = memory_item[1] continue if tracking_uuid != None: temporary_memory.append( (self.KNOWN_OBJECTS[int(image_item[0])], tracking_uuid, image_item[2], image_item[3], image_item[4], image_item[5], image_item[6], image_item[7])) else: temporary_memory.append( (self.KNOWN_OBJECTS[int(image_item[0])], uuid.uuid1(), image_item[2], image_item[3], image_item[4], image_item[5], image_item[6], image_item[7])) #print ("temp memory items", len(temporary_memory)) if self.show_image: for item in temporary_memory: cv2.rectangle(img, (item[2], item[3]), (item[4], item[5]), (0, 0, 0), 5) textLabel = '{}: {}'.format(item[0], item[1]) (retval, baseLine) = cv2.getTextSize(textLabel, cv2.FONT_HERSHEY_COMPLEX, 1, 1) textOrg = (image_item[2] - 20, image_item[3] - 20) cv2.rectangle(img, (textOrg[0] - 5, textOrg[1] + baseLine - 5), (textOrg[0] + retval[0] + 5, textOrg[1] - retval[1] + 5), (0, 0, 0), 2) cv2.rectangle(img, (textOrg[0] - 5, textOrg[1] + baseLine - 5), (textOrg[0] + retval[0] + 5, textOrg[1] - retval[1] + 5), (255, 255, 255), -1) cv2.putText(img, textLabel, textOrg, cv2.FONT_HERSHEY_DUPLEX, 0.3, (0, 0, 0), 1) self.SHORT_TERM_MEMORY = temporary_memory if self.show_image: cv2.imshow('image', img) cv2.waitKey(3000) #time.sleep(1) #cv2.destroyAllWindows() return self.SHORT_TERM_MEMORY
def get_direction(self): next_pos = self.road.GetNodePosition(self.nextNode) return utils.calc_angle(self.position, next_pos)