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
