Example #1
0
    def find_median(paths):
        """
        :returns: tuple of (median path: str, error)
        """
        paths = list(paths)
        distance_sum = [0] * len(paths)

        for index, path in enumerate(paths):
            distance_sum[index] = sum(
                SOM.path_distance(path, other_path) for other_path in paths)

        # XXX What to do if there are more minims?

        median = min_index(distance_sum)

        return paths[median[0]][PATH_CATEGORY], median[1]
Example #2
0
def gaussian_kernel_cross_validation(training_set, testing_set, gammas, sigmas,
                                     k_fold):
    """
    Runs Gaussian kernel ridge regression using k-fold cross validation

    Parameters
    ----------
    training_set: ndarray (m, n)
        Training set

    testing_set: ndarray (m', n)
        Testing set

    gammas: ndarray (g,)
        Array of regularization parameter gammas

    sigmas: ndarray (s,)
        Array of variance parameter sigmas in the Gaussian kernel

    k_fold: int
        K fold cross validation, i.e. number of disjoint sets in
        cross validation

    Returns
    -------
    train_mse: float
        Training set MSE for the best gamma and sigma

    test_mse: float
        Testing set MSE for the best gamma and sigma
    """

    dimension = training_set.shape[1]

    # get training and testing data (without the target output)
    training_data = training_set[:, 0:dimension - 1]
    testing_data = testing_set[:, 0:dimension - 1]
    data = np.vstack((training_data, testing_data))
    data_size = data.shape[0]
    training_set_size = training_set.shape[0]

    # get observed output for all the data
    training_y = training_set[:, dimension - 1]
    testing_y = testing_set[:, dimension - 1]

    k_training_idx = np.array_split(np.arange(training_data.shape[0]), k_fold)

    cross_validation_errors = np.zeros((len(sigmas), len(gammas)))

    for s_idx in range(sigmas.shape[0]):
        s = sigmas[s_idx]
        # Gram matrix for all training points
        K_train_all = get_gaussian_kernel(training_data, s)

        for g_idx in range(gammas.shape[0]):
            g = gammas[g_idx]
            k_scores = np.zeros(k_fold)

            for k in range(k_fold):
                kth_indices = k_training_idx[k]
                # print("k indices", kth_indices)

                # k small training set (our Ktrain = Ktrain, train)
                k_train = np.delete(K_train_all, kth_indices, axis=0)
                k_train = np.delete(k_train, kth_indices,
                                    axis=1)
                k_training_y = np.delete(training_y, kth_indices, axis=0)
                a = kridgereg(k_train, k_training_y, g)

                # k validation set (our Kvalidation = Kvalidation, train)
                k_validation = K_train_all[kth_indices, :]
                k_validation = np.delete(k_validation, kth_indices, axis=1)
                k_validation_y = training_y[kth_indices]

                k_scores[k] = dualcost(k_validation, k_validation_y, a)

            cross_validation_errors[s_idx, g_idx] = np.mean(k_scores)

    sigma_idx, gamma_idx = utils.min_index(cross_validation_errors)
    best_sigma = sigmas[sigma_idx]
    best_gamma = gammas[gamma_idx]

    K_all = get_gaussian_kernel(data, best_sigma)

    K_train_train = K_all[0:training_set_size, :][:, 0:training_set_size]
    astar = kridgereg(K_train_train, training_y, best_gamma)
    train_mse = dualcost(K_train_train, training_y, astar)

    K_test_train = K_all[training_set_size:data_size, :][:, 0:training_set_size]
    test_mse = dualcost(K_test_train, testing_y, astar)

    plot_cross_validation_error(cross_validation_errors, gammas, sigmas)
    print("\t kernel ridge - best sigma: 2^{%.1f}" % (sigma_idx / 2 + 7))
    print("\t kernel ridge - best gamma: 2^{%d}" % (best_gamma - 40))
    print("\t kernel ridge - MSE train", train_mse)
    print("\t kernel ridge - MSE test", test_mse)

    return astar, train_mse, test_mse
Example #3
0
def medoid_of_points(points: List[Point]) -> Point:
    cost = partial(medoid_cost, points)
    best_medoid_index = min_index(list(map(cost, points)))
    
    return points[best_medoid_index] 
