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))