def getJacobianSE2(params, in_pts, out_pts, pts_size):
(trans_mat, rot_mat, scale_mat) = getDecomposedSE2Matrices(params)
rec_pts = util.dehomogenize(trans_mat * rot_mat * scale_mat * in_pts)
out_pts = np.mat(util.dehomogenize(out_pts))
in_pts = np.mat(util.dehomogenize(in_pts))
pts_diff = np.mat(rec_pts - out_pts).transpose()
theta = params[2]
s = params[3]
cos_theta = math.cos(theta)
sin_theta = math.sin(theta)
ds_mat = np.mat([[cos_theta, -sin_theta], [sin_theta, cos_theta]])
dtheta_mat = np.mat([[-sin_theta, -cos_theta], [cos_theta, -sin_theta]])
ds = np.dot(np.ravel(pts_diff), np.ravel(ds_mat * in_pts, order='F'))
dtheta = (1 + s) * np.dot(np.ravel(pts_diff),
np.ravel(dtheta_mat * in_pts, order='F'))
dtx = np.sum(pts_diff[:, 0])
dty = np.sum(pts_diff[:, 1])
jacobian = 2 * np.array([dtx, dty, dtheta, ds]) / pts_size
# print 'params:\n', params
# print 'ds_mat:\n', ds_mat
# print 'in_pts:\n', in_pts
# print 'out_pts:\n', out_pts
# print 'rec_pts:\n', rec_pts
# print 'pts_diff:\n', pts_diff
# print 'jacobian:\n', jacobian
return jacobian
def getNormalizedPoints(pts):
centroid = np.mean(pts, axis=1)
trans_mat = getTranslationMatrix(-centroid[0], -centroid[1])
trans_mat_inv = getTranslationMatrix(centroid[0], centroid[1])
trans_pts = util.dehomogenize(trans_mat * util.homogenize(pts))
mean_dist = np.mean(np.sqrt(np.sum(np.square(trans_pts), axis=1)))
if mean_dist == 0:
print 'Error in getNormalizedPoints:: mean distance between the given points is zero: ', pts
sys.exit()
norm_scale = math.sqrt(2) / mean_dist
scale_mat = getScalingMatrix(norm_scale - 1)
scale_mat_inv = getScalingMatrix(1.0 / norm_scale - 1)
norm_pts = util.dehomogenize(scale_mat * trans_mat * util.homogenize(pts))
norm_mat_inv = trans_mat_inv * scale_mat_inv
# print 'pts:\n', pts
# print 'centroid:\n', centroid
# print 'trans_mat:\n', trans_mat
# print 'trans_mat_inv:\n', trans_mat_inv
# print 'trans_pts:\n', trans_pts
# print 'mean_dist:', mean_dist
# print 'norm_scale:', norm_scale
# print 'scale_mat:\n', scale_mat
# print 'scale_mat_inv:\n', scale_mat_inv
# print 'norm_mat_inv:\n', norm_mat_inv
return norm_pts, norm_mat_inv
def getJacobianAffine(params, in_pts, out_pts, pts_size):
(trans_mat, rot_mat, scale_mat,
shear_mat) = getDecomposedAffineMatrices(params)
rec_pts = trans_mat * rot_mat * scale_mat * shear_mat * in_pts
shear_rec_pts = util.dehomogenize(shear_mat * in_pts)
out_pts = np.mat(util.dehomogenize(out_pts))
in_pts = np.mat(util.dehomogenize(in_pts))
pts_diff = np.mat(rec_pts - out_pts).transpose()
da = np.dot(pts_diff[0, :], in_pts[0, :])
db = np.dot(pts_diff[0, :], in_pts[1, :])
theta = params[2]
s = params[3]
cos_theta = math.cos(theta)
sin_theta = math.sin(theta)
ds_mat = np.mat([[cos_theta, -sin_theta], [sin_theta, cos_theta]])
dtheta_mat = np.mat([[-sin_theta, -cos_theta], [cos_theta, -sin_theta]])
ds = np.dot(np.ravel(pts_diff), np.ravel(ds_mat * shear_rec_pts,
order='F'))
dtheta = (1 + s) * np.dot(np.ravel(pts_diff),
np.ravel(dtheta_mat * shear_rec_pts, order='F'))
dtx = np.sum(pts_diff[:, 0])
dty = np.sum(pts_diff[:, 1])
jacobian = 2 * np.array([dtx, dty, dtheta, ds, da, db]) / pts_size
return jacobian
def getJacobianScale(scale, in_pts, out_pts, pts_size):
scale_mat = getScalingMatrix(scale)
rec_pts = util.dehomogenize(scale_mat * in_pts)
out_pts = np.mat(util.dehomogenize(out_pts))
in_pts = np.mat(util.dehomogenize(in_pts))
pts_diff = np.mat(rec_pts - out_pts).transpose()
ds = 2 * np.dot(np.ravel(pts_diff), np.ravel(in_pts, order='F')) / pts_size
return ds
def getJacobianShear(params, in_pts, out_pts, pts_size):
shear_mat = getShearingMatrix(params[0], params[1])
rec_pts = shear_mat * in_pts
out_pts = np.mat(util.dehomogenize(out_pts))
in_pts = np.mat(util.dehomogenize(in_pts))
pts_diff = np.mat(rec_pts - out_pts).transpose()
da = np.dot(pts_diff[0, :], in_pts[0, :])
db = np.dot(pts_diff[0, :], in_pts[1, :])
jacobian = 2 * np.array([da, db]) / pts_size
return jacobian
def getJacobianRotate(theta, in_pts, out_pts, pts_size):
rot_mat = getRotationMatrix(theta)
rec_pts = util.dehomogenize(rot_mat * in_pts)
out_pts = np.mat(util.dehomogenize(out_pts))
in_pts = np.mat(util.dehomogenize(in_pts))
pts_diff = np.mat(rec_pts - out_pts).transpose()
cos_theta = math.cos(theta)
sin_theta = math.sin(theta)
dtheta_mat = np.mat([[-sin_theta, -cos_theta], [cos_theta, -sin_theta]])
dtheta = 2 * np.dot(np.ravel(pts_diff),
np.ravel(dtheta_mat * in_pts, order='F')) / pts_size
return dtheta
def getJacobianTrans(params, in_pts, out_pts, pts_size):
trans_mat = getTranslationMatrix(params[0], params[1])
rec_pts = util.dehomogenize(trans_mat * in_pts)
out_pts = np.mat(util.dehomogenize(out_pts))
in_pts = np.mat(util.dehomogenize(in_pts))
pts_diff = np.mat(rec_pts - out_pts).transpose()
dtx = np.sum(pts_diff[:, 0])
dty = np.sum(pts_diff[:, 1])
jacobian = 2 * np.array([dtx, dty]) / pts_size
return jacobian
def shiftGeometricCenterToOrigin(corners):
geom_center_x = np.mean(corners[0, :])
geom_center_y = np.mean(corners[1, :])
trans_mat = np.mat([[1, 0, -geom_center_x], [0, 1, -geom_center_y],
[0, 0, 1]])
trans_mat_inv = np.mat([[1, 0, geom_center_x], [0, 1, geom_center_y],
[0, 0, 1]])
shifted_corners = util.dehomogenize(trans_mat * util.homogenize(corners))
return (shifted_corners, trans_mat_inv)
def getJacobianRT(params, in_pts, out_pts, pts_size):
(trans_mat, rot_mat) = getDecomposedRTMatrices(params)
rec_pts = util.dehomogenize(trans_mat * rot_mat * in_pts)
out_pts = np.mat(util.dehomogenize(out_pts))
in_pts = np.mat(util.dehomogenize(in_pts))
pts_diff = np.mat(rec_pts - out_pts).transpose()
theta = params[2]
cos_theta = math.cos(theta)
sin_theta = math.sin(theta)
dtheta_mat = np.mat([[-sin_theta, -cos_theta], [cos_theta, -sin_theta]])
dtheta = np.dot(np.ravel(pts_diff), np.ravel(dtheta_mat * in_pts,
order='F'))
dtx = np.sum(pts_diff[:, 0])
dty = np.sum(pts_diff[:, 1])
jacobian = 2 * np.array([dtx, dty, dtheta]) / pts_size
return jacobian
def getSSDProj(params, in_pts, out_pts, pts_size):
# print 'getSSDShear:: params:', params
params_len = len(params)
# print 'getSSDShear:: params_len:', params_len
if params_len < 2:
# print'getSSDShear:: Invalid params provided'
params = params[0]
# sys.exit()
proj_mat = getProjectionMatrix(params[0], params[1])
rec_pts = util.homogenize(util.dehomogenize(proj_mat * in_pts))
rec_error = np.sum(np.square(rec_pts - out_pts)) / pts_size
return rec_error
def shiftProjectiveCenterToOrigin(corners):
x1 = corners[0, 0]
y1 = corners[1, 0]
x2 = corners[0, 1]
y2 = corners[1, 1]
x3 = corners[0, 2]
y3 = corners[1, 2]
x4 = corners[0, 3]
y4 = corners[1, 3]
m1 = (y1 - y3) / (x1 - x3)
b1 = y1 - m1 * x1
m2 = (y2 - y4) / (x2 - x4)
b2 = y2 - m2 * x2
proj_center_x = -(b1 - b2) / (m1 - m2)
proj_center_y = m1 * proj_center_x + b1
trans_mat = np.mat([[1, 0, -proj_center_x], [0, 1, -proj_center_y],
[0, 0, 1]])
trans_mat_inv = np.mat([[1, 0, proj_center_x], [0, 1, proj_center_y],
[0, 0, 1]])
shifted_corners = util.dehomogenize(trans_mat * util.homogenize(corners))
return (shifted_corners, trans_mat_inv)
ground_truth = readTrackingData(ground_truth_fname)
no_of_frames = ground_truth.shape[0]
print 'no_of_frames: ', no_of_frames
end_id = no_of_frames
init_corners = np.asarray([ground_truth[0, 0:2].tolist(),
ground_truth[0, 2:4].tolist(),
ground_truth[0, 4:6].tolist(),
ground_truth[0, 6:8].tolist()]).T
std_pts, std_corners = getNormalizedUnitSquarePts(std_resx, std_resy, 0.5)
std_pts_hm = util.homogenize(std_pts)
(init_corners_norm, init_norm_mat) = getNormalizedPoints(init_corners)
init_hom_mat = np.mat(util.compute_homography(std_corners, init_corners_norm))
init_pts_norm = util.dehomogenize(init_hom_mat * std_pts_hm)
init_pts = util.dehomogenize(init_norm_mat * util.homogenize(init_pts_norm))
# ret, init_img = cap.read()
init_img = cv2.imread(src_folder + '/frame{:05d}.jpg'.format(1))
if init_img is None:
raise StandardError(
'The initial image could not be read from the file:\n' + src_folder + '/frame{:05d}.jpg'.format(1))
init_img_gs = cv2.cvtColor(init_img, cv2.cv.CV_BGR2GRAY).astype(np.float64)
if filter_type != 'none':
init_img_gs = applyFilter(init_img_gs, filter_type, kernel_size)
if write_img_data:
init_img_gs.astype(np.uint8).tofile(img_root_dir + '/' + 'frame_0_gs.bin')
init_pixel_vals = np.array([util.bilin_interp(init_img_gs, init_pts[0, pt_id], init_pts[1, pt_id]) for pt_id in
xrange(n_pts)])
while frame_id < no_of_frames:
curr_rot_mat = np.mat(
np.reshape(pose_data[:9, frame_id], (3, 3)).transpose())
curr_trans_mat = np.mat(pose_data[9:, frame_id])
curr_trans_mat = curr_trans_mat.transpose()
print 'curr_rot_mat:\n', curr_rot_mat
print 'curr_trans_mat:\n', curr_trans_mat
curr_trans_mat_tiled = np.tile(curr_trans_mat, 4)
print 'curr_trans_mat_tiled:\n', curr_trans_mat_tiled
curr_corners_hm = calibration_matrix * (
curr_rot_mat * init_corners_3d + curr_trans_mat)
curr_corners = util.dehomogenize(curr_corners_hm)
curr_img = cv2.imread('{:s}/frame{:05d}.jpg'.format(src_dir, frame_id))
drawRegion(curr_img, curr_corners, gt_col, 1)
frame_id += 1
cv2.imshow(gt_corners_window_name, curr_img)
key = cv2.waitKey(1)
if key == 32 or key == ord('p'):
pause_seq = not pause_seq
elif key == 27:
break
# corrected_gt.append(curr_corners)
# n_corrected_get = len(corrected_gt)
# out_file = open(corrected_corners_fname, 'w')
# out_file.write('frame ulx uly urx ury lrx lry llx lly \n')
# print 'dx: ', dx
# print 'dy: ', dy
# print 'dxx: ', dxx
# print 'dxy: ', dxy
# print 'dyx: ', dyx
# print 'dyy: ', dyy
# print 'affine_mat:\n', affine_mat
# print 'rt_mat:\n', rt_mat
# print 'trans_mat:\n', trans_mat
base_corners_hm = util.homogenize(base_corners)
# print 'base_corners_hm: ', base_corners_hm
hom_corners_hm = np.mat(H) * np.mat(base_corners_hm)
hom_corners = util.dehomogenize(hom_corners_hm)
hom_error = math.sqrt(np.sum(np.square(hom_corners - current_corners)) / 4)
hom_errors.append(hom_error)
affine_corners_hm = np.mat(affine_mat) * np.mat(base_corners_hm)
affine_corners = util.dehomogenize(affine_corners_hm)
affine_error = math.sqrt(np.sum(np.square(affine_corners - current_corners)) / 4)
affine_errors.append(affine_error)
comp_corners_hm = np.mat(P) * np.mat(affine_mat) * np.mat(base_corners_hm)
comp_corners = util.dehomogenize(comp_corners_hm)
rt_corners_hm = np.mat(rt_mat) * np.mat(base_corners_hm)
rt_corners = util.dehomogenize(rt_corners_hm)
rt_error = math.sqrt(np.sum(np.square(rt_corners - current_corners)) / 4)
rt_errors.append(rt_error)
sel_corners_fid = open(sel_corners_fname, 'w')
sel_corners_fid.write('frame ulx uly urx ury lrx lry llx lly \n')
writeCorners(sel_corners_fid, init_corners_selected, init_frame_id + 1)
cv2.namedWindow(gt_corners_window_name)
prev_corners_gt = init_corners_gt.copy()
prev_corners_selected = init_corners_selected.copy()
for frame_id in xrange(init_frame_id + 1, no_of_frames):
curr_corners_gt = np.asarray([ground_truth[frame_id, 0:2].tolist(),
ground_truth[frame_id, 2:4].tolist(),
ground_truth[frame_id, 4:6].tolist(),
ground_truth[frame_id, 6:8].tolist()]).T
curr_hom_gt = util.compute_homography(prev_corners_gt, curr_corners_gt)
curr_corners_selected = util.dehomogenize(np.mat(curr_hom_gt) * np.mat(util.homogenize(prev_corners_selected)))
curr_img = cv2.imread('{:s}/frame{:05d}.jpg'.format(src_dir, frame_id + 1))
if len(curr_img.shape) == 2:
curr_img = cv2.cvtColor(curr_img, cv2.COLOR_GRAY2RGB)
drawRegion(curr_img, curr_corners_selected, gt_col, gt_thickness,
annotate_corners=True, annotation_col=annotation_col)
fps_text = 'frame: {:4d}'.format(frame_id + 1)
cv2.putText(curr_img, fps_text, (5, 15), cv2.FONT_HERSHEY_COMPLEX_SMALL, 1, (255, 255, 255))
cv2.imshow(gt_corners_window_name, curr_img)
writeCorners(sel_corners_fid, curr_corners_selected, frame_id + 1)
# abs(templ_corners[1, 2] - templ_corners[1, 1])) / 2)
print 'Object size: {:d}x{:d}'.format(size_x, size_y)
if res_from_size:
res_x = int(size_x / resize_factor)
res_y = int(size_y / resize_factor)
n_pts = res_x * res_y
patch_size_x = res_x * resize_factor
patch_size_y = res_y * resize_factor
std_pts, std_corners = getNormalizedUnitSquarePts(res_x, res_y, 0.5)
std_pts_hm = util.homogenize(std_pts)
init_hom_mat = np.mat(util.compute_homography(std_corners, init_corners))
init_pts = util.dehomogenize(init_hom_mat * std_pts_hm)
init_img_gs = cv2.cvtColor(init_img, cv2.cv.CV_BGR2GRAY).astype(np.float64)
img_height, img_width = init_img_gs.shape
init_pixel_vals = np.mat([util.bilin_interp(init_img_gs, init_pts[0, pt_id], init_pts[1, pt_id]) for pt_id in
xrange(n_pts)])
if init_frame_id == 0:
show_init = 0
if show_patches:
init_patch = np.reshape(init_pixel_vals, (res_y, res_x)).astype(np.uint8)
init_patch_resized = cv2.resize(init_patch, (patch_size_x, patch_size_y))
drawRegion(init_img, init_corners, gt_col, 1, annotate_corners=False)
if show_init:
init_patch_win_name = 'Initial Patch'
else:
cv2.putText(
curr_img, 'Tracker Failed',
(curr_img.shape[0] / 2, curr_img.shape[1] / 2),
legend_font_face, failure_font_size,
col_rgb['red'], failure_font_thickness,
legend_font_line_type)
continue
if show_grid:
try:
curr_hom_mat = np.mat(
util.compute_homography(std_corners, curr_corners))
except:
pass
curr_pts = util.dehomogenize(curr_hom_mat * std_pts_hm)
if show_stacked:
new_img = np.copy(curr_img)
if show_legend:
cv2.rectangle(new_img, (legend_x, legend_y + baseline),
(legend_x + text_size[0],
legend_y - text_size[1] - baseline),
legend_bkg_col, -1)
cv2.putText(new_img, tracker['legend'],
(legend_x, legend_y), legend_font_face,
legend_font_size, col_rgb[tracker['col']],
legend_font_thickness,
legend_font_line_type)
if show_grid:
drawGrid(new_img, curr_pts, grid_res_x, grid_res_y,
no_of_frames = ground_truth.shape[0]
print 'no_of_frames: ', no_of_frames
frame_id2_vec=range(2, no_of_frames)
std_pts, std_corners = getNormalizedUnitSquarePts(std_resx, std_resy)
std_pts_hm = util.homogenize(std_pts)
std_corners_hm = util.homogenize(std_corners)
corners = np.asarray([ground_truth[frame_id1 - 1, 0:2].tolist(),
ground_truth[frame_id1 - 1, 2:4].tolist(),
ground_truth[frame_id1 - 1, 4:6].tolist(),
ground_truth[frame_id1 - 1, 6:8].tolist()]).T
(corners_norm, norm_mat) = getNormalizedPoints(corners)
hom_mat = np.mat(util.compute_homography(std_corners, corners_norm))
pts_norm = util.dehomogenize(hom_mat * std_pts_hm)
pts = util.dehomogenize(norm_mat * util.homogenize(pts_norm))
img = cv2.imread(src_folder + '/frame{:05d}.jpg'.format(frame_id1))
img_gs = cv2.cvtColor(img, cv2.cv.CV_BGR2GRAY).astype(np.float64)
corners2 = np.asarray([ground_truth[frame_id2 - 1, 0:2].tolist(),
ground_truth[frame_id2 - 1, 2:4].tolist(),
ground_truth[frame_id2 - 1, 4:6].tolist(),
ground_truth[frame_id2 - 1, 6:8].tolist()]).T
(corners2_norm, norm_mat) = getNormalizedPoints(corners2)
hom_mat = np.mat(util.compute_homography(std_corners, corners2_norm))
pts2_norm = util.dehomogenize(hom_mat * std_pts_hm)
pts2 = util.dehomogenize(norm_mat * util.homogenize(pts2_norm))
img2 = cv2.imread(src_folder + '/frame{:05d}.jpg'.format(frame_id2))
