def relative_future_course_speed(speed, nfuture, sample_rate): def norm_course_diff(course): if course > math.pi: course = course - 2 * math.pi if course < -math.pi: course = course + 2 * math.pi return course # given the speed vectors, calculate the future location relative to # the current location, with facing considered course_list = MyDataset.to_course_list(speed) course_list = MyDataset.fix_none_in_course(course_list) # integrate the speed to get the location loc = util_car.integral(speed, 1.0 / sample_rate) out = np.zeros_like(loc) l = out.shape[0] for i in range(l): if i + nfuture < l: fi = min(i + nfuture, l - 1) # first is course diff out[i, 0] = norm_course_diff(course_list[fi] - course_list[i]) # second is the distance out[i, 1] = np.linalg.norm(loc[fi, :] - loc[i, :]) else: # at the end of the video, just use what has before out[i, :] = out[i - 1, :] # normalize the speed to be per second timediff = 1.0 * nfuture / sample_rate out = out / timediff return out
def relative_future_location(speed, nfuture, sample_rate): # given the speed vectors, calculate the future location relative to # the current location, with facing considered course_list = MyDataset.to_course_list(speed) course_list = MyDataset.fix_none_in_course(course_list) # integrate the speed to get the location loc = util_car.integral(speed, 1.0 / sample_rate) # project future motion on to the current facing direction # this is counter clock wise def rotate(vec, theta): c = math.cos(theta) s = math.sin(theta) xp = c * vec[0] - s * vec[1] yp = s * vec[0] + c * vec[1] return np.array([xp, yp]) out = np.zeros_like(loc) l = out.shape[0] for i in range(l): future = loc[min(i + nfuture, l - 1), :] delta = future - loc[i, :] out[i, :] = rotate(delta, course_list[i]) return out