def compute_error(self, method, threshold=10): ''' compute the average accuracy over all images ''' if self.debug: assert self.length > 0, 'no enough data' assert method == 'forward' or method == 'backward', 'the accuracy can only be computed based on forward with annotations or backward check' N = 1.0 average_error = 0 for key in self.key_list: if not key in self.pts_valid_index: # if valid index has not already been computed index_tmp = self.index_dict[key] self.compute_valid_tracking(index=index_tmp, threshold=threshold) valid_index_tmp = self.pts_valid_index[key] if method == 'forward': pts_forward, pts_anno = self.pts_forward[key], self.pts_anno[key] if self.debug: assert pts_anno is not None pts_forward_valid, pts_anno_valid = pts_forward[:, valid_index_tmp], pts_anno[:, valid_index_tmp] error_tmp, _ = pts_euclidean(pts_forward_valid, pts_anno_valid, debug=self.debug) elif method == 'backward': pts_backward, pts_root = self.pts_backward[key], self.pts_root[key] if self.debug: assert pts_backward is not None pts_backward_valid, pts_root_valid = pts_backward[:, valid_index_tmp], pts_root[:, valid_index_tmp] error_tmp, _ = pts_euclidean(pts_backward_valid, pts_root_valid, debug=self.debug) average_error = (N-1)/N * average_error + error_tmp / N N += 1.0 return average_error
def compute_valid_tracking(self, index, threshold=10): ''' compute the success of the tracking based on threshold ''' key = self.convert_index2key(index) pts_root_tmp, pts_anno_tmp, pts_forward_tmp, pts_backward_tmp = self.pts_root[key], self.pts_anno[key], self.pts_forward[key], self.pts_backward[key] if pts_anno_tmp is not None: # compute the valid tracking based-on forward-backward check ave_dist, pts_distance = pts_euclidean(pts_forward_tmp, pts_anno_tmp, debug=self.debug) # 1 x N # print 'using anno' # print pts_forward_tmp # print pts_anno_tmp else: # compute the valid tracking based-on accuracy ave_dist, pts_distance = pts_euclidean(pts_root_tmp, pts_backward_tmp, debug=self.debug) # print 'using backward' valid_index = np.where(np.array(pts_distance) < threshold)[0].tolist() pts_forward_valid = pts_forward_tmp[:, valid_index].copy() pts_backward_valid = pts_backward_tmp[:, valid_index].copy() if pts_anno_tmp is not None: ave_dist_valid, _ = pts_euclidean(pts_forward_valid, pts_anno_tmp[:, valid_index], debug=self.debug) else: ave_dist_valid, _ = pts_euclidean(pts_root_tmp[:, valid_index], pts_backward_valid, debug=self.debug) self.pts_valid_index[key] = valid_index return valid_index, pts_forward_valid, pts_backward_valid, ave_dist, ave_dist_valid
def tracking_lk_opencv(input_image1, input_image2, input_pts, backward=False, win_size=15, pyramid=5, warning=True, debug=True):
'''
tracking a set of points in two images using Lucas-Kanade tracking implemented in opencv
parameters:
input_image1, input_image2: a pil or numpy image, color or gray
input_pts: a list of 2 elements, a listoflist of 2 elements:
e.g., [[1,2], [5,6]], a numpy array with shape or (2, N) or (2, )
backward: run backward tracking if true
win_sie: window sized used for lucas kanade tracking
pyramid: number of levels of pyramid used for lucas kanade tracking
outputs:
pts_forward: tracked points in forward pass, 2 x N float32 numpy
pts_bacward: tracked points in backward pass, 2 x N float32 numpy, None is not runnign the backward pass
backward_err_list: a list of error in forward-backward pass check, None if not running the backward pass
found_index_list: a list of 0 or 1, 1 if the tracking converges, 0 if not converging
'''
np_image1, _ = safe_image(input_image1, warning=warning, debug=debug)
np_image2, _ = safe_image(input_image2, warning=warning, debug=debug)
np_pts = safe_2dptsarray(input_pts, homogeneous=False, warning=warning, debug=debug).astype('float32') # 2 x N
if debug: assert isscalar(win_size) and isscalar(pyramid), 'the hyperparameters of lucas-kanade tracking is not correct'
num_pts = np_pts.shape[1]
# formatting the input
if iscolorimage_dimension(np_image1): np_image1 = rgb2gray(np_image1)
if iscolorimage_dimension(np_image2): np_image2 = rgb2gray(np_image2)
lk_params = dict(winSize=(win_size, win_size), maxLevel=pyramid, criteria=(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10000, 0.03))
pts_root = np.expand_dims(np_pts.transpose(), axis=1) # N x 1 x 2
pts_forward, status_for, err_for = cv2.calcOpticalFlowPyrLK(np_image1, np_image2, pts_root, None, **lk_params)
found_index_for = np.where(status_for[:, 0] == 1)[0].tolist()
if backward:
pts_bacward, status_bac, err_bac = cv2.calcOpticalFlowPyrLK(np_image2, np_image1, pts_forward, None, **lk_params)
# print(status_bac)
found_index_bac = np.where(status_bac[:, 0] == 1)[0].tolist()
# aa
pts_bacward = pts_bacward.reshape((-1, 2)).transpose()
_, backward_err_list = pts_euclidean(np_pts, pts_bacward, warning=warning, debug=debug)
found_index_list = find_unique_common_from_lists(found_index_for, found_index_bac, warning=warning, debug=debug)
else:
pts_bacward, backward_err_list = None, None
found_index_list = found_index_for
pts_forward = pts_forward.reshape((-1, 2)).transpose() # 2 x N
return pts_forward, pts_bacward, backward_err_list, found_index_list
def facial_landmark_evaluation(pred_dict_all, anno_dict, num_pts, error_threshold, normalization_ced=True, normalization_vec=False, covariance=True, display_list=None, debug=True, vis=False, save=True, save_path=None):
'''
evaluate the performance of facial landmark detection
parameter:
pred_dict_all: a dictionary for all basline methods. Each key is the method name and the value is corresponding prediction dictionary,
which keys are the image path and values are 2 x N prediction results
anno_dict: a dictionary which keys are the image path and values are 2 x N annotation results
num_pts: number of points
vis: determine if visualizing the pck curve
save: determine if saving the visualization results
save_path: a directory to save all the results
visualization:
1. 2d pck curve (total and point specific) for all points for all methods
2. point error vector (total and point specific) for all points and for all methods
3. mean square error
return:
metrics_all: a list of list to have detailed metrics over all methods
ptswise_mse: a list of list to have average MSE over all key-points for all methods
'''
num_methods = len(pred_dict_all)
if debug:
assert isdict(pred_dict_all) and num_methods > 0 and all(isdict(pred_dict) for pred_dict in pred_dict_all.values()), 'predictions result format is not correct'
assert isdict(anno_dict), 'annotation result format is not correct'
assert ispositiveinteger(num_pts), 'number of points is not correct'
assert isscalar(error_threshold), 'error threshold is not correct'
assert islogical(normalization_ced) and islogical(normalization_vec), 'normalization flag is not correct'
if display_list is not None: assert len(display_list) == num_methods, 'display list is not correct %d vs %d' % (len(display_list), num_methods)
num_images = len(pred_dict_all.values()[0])
if debug:
assert num_images > 0, 'the predictions are empty'
assert num_images == len(anno_dict), 'number of images is not equal to number of annotations: %d vs %d' % (num_images, len(anno_dict))
assert all(num_images == len(pred_dict) for pred_dict in pred_dict_all.values()), 'number of images in results from different methods are not equal'
# calculate normalized mean error for each single image based on point-to-point Euclidean distance normalized by the bounding box size
# calculate point error vector for each single image based on error vector normalized by the bounding box size
normed_mean_error_dict = dict()
normed_mean_error_pts_specific_dict = dict()
normed_mean_error_pts_specific_valid_dict = dict()
pts_error_vec_dict = dict()
pts_error_vec_pts_specific_dict = dict()
mse_error_dict_dict = dict()
for method_name, pred_dict in pred_dict_all.items():
normed_mean_error_total = np.zeros((num_images, ), dtype='float32')
normed_mean_error_pts_specific = np.zeros((num_images, num_pts), dtype='float32')
normed_mean_error_pts_specific_valid = np.zeros((num_images, num_pts), dtype='bool')
pts_error_vec = np.zeros((num_images, 2), dtype='float32')
pts_error_vec_pts_specific = np.zeros((num_images, 2, num_pts), dtype='float32')
mse_error_dict = dict()
count = 0
count_skip_num_images = 0 # it's possible that no annotation exists on some images, than no error should be counted for those images, we count the number of those images
for image_path, pts_prediction in pred_dict.items():
_, filename, _ = fileparts(image_path)
pts_anno = anno_dict[filename] # 2 x N annotation
pts_keep_index = range(num_pts)
# to avoid list object type, do conversion here
if islist(pts_anno): pts_anno = np.asarray(pts_anno)
if islist(pts_prediction): pts_prediction = np.asarray(pts_prediction)
if debug: assert (is2dptsarray(pts_anno) or is2dptsarray_occlusion(pts_anno)) and pts_anno.shape[1] == num_pts, 'shape of annotations is not correct (%d x %d) vs (%d x %d)' % (2, num_pts, pts_anno.shape[0], pts_anno.shape[1])
# if the annotation has 3 channels (include extra occlusion channel, we keep only the points with annotations)
# occlusion: -1 -> visible but not annotated, 0 -> invisible and not annotated, 1 -> visible, we keep only visible and annotated points
if pts_anno.shape[0] == 3:
pts_keep_index = np.where(pts_anno[2, :] == 1)[0].tolist()
if len(pts_keep_index) <= 0: # if no point is annotated in current image
count_skip_num_images += 1
continue
pts_anno = pts_anno[0:2, pts_keep_index]
pts_prediction = pts_prediction[:, pts_keep_index]
# to avoid the point location includes the score or occlusion channel, only take the first two channels here
if pts_prediction.shape[0] == 3 or pts_prediction.shape[0] == 4:
pts_prediction = pts_prediction[0:2, :]
num_pts_tmp = len(pts_keep_index)
if debug:
assert pts_anno.shape[1] <= num_pts, 'number of points is not correct: %d vs %d' % (pts_anno.shape[1], num_pts)
assert pts_anno.shape == pts_prediction.shape, 'shape of annotations and predictions are not the same {} vs {}'.format(print_np_shape(pts_anno, debug=debug), print_np_shape(pts_prediction, debug=debug))
# print 'number of points to keep is %d' % num_pts_tmp
# calculate bbox for normalization
if normalization_ced or normalization_vec:
assert len(pts_keep_index) == num_pts, 'some points are not annotated. Normalization on PCK curve is not allowed.'
bbox_anno = pts2bbox(pts_anno, debug=debug) # 1 x 4
bbox_TLWH = bbox_TLBR2TLWH(bbox_anno, debug=debug) # 1 x 4
bbox_size = math.sqrt(bbox_TLWH[0, 2] * bbox_TLWH[0, 3]) # scalar
# calculate normalized error for all points
normed_mean_error, _ = pts_euclidean(pts_prediction, pts_anno, debug=debug) # scalar
if normalization_ced: normed_mean_error /= bbox_size
normed_mean_error_total[count] = normed_mean_error
mse_error_dict[image_path] = normed_mean_error
if normed_mean_error == 0:
print pts_prediction
print pts_anno
# calculate normalized error point specifically
for pts_index in xrange(num_pts):
if pts_index in pts_keep_index: # if current point not annotated in current image, just keep 0
normed_mean_error_pts_specific_valid[count, pts_index] = True
else: continue
pts_index_from_keep_list = pts_keep_index.index(pts_index)
pts_prediction_tmp = np.reshape(pts_prediction[:, pts_index_from_keep_list], (2, 1))
pts_anno_tmp = np.reshape(pts_anno[:, pts_index_from_keep_list], (2, 1))
normed_mean_error_pts_specifc_tmp, _ = pts_euclidean(pts_prediction_tmp, pts_anno_tmp, debug=debug)
if normalization_ced: normed_mean_error_pts_specifc_tmp /= bbox_size
normed_mean_error_pts_specific[count, pts_index] = normed_mean_error_pts_specifc_tmp
# calculate the point error vector
error_vector = pts_prediction - pts_anno # 2 x num_pts_tmp
if normalization_vec: error_vector /= bbox_size
pts_error_vec_pts_specific[count, :, pts_keep_index] = np.transpose(error_vector)
pts_error_vec[count, :] = np.sum(error_vector, axis=1) / num_pts_tmp
count += 1
print 'number of skipped images is %d' % count_skip_num_images
assert count + count_skip_num_images == num_images, 'all cells in the array must be filled %d vs %d' % (count + count_skip_num_images, num_images)
# print normed_mean_error_total
# time.sleep(1000)
# save results to dictionary
normed_mean_error_dict[method_name] = normed_mean_error_total[:count]
normed_mean_error_pts_specific_dict[method_name] = normed_mean_error_pts_specific[:count, :]
normed_mean_error_pts_specific_valid_dict[method_name] = normed_mean_error_pts_specific_valid[:count, :]
pts_error_vec_dict[method_name] = np.transpose(pts_error_vec[:count, :]) # 2 x num_images
pts_error_vec_pts_specific_dict[method_name] = pts_error_vec_pts_specific[:count, :, :]
mse_error_dict_dict[method_name] = mse_error_dict
# calculate mean value
if mse:
mse_value = dict() # dictionary to record all average MSE for different methods
mse_dict = dict() # dictionary to record all point-wise MSE for different keypoints
for method_name, error_array in normed_mean_error_dict.items():
mse_value[method_name] = np.mean(error_array)
else: mse_value = None
# save mse error list to file for each method
error_list_savedir = os.path.join(save_path, 'error_list')
mkdir_if_missing(error_list_savedir)
for method_name, mse_error_dict in mse_error_dict_dict.items():
mse_error_list_path = os.path.join(error_list_savedir, 'error_%s.txt' % method_name)
mse_error_list = open(mse_error_list_path, 'w')
sorted_tuple_list = sorted(mse_error_dict.items(), key=operator.itemgetter(1), reverse=True)
for tuple_index in range(len(sorted_tuple_list)):
image_path_tmp = sorted_tuple_list[tuple_index][0]
mse_error_tmp = sorted_tuple_list[tuple_index][1]
mse_error_list.write('{:<200} {}\n'.format(image_path_tmp, '%.2f' % mse_error_tmp))
mse_error_list.close()
print '\nsave mse error list for %s to %s' % (method_name, mse_error_list_path)
# visualize the ced (cumulative error distribution curve)
print('visualizing pck curve....\n')
pck_savedir = os.path.join(save_path, 'pck')
mkdir_if_missing(pck_savedir)
pck_savepath = os.path.join(pck_savedir, 'pck_curve_overall.png')
table_savedir = os.path.join(save_path, 'metrics')
mkdir_if_missing(table_savedir)
table_savepath = os.path.join(table_savedir, 'detailed_metrics_overall.txt')
_, metrics_all = visualize_ced(normed_mean_error_dict, error_threshold=error_threshold, normalized=normalization_ced, truncated_list=truncated_list, title='2D PCK curve (all %d points)' % num_pts, display_list=display_list, debug=debug, vis=vis, pck_savepath=pck_savepath, table_savepath=table_savepath)
metrics_title = ['Method Name / Point Index']
ptswise_mse_table = [[normed_mean_error_pts_specific_dict.keys()[index_tmp]] for index_tmp in xrange(num_methods)]
for pts_index in xrange(num_pts):
metrics_title.append(str(pts_index + 1))
normed_mean_error_dict_tmp = dict()
for method_name, error_array in normed_mean_error_pts_specific_dict.items():
normed_mean_error_pts_specific_valid_temp = normed_mean_error_pts_specific_valid_dict[method_name]
# Some points at certain images might not be annotated. When calculating MSE for these specific point, we remove those images to avoid "false" mean average error
valid_array_per_pts_per_method = np.where(normed_mean_error_pts_specific_valid_temp[:, pts_index] == True)[0].tolist()
error_array_per_pts = error_array[:, pts_index]
error_array_per_pts = error_array_per_pts[valid_array_per_pts_per_method]
num_image_tmp = len(valid_array_per_pts_per_method)
# print(num_image_tmp)
if num_image_tmp == 0: continue
# aaa
normed_mean_error_dict_tmp[method_name] = np.reshape(error_array_per_pts, (num_image_tmp, ))
pck_savepath = os.path.join(pck_savedir, 'pck_curve_pts_%d.png' % (pts_index+1))
table_savepath = os.path.join(table_savedir, 'detailed_metrics_pts_%d.txt' % (pts_index+1))
if len(normed_mean_error_dict_tmp) == 0: continue
metrics_dict, _ = visualize_ced(normed_mean_error_dict_tmp, error_threshold=error_threshold, normalized=normalization_ced, truncated_list=truncated_list, display2terminal=False, title='2D PCK curve for point %d' % (pts_index+1), display_list=display_list, debug=debug, vis=vis, pck_savepath=pck_savepath, table_savepath=table_savepath)
for method_index in range(num_methods):
method_name = normed_mean_error_pts_specific_dict.keys()[method_index]
ptswise_mse_table[method_index].append('%.1f' % metrics_dict[method_name]['MSE'])
# reorder the table
order_index_list = [display_list.index(method_name_tmp) for method_name_tmp in normed_mean_error_pts_specific_dict.keys()]
order_index_list = [0] + [order_index_tmp + 1 for order_index_tmp in order_index_list]
# print table to terminal
ptswise_mse_table = list_reorder([metrics_title] + ptswise_mse_table, order_index_list, debug=debug)
table = AsciiTable(ptswise_mse_table)
print '\nprint point-wise average MSE'
print table.table
# save table to file
ptswise_savepath = os.path.join(table_savedir, 'pointwise_average_MSE.txt')
table_file = open(ptswise_savepath, 'w')
table_file.write(table.table)
table_file.close()
print '\nsave point-wise average MSE to %s' % ptswise_savepath
# visualize the error vector map
# print('visualizing error vector distribution map....\n')
# error_vec_save_dir = os.path.join(save_path, 'error_vec')
# mkdir_if_missing(error_vec_save_dir)
# savepath_tmp = os.path.join(error_vec_save_dir, 'error_vector_distribution_all.png')
# visualize_pts(pts_error_vec_dict, title='Point Error Vector Distribution (all %d points)' % num_pts, mse=mse, mse_value=mse_value, display_range=display_range, display_list=display_list, xlim=xlim, ylim=ylim, covariance=covariance, debug=debug, vis=vis, save_path=savepath_tmp)
# for pts_index in xrange(num_pts):
# pts_error_vec_pts_specific_dict_tmp = dict()
# for method_name, error_vec_dict in pts_error_vec_pts_specific_dict.items():
# pts_error_vec_pts_specific_valid = normed_mean_error_pts_specific_valid_dict[method_name] # get valid flag
# valid_image_index_per_pts = np.where(pts_error_vec_pts_specific_valid[:, pts_index] == True)[0].tolist() # get images where the points with current index are annotated
# print(len(valid_image_index_per_pts))
# pts_error_vec_pts_specific_dict_tmp[method_name] = np.transpose(error_vec_dict[valid_image_index_per_pts, :, pts_index]) # 2 x num_images
# savepath_tmp = os.path.join(error_vec_save_dir, 'error_vector_distribution_pts_%d.png' % (pts_index+1))
# if mse:
# mse_dict_tmp = visualize_pts(pts_error_vec_pts_specific_dict_tmp, title='Point Error Vector Distribution for Point %d' % (pts_index+1), mse=mse, display_range=display_range, display_list=display_list, xlim=xlim, ylim=ylim, covariance=covariance, debug=debug, vis=vis, save_path=savepath_tmp)
# mse_best = min(mse_dict_tmp.values())
# mse_single = dict()
# mse_single['mse'] = mse_best
# mse_single['num_images'] = len(valid_image_index_per_pts) # assume number of valid images is equal for all methods
# mse_dict[pts_index] = mse_single
# else:
# visualize_pts(pts_error_vec_pts_specific_dict_tmp, title='Point Error Vector Distribution for Point %d' % (pts_index+1), mse=mse, display_range=display_range, display_list=display_list, xlim=xlim, ylim=ylim, covariance=covariance, debug=debug, vis=vis, save_path=savepath_tmp)
# save mse to json file for further use
# if mse:
# json_path = os.path.join(save_path, 'mse_pts.json')
# # if existing, compare and select the best
# if is_path_exists(json_path):
# with open(json_path, 'r') as file:
# mse_dict_old = json.load(file)
# file.close()
# for pts_index, mse_single in mse_dict_old.items():
# mse_dict_new = mse_dict[int(pts_index)]
# mse_new = mse_dict_new['mse']
# if mse_new < mse_single['mse']:
# mse_single['mse'] = mse_new
# mse_dict_old[pts_index] = mse_single
# with open(json_path, 'w') as file:
# print('overwrite old mse to {}'.format(json_path))
# json.dump(mse_dict_old, file)
# file.close()
# else:
# with open(json_path, 'w') as file:
# print('save mse for all keypoings to {}'.format(json_path))
# json.dump(mse_dict, file)
# file.close()
print('\ndone!!!!!\n')
return metrics_all, ptswise_mse_table
def visualize_pts(pts,
title=None,
fig=None,
ax=None,
display_range=False,
xlim=[-100, 100],
ylim=[-100, 100],
display_list=None,
covariance=False,
mse=False,
mse_value=None,
vis=True,
save_path=None,
debug=True,
closefig=True):
'''
visualize point scatter plot
parameter:
pts: 2 x num_pts numpy array or a dictionary containing 2 x num_pts numpy array
'''
if debug:
if isdict(pts):
for pts_tmp in pts.values():
assert is2dptsarray(
pts_tmp
), 'input points within dictionary are not correct: (2, num_pts) vs %s' % print_np_shape(
pts_tmp)
if display_list is not None:
assert islist(display_list) and len(display_list) == len(
pts), 'the input display list is not correct'
assert CHECK_EQ_LIST_UNORDERED(
display_list, pts.keys(), debug=debug
), 'the input display list does not match the points key list'
else:
display_list = pts.keys()
else:
assert is2dptsarray(
pts
), 'input points are not correct: (2, num_pts) vs %s' % print_np_shape(
pts)
if title is not None: assert isstring(title), 'title is not correct'
else: title = 'Point Error Vector Distribution Map'
assert islogical(
display_range
), 'the flag determine if to display in a specific range should be logical value'
if display_range:
assert islist(xlim) and islist(ylim) and len(xlim) == 2 and len(
ylim) == 2, 'the input range for x and y is not correct'
assert xlim[1] > xlim[0] and ylim[1] > ylim[
0], 'the input range for x and y is not correct'
# figure setting
width, height = 1024, 1024
fig, _ = get_fig_ax_helper(fig=fig, ax=ax, width=width, height=height)
if ax is None:
plt.title(title, fontsize=20)
if isdict(pts):
num_pts_all = pts.values()[0].shape[1]
if all(pts_tmp.shape[1] == num_pts_all
for pts_tmp in pts.values()):
plt.xlabel('x coordinate (%d points)' %
pts.values()[0].shape[1],
fontsize=16)
plt.ylabel('y coordinate (%d points)' %
pts.values()[0].shape[1],
fontsize=16)
else:
print('number of points is different across different methods')
plt.xlabel('x coordinate', fontsize=16)
plt.ylabel('y coordinate', fontsize=16)
else:
plt.xlabel('x coordinate (%d points)' % pts.shape[1], fontsize=16)
plt.ylabel('y coordinate (%d points)' % pts.shape[1], fontsize=16)
plt.axis('equal')
ax = plt.gca()
ax.grid()
# internal parameters
pts_size = 5
std = None
conf = 0.98
color_index = 0
marker_index = 0
hatch_index = 0
alpha = 0.2
legend_fontsize = 10
scale_distance = 48.8
linewidth = 2
# plot points
handle_dict = dict() # for legend
if isdict(pts):
num_methods = len(pts)
assert len(color_set) * len(marker_set) >= num_methods and len(
color_set
) * len(
hatch_set
) >= num_methods, 'color in color set is not enough to use, please use different markers'
mse_return = dict()
for method_name, pts_tmp in pts.items():
color_tmp = color_set[color_index]
marker_tmp = marker_set[marker_index]
hatch_tmp = hatch_set[hatch_index]
# plot covariance ellipse
if covariance:
_, covariance_number = visualize_pts_covariance(
pts_tmp[0:2, :],
std=std,
conf=conf,
ax=ax,
debug=debug,
color=color_tmp,
hatch=hatch_tmp,
linewidth=linewidth)
handle_tmp = ax.scatter(pts_tmp[0, :],
pts_tmp[1, :],
color=color_tmp,
marker=marker_tmp,
s=pts_size,
alpha=alpha)
if mse:
if mse_value is None:
num_pts = pts_tmp.shape[1]
mse_tmp, _ = pts_euclidean(pts_tmp[0:2, :],
np.zeros((2, num_pts),
dtype='float32'),
debug=debug)
else:
mse_tmp = mse_value[method_name]
display_string = '%s, MSE: %.1f (%.1f um), Covariance: %.1f' % (
method_name, mse_tmp, mse_tmp * scale_distance,
covariance_number)
mse_return[method_name] = mse_tmp
else:
display_string = method_name
handle_dict[display_string] = handle_tmp
color_index += 1
if color_index / len(color_set) == 1:
marker_index += 1
hatch_index += 1
color_index = color_index % len(color_set)
# reorder the handle before plot
handle_key_list = handle_dict.keys()
handle_value_list = handle_dict.values()
order_index_list = [
display_list.index(method_name_tmp.split(', ')[0])
for method_name_tmp in handle_dict.keys()
]
ordered_handle_key_list = list_reorder(handle_key_list,
order_index_list,
debug=debug)
ordered_handle_value_list = list_reorder(handle_value_list,
order_index_list,
debug=debug)
plt.legend(list2tuple(ordered_handle_value_list),
list2tuple(ordered_handle_key_list),
scatterpoints=1,
markerscale=4,
loc='lower left',
fontsize=legend_fontsize)
else:
color_tmp = color_set[color_index]
marker_tmp = marker_set[marker_index]
hatch_tmp = hatch_set[hatch_index]
handle_tmp = ax.scatter(pts[0, :],
pts[1, :],
color=color_tmp,
marker=marker_tmp,
s=pts_size,
alpha=alpha)
# plot covariance ellipse
if covariance:
_, covariance_number = visualize_pts_covariance(
pts[0:2, :],
std=std,
conf=conf,
ax=ax,
debug=debug,
color=color_tmp,
hatch=hatch_tmp,
linewidth=linewidth)
if mse:
if mse_value is None:
num_pts = pts.shape[1]
mse_tmp, _ = pts_euclidean(pts[0:2, :],
np.zeros((2, num_pts),
dtype='float32'),
debug=debug)
display_string = 'MSE: %.1f (%.1f um), Covariance: %.1f' % (
mse_tmp, mse_tmp * scale_distance, covariance_number)
mse_return = mse_tmp
else:
display_string = 'MSE: %.1f (%.1f um), Covariance: %.1f' % (
mse_value, mse_value * scale_distance, covariance_number)
mse_return = mse_value
handle_dict[display_string] = handle_tmp
plt.legend(list2tuple(handle_dict.values()),
list2tuple(handle_dict.keys()),
scatterpoints=1,
markerscale=4,
loc='lower left',
fontsize=legend_fontsize)
# display only specific range
if display_range:
axis_bin = 10 * 2
interval_x = (xlim[1] - xlim[0]) / axis_bin
interval_y = (ylim[1] - ylim[0]) / axis_bin
plt.xlim(xlim[0], xlim[1])
plt.ylim(ylim[0], ylim[1])
plt.xticks(np.arange(xlim[0], xlim[1] + interval_x, interval_x))
plt.yticks(np.arange(ylim[0], ylim[1] + interval_y, interval_y))
plt.grid()
save_vis_close_helper(fig=fig,
ax=ax,
vis=vis,
save_path=save_path,
warning=warning,
debug=debug,
closefig=closefig,
transparent=False)
return mse_return
def triangulate_two_views(input_pts1,
input_pts2,
projection1,
projection2,
scaling_factor=1,
warning=True,
debug=True):
'''
triangulation from two views
'''
# projection1 - 3 x 4 Camera Matrix 1
# pts_array1 - 3 x N set of points
# projection2 - 3 x 4 Camera Matrix 2
# pts_array2 - 3 x N set of points
try:
pts_array1 = safe_2dptsarray(input_pts1,
homogeneous=True,
warning=warning,
debug=debug)
except AssertionError:
pts_array1 = safe_2dptsarray(input_pts1,
homogeneous=False,
warning=warning,
debug=debug)
if debug:
assert is2dptsarray(pts_array1) or is2dptsarray_occlusion(
pts_array1) or is2dptsarray_confidence(
pts_array1), 'first input points are not correct'
try:
pts_array2 = safe_2dptsarray(input_pts2,
homogeneous=True,
warning=warning,
debug=debug)
except AssertionError:
pts_array2 = safe_2dptsarray(input_pts2,
homogeneous=False,
warning=warning,
debug=debug)
if debug:
assert is2dptsarray(pts_array2) or is2dptsarray_occlusion(
pts_array2) or is2dptsarray_confidence(
pts_array2), 'second input points are not correct'
pts_array1 = pts_array1.astype('float32')
pts_array2 = pts_array2.astype('float32')
# initialization
num_pts = pts_array1.shape[1]
pts_array1[0:2, :] = pts_array1[0:2, :] / float(scaling_factor)
pts_array2[0:2, :] = pts_array2[0:2, :] / float(scaling_factor)
# least square
p1T1 = projection1[0, :] # 1 x 4
p1T2 = projection1[1, :]
p1T3 = projection1[2, :]
p2T1 = projection2[0, :]
p2T2 = projection2[1, :]
p2T3 = projection2[2, :]
# condition_matrix = np.array([[1e-3, 0, 0, 0], # 4 x 4
# [0, 1e-3, 0, 0],
# [0, 0, 1e-3, 0],
# [0, 0, 0, 1e-6]])
condition_matrix = np.array([
[1, 0, 0, 0], # 4 x 4
[0, 1, 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]
])
pts_3d = np.zeros((num_pts, 4), 'float32') # N x 4
pts_3d.fill(-1)
p1_proj = pts_array1.copy().transpose() # N x 3
p2_proj = pts_array2.copy().transpose()
for i in range(num_pts):
if pts_array1[2, i] == -1 or pts_array2[2, i] == -1: continue
U = np.zeros((6, 4), dtype='float32') # 6 x 4
U[0, :] = np.multiply(pts_array1[1, i], p1T3) - p1T2 # y * p1T3 - p1T2
U[1, :] = p1T1 - np.multiply(pts_array1[0, i], p1T3) # p1T1 - x * p1T3
U[2, :] = np.multiply(pts_array1[0, i], p1T2) - np.multiply(
pts_array1[1, i], p1T1) # x * p1T2 - y * p1T1
U[3, :] = np.multiply(pts_array2[1, i], p2T3) - p2T2 # y * p2T3 - p2T2
U[4, :] = p2T1 - np.multiply(pts_array2[0, i], p2T3) # p2T1 - x * p2T3
U[5, :] = np.multiply(pts_array2[0, i], p2T2) - np.multiply(
pts_array2[1, i], p2T1) # x * p2T2 - y * p2T1
conditioned_U = np.matmul(U, condition_matrix)
# print(conditioned_U)
_, _, V = np.linalg.svd(conditioned_U)
conditioned_pts_3d = V[
-1, :] # 4 x 1, V is the transpose version of V in matlab
pts_3d[i, :] = np.matmul(condition_matrix,
conditioned_pts_3d.reshape(
(4, 1))).transpose()
pts_3d[i, :] = np.divide(pts_3d[i, :], pts_3d[i, 3]) # N x 4
# print(pts_3d)
# compute reprojection error
p1_proj[i, :] = (np.matmul(projection1,
pts_3d[i, :].transpose().reshape(
(4, 1)))).transpose() # 1 x 3
p2_proj[i, :] = (np.matmul(projection2,
pts_3d[i, :].transpose().reshape(
(4, 1)))).transpose()
p1_proj[i, :] = np.divide(p1_proj[i, :], p1_proj[i, -1])
p2_proj[i, :] = np.divide(p2_proj[i, :], p2_proj[i, -1])
# error = error + np.norm(p1_proj[i, 0:2] - pts_array1[0:2, i]) + np.norm(p2_proj[i, 0:2] - pts_array2[0:2, i])
error_tmp, error_list = pts_euclidean(p1_proj[:, 0:2].transpose(),
pts_array1[0:2, :],
warning=warning,
debug=debug)
error = error_tmp * num_pts
print(error_list)
error_tmp, error_list = pts_euclidean(p2_proj[:, 0:2].transpose(),
pts_array2[0:2, :],
warning=warning,
debug=debug)
error += error_tmp * num_pts
print(error_list)
pts_3d = pts_3d[:, 0:3].transpose() # 3 x N
return pts_3d, p1_proj.transpose(), p2_proj.transpose()
def triangulate_multiple_views(input_pts,
projection,
scaling_factor=1,
warning=True,
debug=True):
'''
triangulation from M views
'''
# projection - M x 3 x 4 Camera Matrix 1
# pts_array - M x 3 x N set of points
pts_array = input_pts.copy().astype('float32')
projection = projection.copy()
# initialization
num_pts = pts_array.shape[2]
num_cam = pts_array.shape[0]
# pts_array[:, 0:2, :] = pts_array[:, 0:2, :] / float(scaling_factor)
condition_matrix = np.array([
[1e-4, 0, 0, 0], # 4 x 4
[0, 1e-4, 0, 0],
[0, 0, 1e-4, 0],
[0, 0, 0, 1e-7]
])
# condition_matrix = np.array([[1, 0, 0, 0], # 4 x 4
# [0, 1, 0, 0],
# [0, 0, 1, 0],
# [0, 0, 0, 1]])
pts_3d = np.zeros((num_pts, 4), 'float32') # N x 4
pts_3d.fill(-1)
pts_proj = pts_array.copy() # M x 3 x N
pts_merged = pts_array.copy() # M x 3 x N
for i in range(num_pts):
count = 0
cam_valid_list = []
for cam_index in range(num_cam):
if pts_array[cam_index, 2, i] == 1:
cam_valid_list.append(cam_index)
count += 1
if count < 2:
continue # triangulation is valid until more than 1 point
# print(i)
# print(cam_valid_list)
U = np.zeros((3 * num_cam, 4), dtype='float32') # 6 x 4
for cam_index in cam_valid_list:
p1T1 = projection[cam_index, 0, :] # 1 x 4
p1T2 = projection[cam_index, 1, :]
p1T3 = projection[cam_index, 2, :]
U[0 + cam_index * 3, :] = np.multiply(
pts_array[cam_index, 1, i], p1T3) - p1T2 # y * p1T3 - p1T2
U[1 + cam_index * 3, :] = p1T1 - np.multiply(
pts_array[cam_index, 0, i], p1T3) # p1T1 - x * p1T3
U[2 + cam_index * 3, :] = np.multiply(
pts_array[cam_index, 0, i], p1T2) - np.multiply(
pts_array[cam_index, 1, i], p1T1) # x * p1T2 - y * p1T1
conditioned_U = np.matmul(U, condition_matrix)
_, _, V = np.linalg.svd(conditioned_U)
conditioned_pts_3d = V[
-1, :] # 4 x 1, V is the transpose version of V in matlab
pts_3d[i, :] = np.matmul(condition_matrix,
conditioned_pts_3d.reshape(
(4, 1))).transpose()
pts_3d[i, :] = np.divide(pts_3d[i, :], pts_3d[i, 3]) # N x 4
# compute reprojection error
for cam_index in range(num_cam):
# print((np.matmul(projection[cam_index, :, :], pts_3d[i, :].transpose().reshape((4, 1)))).shape)
pts_proj[cam_index, :,
i] = np.matmul(projection[cam_index, :, :],
pts_3d[i, :].transpose().reshape(
(4, 1))).reshape((3, )) # 3 x 1
pts_proj[cam_index, :, i] = np.divide(pts_proj[cam_index, :, i],
pts_proj[cam_index, -1, i])
if pts_array[cam_index, 2, i] != 1:
pts_merged[cam_index, :, i] = pts_proj[
cam_index, :,
i].copy() # keep the original, replace the empty one
for cam_index in range(num_cam):
valid_index_list = np.where(pts_array[cam_index,
2, :] == 1)[0].tolist()
# print(type(valid_index_list))
# print(valid_index_list)
# print(pts_proj[cam_index, 0:2, :])
# print(pts_proj[cam_index, 0:2, valid_index_list])
# bbb
error_tmp, error_list = pts_euclidean(
pts_proj[cam_index, 0:2, valid_index_list].transpose(),
pts_array[cam_index, 0:2, valid_index_list].transpose(),
warning=warning,
debug=debug)
# error_tmp, error_list = pts_euclidean(p1_proj[:, 0:2].transpose(), pts_array1[0:2, :], warning=warning, debug=debug)
# error_tmp, error_list = pts_euclidean(p1_proj[:, 0:2].transpose(), pts_array1[0:2, :], warning=warning, debug=debug)
# error = error_tmp * num_pts
# print(error_list)
pts_3d = pts_3d[:, 0:3].transpose() # 3 x N
return pts_3d, pts_proj, pts_merged