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