def updatefig(j):
nonlocal rect, rect_
sys.stdout.write('\rFrame: %04d/%04d | FPS: %d | Sequence: [%s]%s' %
(j, total_frames, fps, filename, ' ' * 10))
# First frame rect
if j == 1: rect, rect_ = init_rect, init_rect
# Lucas-Kanade using latest template
template = crop(frames[:, :, j - 1], rect)
p = LucasKanade(template, frames[:, :, j], rect)
rect_temp = (rect.reshape(2, 2) + p).flatten()
# Lucas-Kanade using initial template
p_star = LucasKanade(init_template, frames[:, :, j], rect_temp, p)
rect = (rect.reshape(2, 2) + p_star).flatten()
save_rects.append(rect)
# Lucas-Kanade wihtout drift correction
template_ = crop(frames[:, :, j - 1], rect_)
p_ = LucasKanade(template_, frames[:, :, j], rect_)
rect_ = (rect_.reshape(2, 2) + p_).flatten()
# Update image for display
im.set_array(frames[:, :, j])
# Clear patches and draw new boxes
for patch in reversed(ax.patches):
patch.remove()
box = patches.Rectangle((rect[0], rect[1]),
rect[2] - rect[0],
rect[3] - rect[1],
linewidth=2,
edgecolor='y',
facecolor='none')
ax.add_patch(box)
box = patches.Rectangle((rect_[0], rect_[1]),
rect_[2] - rect_[0],
rect_[3] - rect_[1],
linewidth=2,
edgecolor='g',
facecolor='none')
ax.add_patch(box)
# Save frames
if j in [1, 100, 200, 300, 400]:
fig.savefig('../writeup/carseqrects-wcrt_%d.png' % j,
bbox_inches='tight',
pad_inches=0)
# Save rects
if j == total_frames - 1:
# np.save('carseqrects-wcrt', np.asarray(save_rects))
pass
return im
def LucasKanadeBasis(It, It1, rect, bases, p0=np.zeros(2), threshold=0.001, iters=20):
'''
[input]
* It - Template image
* It1 - Current image
* rect - Current position of the car (top left, bot right coordinates)
* bases - [n, m, k] where (n x m) is the size of the template
* threshold - Threshold for error convergence (default: 0.001)
* iters - Number of iterations for error convergence (default: 20)
[output]
* p - Movement vector [dp_x, dp_y]
'''
# Initial parameters
p = p0
bases = bases.reshape(bases.shape[0] * bases.shape[1], bases.shape[2])
# Iterate
for i in range(iters):
# Step 1 - Warp image
warp_img = shift(It1, np.flip(-p))
# Step 2 - Compute error image
error_img = It - crop(warp_img, rect)
error_img = error_img.flatten() - np.matmul(bases, np.matmul(bases.T, error_img.flatten()))
# Step 3 - Compute and warp the gradient
gradient = np.dstack(np.gradient(warp_img)[::-1])
gradient = np.dstack([crop(gradient[:, :, 0], rect), crop(gradient[:, :, 1], rect)])
warp_gradient = gradient.reshape(gradient.shape[0] * gradient.shape[1], 2)
# Step 4 - Evaluate jacobian
jacobian = np.eye(2)
# Step 5 - Compute the steepest descent images
steepest_descent = np.matmul(warp_gradient, jacobian)
steepest_descent = steepest_descent - np.matmul(bases, np.matmul(bases.T, steepest_descent))
# Step 6 - Compute the Hessian matrix
hessian = np.matmul(steepest_descent.T, steepest_descent)
# Step 7/8 - Compute delta P
delta_p = np.matmul(np.linalg.inv(hessian), np.matmul(steepest_descent.T, error_img.flatten()))
# Step 9 - Update the parameters
p = p + delta_p
# Test for convergence
if np.linalg.norm(delta_p) <= threshold: break
return p
def play(filename):
frames = np.load(filename)
total_frames = frames.shape[2]
fps = 24
# Init figure
fig, ax = plt.subplots(1)
im = ax.imshow(frames[:, :, 0], animated=True, cmap='gray')
# Initial rect
init_rect = np.asarray([59, 116, 145, 151])
rect, rect_ = init_rect, init_rect
save_rects = []
# Initial template
init_template = crop(frames[:, :, 0], init_rect)
def updatefig(j):
nonlocal rect, rect_
sys.stdout.write('\rFrame: %04d/%04d | FPS: %d | Sequence: [%s]%s' %
(j, total_frames, fps, filename, ' ' * 10))
# First frame rect
if j == 1: rect, rect_ = init_rect, init_rect
# Lucas-Kanade using latest template
template = crop(frames[:, :, j - 1], rect)
p = LucasKanade(template, frames[:, :, j], rect)
rect_temp = (rect.reshape(2, 2) + p).flatten()
# Lucas-Kanade using initial template
p_star = LucasKanade(init_template, frames[:, :, j], rect_temp, p)
rect = (rect.reshape(2, 2) + p_star).flatten()
save_rects.append(rect)
# Lucas-Kanade wihtout drift correction
template_ = crop(frames[:, :, j - 1], rect_)
p_ = LucasKanade(template_, frames[:, :, j], rect_)
rect_ = (rect_.reshape(2, 2) + p_).flatten()
# Update image for display
im.set_array(frames[:, :, j])
# Clear patches and draw new boxes
for patch in reversed(ax.patches):
patch.remove()
box = patches.Rectangle((rect[0], rect[1]),
rect[2] - rect[0],
rect[3] - rect[1],
linewidth=2,
edgecolor='y',
facecolor='none')
ax.add_patch(box)
box = patches.Rectangle((rect_[0], rect_[1]),
rect_[2] - rect_[0],
rect_[3] - rect_[1],
linewidth=2,
edgecolor='g',
facecolor='none')
ax.add_patch(box)
# Save frames
if j in [1, 100, 200, 300, 400]:
fig.savefig('../writeup/carseqrects-wcrt_%d.png' % j,
bbox_inches='tight',
pad_inches=0)
# Save rects
if j == total_frames - 1:
# np.save('carseqrects-wcrt', np.asarray(save_rects))
pass
return im
# Run animation and display window
ani = animation.FuncAnimation(fig,
updatefig,
frames=range(1, total_frames),
interval=1000 // fps)
plt.show()
gradient = np.dstack(np.gradient(warp_img)[::-1])
gradient = np.dstack([crop(gradient[:, :, 0], rect), crop(gradient[:, :, 1], rect)])
warp_gradient = gradient.reshape(gradient.shape[0] * gradient.shape[1], 2)
# Step 4 - Evaluate jacobian
jacobian = np.eye(2)
# Step 5 - Compute the steepest descent images
steepest_descent = np.matmul(warp_gradient, jacobian)
steepest_descent = steepest_descent - np.matmul(bases, np.matmul(bases.T, steepest_descent))
# Step 6 - Compute the Hessian matrix
hessian = np.matmul(steepest_descent.T, steepest_descent)
# Step 7/8 - Compute delta P
delta_p = np.matmul(np.linalg.inv(hessian), np.matmul(steepest_descent.T, error_img.flatten()))
# Step 9 - Update the parameters
p = p + delta_p
# Test for convergence
if np.linalg.norm(delta_p) <= threshold: break
return p
if __name__ == '__main__':
frames = np.load('../data/sylvseq.npy')
bases = np.load('../data/sylvbases.npy')
rect = np.asarray([101, 61, 155, 107])
LucasKanadeBasis(crop(frames[:, :, 0], rect), frames[:, :, 1], rect, bases)