Example #1
0
def get_ride_heading(ride, variations=False, moving_average_window=3, stops=False, version=1):
  '''
  I don't know exactly what this does. I was drunk when I wrote it.
  '''
  ride = np.array(ride)
  if moving_average_window:
    ride = util.movingaverage(ride, moving_average_window)

  ROLL_STEP = 3
  ride2 = np.array(ride)
  ride1 = np.roll(ride2, ROLL_STEP, axis=0)
  l = len(ride)
  ride0 = np.hstack((np.ones((l,1)), np.zeros((l,1))))

  ride0 = ride0[ROLL_STEP:]
  ride1 = ride1[ROLL_STEP:]
  ride2 = ride2[ROLL_STEP:]

  a1 = ride0
  a2 = ride2 - ride1

  distances1 = np.linalg.norm(a1, axis=1)
  distances = np.linalg.norm(a2, axis=1)

  x = (a1 * a2).sum(1) / np.maximum(distances1 * distances, 0.1)
  y = np.sign(a2[:,1])
  np.seterr(all='ignore')
  angles = np.arccos(x) * 180 / np.pi
  np.seterr(all='print')
  angles[y<0] = 360 - angles[y<0]
  angles[distances < 2] = np.nan

  is_stopped = []
  angle = []
  angle_window = []
  WINDOW_SIZE = 2
  MIN_SPEED = 2
  direction = []
  for i, dist in enumerate(distances):
    if dist > MIN_SPEED:
      angle_window.append(a2[i])
      angle_window = angle_window[-WINDOW_SIZE:]
      if len(angle_window) < WINDOW_SIZE:
        direction.append(np.nan)
      else:
        d = get_angle(angle_window[-WINDOW_SIZE/2:]) - get_angle(angle_window[:WINDOW_SIZE/2])
        if d > 180:
          d -= 360
        if d < -180:
          d += 360
        direction.append(d)
    else:
      direction.append(np.nan)

    if dist < MIN_SPEED or len(angle_window) < WINDOW_SIZE:
      is_stopped.append(True)
    else:
      is_stopped.append(False)
      angle.append(get_angle(angle_window))

  windows = []
  current_window = []
  current_window_type = 0
  for i in range(len(direction)):
    if np.isnan(direction[i]):
      current_window = []
      windows.append(0)
      continue

    current_window.append(direction[i])
    current_window = current_window[-4:]
    t = np.mean(current_window)
    if current_window_type == 0:
      if np.abs(t) > 3:
        current_window_type = np.sign(t)
    else:
      if np.sign(current_window[-1]) != current_window_type:
        current_window_type = 0

    windows.append(current_window_type)

  windows = windows[2:] + [0, 0]
  sw = True
  while sw:
    sw = False
    for i in range(1, len(windows) - 1):
      if windows[i] != windows[i-1] and windows[i] != windows[i+1]:
        windows[i] = windows[i+1]
        sw = True
    for i in range(3, len(windows)):
      if windows[i-3] != windows[i-2] and \
          windows[i-2] == windows[i-1] and \
          windows[i-1] != windows[i]:
        windows[i-2] = windows[i-3]
        windows[i-1] = windows[i]
        sw = True

  description = []
  current_window_type = 'stop' if stops else 'straight'
  new_type = current_window_type
  current_window_length = 0
  for i in range(1, len(windows)):
    if stops:
      if is_stopped[i]:
        if current_window_type != 'stop':
          new_type = 'stop'

      if windows[i] == 0 and not is_stopped[i]:
        if current_window_type != 'straight':
          new_type = 'straight'
    else:
      if windows[i] == 0 or is_stopped[i]:
        if current_window_type != 'straight':
          new_type = 'straight'

    if windows[i] == 1:
      if current_window_type != 'left':
        new_type = 'left'

    if windows[i] == -1:
      if current_window_type != 'right':
        new_type = 'right'

    if new_type == current_window_type:
      current_window_length += 1
    else:
      if current_window_length:
        description.append([
            current_window_type,
            current_window_length,
            util.euclidian_distance(ride2[i-current_window_length], ride2[i]),
            -np.sum(direction[i-current_window_length : i-1])
        ])
      current_window_type = new_type
      current_window_length = 0

  i = 0
  while i < len(description) - 1:
    if description[i][0] in ['right', 'left'] and np.abs(description[i][3]) < 15:
      description[i+1][2] += description[i][2]
      description.pop(i)
    else:
      i += 1

  i = 0
  while i < len(description) - 1:
    if description[i][0] == description[i+1][0]:
      description[i+1][1] += description[i][1]
      description[i+1][2] += description[i][2]
      description[i+1][3] += description[i][3]
      description.pop(i)
    else:
      i += 1

  # convert to words
  DIST_TH = [0, 10, 50, 100, 250, 500, 1500]
  if version == 1:
    TIME_TH = [0, 2, 8, 50]
    ANGLE_TH = [0, 35, 70, 110, 145]
  else:
    TIME_TH = [0, 8, 50]
    ANGLE_TH = [0, 10, 30, 55, 80, 110, 145]
  mirrored = {'straight': 'straight', 'left': 'right', 'right': 'left', 'stop': 'stop'}
  words_original = []
  words_mirror = []
  for row in description:
    if row[0] == 'stop':
      v = np.digitize([row[1]], TIME_TH)[0]
    elif row[0] == 'straight':
      v = np.digitize([row[2]], DIST_TH)[0]
    else:
      v = np.digitize([np.abs(row[3])], ANGLE_TH)[0]

    word = '%s_%s' % (row[0], v)
    words_original.append(word)

    word = '%s_%s' % (mirrored[row[0]], v)
    words_mirror.append(word)

  if variations:
    words_inverted = list(reversed(words_mirror))
    words_mirror_inverted = list(reversed(words_original))
    return [words_original, words_mirror, words_inverted, words_mirror_inverted]

  return words_original