Example #4
0
def stream(tracker, camera=0, server=0):
  """ 
  @brief Captures video and runs tracking and moves robot accordingly

  @param tracker The BallTracker object to be used
  @param camera The camera number (0 is default) for getting frame data
    camera=1 is generally the first webcam plugged in
  """

  ######## GENERAL PARAMETER SETUP ########
  MOVE_DIST_THRESH = 20 # distance at which robot will stop moving
  SOL_DIST_THRESH = 150 # distance at which solenoid fires
  PACKET_DELAY = 1 # number of frames between sending data packets to pi
  OBJECT_RADIUS = 13 # opencv radius for circle detection
  AXIS_SAFETY_PERCENT = 0.05 # robot stops if within this % dist of axis edges

  packet_cnt = 0
  tracker.radius = OBJECT_RADIUS

  ######## SERVER SETUP ########
  motorcontroller_setup = False
  if server:
    # Create a TCP/IP socket
    sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    # Obtain server address by going to network settings and getting eth ip
    server_address = ('169.254.171.10',10000) # CHANGE THIS
    #server_address = ('localhost', 10000) # for local testing
    print 'starting up on %s port %s' % server_address
    sock.bind(server_address)
    sock.listen(1)
    connection, client_address = None, None
    while True:
      # Wait for a connection
      print 'waiting for a connection'
      connection, client_address = sock.accept()
      break


  ######## CV SETUP ########

  # create video capture object for
  #cap = cv2.VideoCapture(camera)
  cap = WebcamVideoStream(camera).start() # WEBCAM
  #cap = cv2.VideoCapture('../media/goalie-test.mov')
  #cap = cv2.VideoCapture('../media/bounce.mp4')

  cv2.namedWindow(tracker.window_name)

  # create trajectory planner object
  # value of bounce determines # of bounces. 0 is default (no bounces)
  # bounce not currently working correctly
  planner = TrajectoryPlanner(frames=4, bounce=0)

  # create FPS object for frame rate tracking
  fps_timer = FPS(num_frames=20)


  while(True):
    # start fps timer
    fps_timer.start_iteration()

    ######## CAPTURE AND PROCESS FRAME ########
    ret, frame = True, cap.read() # WEBCAM
    #ret, frame = cap.read() # for non-webcam testing
    if ret is False:
      print 'Frame not read'
      exit()

    # resize to 640x480, flip and blur
    frame,img_hsv = tracker.setup_frame(frame=frame, w=640,h=480,
      scale=1, blur_window=15)


    ######## TRACK OBJECTS ########
    # use the HSV image to get Circle objects for robot and objects.
    # object_list is list of Circle objects found.
    # robot is single Circle for the robot position
    # robot_markers is 2-elem list of Circle objects for robot markers
    object_list = tracker.find_circles(img_hsv.copy(), tracker.track_colors,
      tracker.num_objects)
    robot, robot_markers = tracker.find_robot_system(img_hsv)
    walls = tracker.get_rails(img_hsv, robot_markers, colors.Yellow)
    planner.walls = walls

    # Get the line/distances between the robot markers
    # robot_axis is Line object between the robot axis markers
    # points is list of Point objects of closest intersection w/ robot axis
    # distanes is a list of distances of each point to the robot axis
    robot_axis = utils.line_between_circles(robot_markers)
    points, distances = utils.distance_from_line(object_list, robot_axis)
    planner.robot_axis = robot_axis

    ######## TRAJECTORY PLANNING ########
    # get closest object and associated point, generate trajectory
    closest_obj_index = utils.min_index(distances) # index of min value
    closest_line = None
    closest_pt = None
    if closest_obj_index is not None:
      closest_obj = object_list[closest_obj_index]
      closest_pt = points[closest_obj_index]

      closest_line = utils.get_line(closest_obj, closest_pt) # only for viewing
      planner.add_point(closest_obj)


    # Get trajectory - list of elements for bounces, and final line traj
    # Last line intersects with robot axis
    traj_list = planner.get_trajectory_list(colors.Cyan)
    traj = planner.traj


    ######## SEND DATA TO CLIENT ########
    if packet_cnt != PACKET_DELAY:
      packet_cnt = packet_cnt + 1
    else:
      packet_cnt = 0 # reset packet counter

      # error checking to ensure will run properly
      if len(robot_markers) is not 2 or robot is None or closest_pt is None:
        pass
      elif server:
        try:
          if motorcontroller_setup is False:
            # send S packet for motorcontroller setup
            motorcontroller_setup = True


            ######## SETUP MOTORCONTROLLER ########
            axis_pt1 = robot_markers[0].to_pt_string()
            axis_pt2 = robot_markers[1].to_pt_string()
            data = 'SM '+axis_pt1+' '+axis_pt2+' '+robot.to_pt_string()
            print data
            connection.sendall(data)

          # setup is done, send packet with movement data
          else:
            obj_robot_dist = utils.get_pt2pt_dist(robot, closest_obj)
            print 'dist: ' + str(obj_robot_dist) # USE FOR CALIBRATION

            ######## SOLENOID ACTIVATION CODE ########
            # check if solenoid should fire
            if obj_robot_dist <= SOL_DIST_THRESH: # fire solenoid, dont move
              print 'activate solenoid!'
              # TODO SEND SOLENOID ACTIVATE


            ######## MOTOR CONTROL ########
            # get safety parameters
            rob_ax1_dist = utils.get_pt2pt_dist(robot_markers[0],robot)
            rob_ax2_dist = utils.get_pt2pt_dist(robot_markers[1],robot)
            axis_length = utils.get_pt2pt_dist(robot_markers[0],
              robot_markers[1])

            # ensure within safe bounds of motion relative to axis
            # if rob_ax1_dist/axis_length <= AXIS_SAFETY_PERCENT or \
            #   rob_ax2_dist/axis_length <= AXIS_SAFETY_PERCENT:
            #   # in danger zone, kill motor movement
            #   print 'INVALID ROBOT LOCATION: stopping motor'
            #   data = 'KM'
            #   connection.sendall(data)

            # if in danger zone near axis edge, move towards other edge
            if rob_ax1_dist/axis_length <= AXIS_SAFETY_PERCENT:
              print 'INVALID ROBOT LOCATION'
              data = 'MM '+robot.to_pt_string()+' '+robot_markers[1].to_pt_string()
            elif rob_ax2_dist/axis_length <= AXIS_SAFETY_PERCENT:
              print 'INVALID ROBOT LOCATION'
              data = 'MM '+robot.to_pt_string()+' '+robot_markers[0].to_pt_string()

            # check if robot should stop moving
            elif obj_robot_dist <= MOVE_DIST_THRESH: # obj close to robot
              # Send stop command, obj is close enough to motor to hit
              data = 'KM'
              print data
              connection.sendall(data)
              pass 

            # Movement code
            else: # far enough so robot should move

              #### FOR TRAJECTORY ESTIMATION
              # if planner.traj is not None:
              #   axis_intersect=shapes.Point(planner.traj.x2,planner.traj.y2)
              #   # Clamp the point to send to the robot axis
              #   traj_axis_pt = utils.clamp_point_to_line(
              #     axis_intersect, robot_axis)

              #   data = 'D '+robot.to_pt_string()+' '+traj_axis_pt.to_string()
              #   connection.sendall(data)

              #### FOR CLOSEST POINT ON AXIS ####
              if closest_pt is not None and robot is not None:
                # if try to move more than length of axis, stop instead
                if utils.get_pt2pt_dist(robot,closest_pt) > axis_length:
                  print 'TRYING TO MOVE OUT OF RANGE'
                  data = 'KM'
                  print data
                  connection.sendall(data)
                else:
                  data = 'MM ' + robot.to_pt_string() + ' ' + \
                    closest_pt.to_string()
                  print data
                  connection.sendall(data)

        except IOError:
          pass # don't send anything



    ######## ANNOTATE FRAME FOR VISUALIZATION ########
    frame = gfx.draw_lines(img=frame, line_list=walls)

    frame = gfx.draw_robot_axis(img=frame, line=robot_axis) # draw axis line
    frame = gfx.draw_robot(frame, robot) # draw robot
    frame = gfx.draw_robot_markers(frame, robot_markers) # draw markers

    frame = gfx.draw_circles(frame, object_list) # draw objects

    # eventually won't need to print this one
    frame = gfx.draw_line(img=frame, line=closest_line) # closest obj>axis

    # draw full set of trajectories, including bounces
    #frame = gfx.draw_lines(img=frame, line_list=traj_list)
    #frame = gfx.draw_line(img=frame, line=traj) # for no bounces

    frame = gfx.draw_point(img=frame, pt=closest_pt)
    #frame=gfx.draw_line(frame,planner.debug_line) # for debug



    ######## FPS COUNTER ########
    fps_timer.get_fps()
    fps_timer.display(frame)


    ######## DISPLAY FRAME ON SCREEN ########
    cv2.imshow(tracker.window_name,frame)
    # quit by pressing q
    if cv2.waitKey(1) & 0xFF == ord('q'):
      break

  # release capture
  cap.stop() # WEBCAM
  #cap.release() # for testing w/o webcam
  cv2.destroyAllWindows()
Example #5
0
def find_nearest_centroid(centroids, metric, point: Point) -> Point:
    get_distance_from_point = partial(metric, point)
    index = min_index(
        strictMap(get_distance_from_point, centroids))

    return centroids[index]