Example #2
0
    def annotate_events(self, df, index_col='frameIndex'):
        self.stats_file = open('%s/stats.txt' % self.data_direc, 'a')
        self.stats_file.seek(0)
        self.stats_file.truncate()

        df['gz'] = util.movingaverage(df['gz'], self.moving_average_size)
        windowed_df_test = util.generate_windows(df, window=self.window_size, ignore_columns=self.ignore_columns)
        windowed_df_test = windowed_df_test[self.active_features]

        predicted_labels_test = self.model.predict(windowed_df_test)

        events = {
                "left_lc_start": set(),
                "left_lc_end"  : set(),
                "right_lc_start": set(),
                "right_lc_end"  : set(),
                }

        left_lc_start = 0
        right_lc_start = 0
        left_lc_end = 0
        right_lc_end = 0
        
        pos_label = 2
        neg_label = 1
        null_label = 0

        l_start = OrderedDict()
        r_start = OrderedDict()

        for i in xrange(len(predicted_labels_test) - 4):
            # starts with OO
            if predicted_labels_test[i+1] == pos_label and len(set(predicted_labels_test[i+1:i+4])) == 1 \
            and (predicted_labels_test[i] == null_label or predicted_labels_test[i] == neg_label or i == 0):
                l_start[i] = 0
                for k in r_start.keys():
                    r_start[k] += 1
            # starts with <<
            if predicted_labels_test[i+1] == neg_label and len(set(predicted_labels_test[i+1:i+4])) == 1 \
            and (predicted_labels_test[i] == null_label or predicted_labels_test[i] == pos_label or i == 0):
                r_start[i] = 0
                for k in l_start.keys():
                    l_start[k] += 1
            # ends with OO
            if predicted_labels_test[i] == pos_label and predicted_labels_test[i+1] == pos_label \
            and (predicted_labels_test[i+2] == null_label or predicted_labels_test[i+2] == neg_label):
                found = False
                for k, v in l_start.items():
                    if v >= 1:
                        del l_start[k]
                        if found:
                            continue
                        signal = df.iloc[k:i]['theta'].tolist()
                        if self.is_valid_event(signal, k, i, 'left'):
                            if (len(events['right_lc_end']) > 0 and k > max(events['right_lc_end']) or len(events['right_lc_end']) == 0) \
                            and (len(events['left_lc_end']) > 0 and k > max(events['left_lc_end']) or len(events['left_lc_end']) == 0):
                                events['left_lc_start'].add(k)
                                events['left_lc_end'].add(i)
                                found = True

            # ends with <<
            if predicted_labels_test[i] == neg_label and predicted_labels_test[i+1] == neg_label \
            and (predicted_labels_test[i+2] == null_label or predicted_labels_test[i+2] == pos_label):
                found = False
                for k, v in r_start.items():
                    if v >= 1:
                        del r_start[k]
                        if found:
                            continue
                        signal = df.iloc[k:i]['theta'].tolist()
                        if self.is_valid_event(signal, k, i, 'right'):
                            if (len(events['right_lc_end']) > 0 and k > max(events['right_lc_end']) or len(events['right_lc_end']) == 0) \
                            and (len(events['left_lc_end']) > 0 and k > max(events['left_lc_end']) or len(events['left_lc_end']) == 0):
                                events['right_lc_start'].add(k)
                                events['right_lc_end'].add(i)
                                found = True

        for k, v in events.iteritems():
            events[k] = sorted(list(v))

        events_indices = []
        for i in xrange(len(events['left_lc_start'])):
            t = (events['left_lc_start'][i], events['left_lc_end'][i], 'left_lane_change')
            events_indices.append(t)

        for i in xrange(len(events['right_lc_start'])):
            t = (events['right_lc_start'][i], events['right_lc_end'][i], 'right_lane_change')
            events_indices.append(t)

        self.stats_file.close()
        
        print events_indices
        
        return events, events_indices