diff --git a/.gitignore b/.gitignore index 77ceb39..5350931 100644 --- a/.gitignore +++ b/.gitignore @@ -125,3 +125,7 @@ dmypy.json # Datas data/ +experiments/ +multiviewunsynch/drone-tracking-datasets +multiviewunsynch/*.png +multiviewunsynch/*-datasets.png \ No newline at end of file diff --git a/multiviewunsynch/analysis/analysis_reconstruction.py b/multiviewunsynch/analysis/analysis_reconstruction.py new file mode 100644 index 0000000..c7880e5 --- /dev/null +++ b/multiviewunsynch/analysis/analysis_reconstruction.py @@ -0,0 +1,332 @@ +import numpy as np +import pickle +import tools.visualization as vis +from datetime import datetime +from analysis.compare_gt import align_gt +from reconstruction import synchronization as sync + +from reconstruction import epipolar as ep +from tools.util import match_overlap, homogeneous +from matplotlib import pyplot as plt +import os +import cv2 +import argparse +from glob import glob +import json + +def reproject_ground_truth(cameras, gt_pts, gt_dets, res_json, ref_cam=0, output_dir='', prefix='', flight=None, ax=None): + ''' + Function: + Compute and plot the reprojection errors of the ground truth matches + Input: + cameras = the list of Camera objects to be evaluated + gt_pts = the ground truth matches + n_bins = number of bins for plotting the histogram of the errors + ''' + f = open(os.path.join(output_dir, 'result.txt'), 'a') + f.write(prefix) + + res = {} + + # triangulate points + gt_pts2d = [] + traj_pts2d = [] + timestamp = gt_dets[ref_cam][0] + Projs = [] + for cam, gt_pt, gt_det in zip(cameras, gt_pts, gt_dets): + gt_pts2d.append(cam.undist_point(gt_pt.T)) + + # sample to the ref camera + _, mgt_det = match_overlap(gt_dets[ref_cam], gt_det) + traj_pts2d.append(mgt_det) + #traj_pts2d.append(np.vstack([mgt_det[0],cam.undist_point(mgt_det[1:])])) + Projs.append(cam.P) + timestamp = np.intersect1d(timestamp, mgt_det[0]) + + traj_pts2d = list(map(lambda x: x[1:,np.isin(x[0], timestamp)], traj_pts2d)) + + # stack points + gt_pts2d = np.vstack(gt_pts2d) + traj_pts2d = np.vstack(traj_pts2d) + + # triangulate gt + X_gt = ep.triangulate_matlab_mv(gt_pts2d, Projs) + Traj_gt = ep.triangulate_matlab_mv(traj_pts2d, Projs) + + # plot the reconstructed scene + vis.show_3D_all(np.empty([3,0]), X_gt, np.empty([3,0]), Traj_gt, label=['','Reconstructed Ground Truth Matches','','Reconstructed Ground Truth Trajectory'], color=True, line=False, flight=flight, output_dir=output_dir, title=prefix+'reconstructed_scene') + + gt_err = [] + traj_err = [] + gt_labels = [] + traj_labels = [] + match_err = [] + match_labels = [] + for i, (cam, gt_pt, gt_det) in enumerate(zip(cameras, gt_pts, gt_dets)): + # log K and d after BA + f.write(f'\ncamera {i} K:\n {np.array2string(cam.K)}') + f.write(f'\ncamera {i} d:\n {np.array2string(cam.d)}') + cam_res = {} + cam_res['K'] = cam.K.tolist() + cam_res['d'] = cam.d.tolist() + + # reproject to image -- static matches + pts = cam.undist_point(cam.kp[cam.index_registered_2d,:].T) + err = ep.reprojection_error(pts, cam.projectPoint(flight.static[:,flight.inlier_mask==1])) + match_err.append(err) + match_labels.append('{}cam{}'.format(prefix,i)) + print("mean reprojection error of static features in camera %d is %f" %(i, np.mean(err))) + f.write(f'\n\nmean reprojection error of static features in camera {i} is {np.mean(err)}') + cam_res['err_static'] = np.mean(err) + + # reproject to image -- ground truth static matches + gt_repro = cam.dist_point3d(X_gt[:-1].T) + err1 = ep.reprojection_error(gt_pts2d[2*i:2*i+2], cam.projectPoint(X_gt)) + gt_err.append(err1) + gt_labels.append('{}cam{}'.format(prefix,i)) + print("mean reprojection error of the ground truth matches in camera %d is %f" %(i, np.mean(err1))) + f.write(f'\n\nmean reprojection error of the ground truth matches in camera {i} is {np.mean(err1)}') + cam_res['err_cp'] = np.mean(err1) + + # reproject trajectories -- detection matches + # if static-dynamic -- use the fitted trajectory + # if len(flight.traj) > 0: + # traj_repro = cam.dist_point3d(flight.traj[1:]) + # err2 = flight.error_cam(i,mode='dist') + # else: + traj_repro = cam.dist_point3d(Traj_gt[:-1].T) + err2 = ep.reprojection_error(traj_pts2d[2*i:2*i+2], cam.projectPoint(Traj_gt)) + traj_err.append(err2) + traj_labels.append('{}cam{}'.format(prefix,i)) + print("mean reprojection error of the dynamic features in camera %d is %f" %(i, np.mean(err2))) + f.write(f'\n\nmean reprojection error of the dynamic features in camera {i} is {np.mean(err2)}') + cam_res['err_traj'] = np.mean(err2) + + # compute the camera center + cam_centers = [] + for j, cam_other in enumerate(cameras): + if i == j: + continue + cam_centers.append(-cam_other.R.T @ cam_other.t.reshape(-1,1)) + + cam_centers = np.hstack(cam_centers) + cam_centers = cam.dist_point3d(cam_centers.T) + + h, w, _ = cam.img.shape + cam_centers = cam_centers[:,(cam_centers[0] >=0) & (cam_centers[0] <= w) & (cam_centers[1] >= 0) & (cam_centers[1] <= h)] + + # plot reprojected points on the image + vis.show_2D_all(gt_pt.T, gt_repro, cam.dist_point2d(traj_pts2d[2*i:2*i+2]), traj_repro, vec=1, title=prefix+'cam'+str(i)+'_ground_truth_reprojection', color=True, line=False, bg=cam.img, output_dir=output_dir, label=['ground truth matches', 'reconstructed ground truth matches', 'extracted dynamic features', 'reconstructed trajectories'], cam_center=cam_centers) + + res[f'cam{i}'] = cam_res + # plot the reprojection error boxplot + # vis.error_boxplot(gt_err, gt_labels, title=prefix+'reprojection_error_static_ground_truth.png', ax=ax[0]) + # vis.error_boxplot(traj_err, traj_labels, title=prefix+'reprojection_error_dynamic.png', ax=ax[1]) + # vis.error_boxplot(match_err, match_labels, title=prefix+'reprojection_error_static.png', ax=ax[2]) + + f.write('\n\n#################################################################\n\n') + f.close() + + res_json[prefix[:-1]] = res + + return gt_err, gt_labels, traj_err, traj_labels, match_err, match_labels + +def convert_timestamps(gt_dets, alphas, betas): + ''' + Function: + convert the detection timestamps into the global timestamps according to the computed alpha, beta + Input: + cameras = the list of cameras to be evaluated + gt_dets = ground truth detections + Output: + gt_dets_global = the detection pairs in the global time frame + ''' + for gt_det, alpha, beta in zip(gt_dets, alphas, betas): + gt_det[0] = alpha * gt_det[0] + beta + + return gt_dets + +def undistort_image(cam, output_dir = '', title=None): + HEIGHT, WIDTH, _ = cam.img.shape + newK, roi = cv2.getOptimalNewCameraMatrix(cam.K, cam.d, (WIDTH, HEIGHT), 1, (WIDTH, HEIGHT)) + + dst = cv2.undistort(cam.img, cam.K, cam.d, None, newK) + x, y, w, h = roi + dst = dst[y:y+h, x:x+w] + + if w == 0 or h == 0: + return + + if title is not None: + cv2.imwrite(os.path.join(output_dir, title), dst) + else: + cv2.imwrite(os.path.join(output_dir, 'undistorted.png'), dst) + + +def main(data_file_static, data_file_static_dynamic, data_file_static_dynamic_no_sync, gt_static_file, output_dir): + res_json = {} + + gt_static = [] + for gfs in gt_static_file: + gt_static.append(np.loadtxt(gfs, delimiter=' ')) + + # read result files + with open(data_file_static, 'rb') as file: + flight_static = pickle.load(file) + + with open(data_file_static_dynamic, 'rb') as file: + flight_static_dynamic = pickle.load(file) + + with open(data_file_static_dynamic_no_sync, 'rb') as file: + flight_static_dynamic_no_sync = pickle.load(file) + + print("Undistort images") + for i, cam in enumerate(flight_static.cameras): + undistort_image(cam, output_dir=output_dir, title='static_only_cam{}.png'.format(i)) + for i, cam in enumerate(flight_static_dynamic.cameras): + undistort_image(cam, output_dir=output_dir, title='static_dynamic_sync_cam{}.png'.format(i)) + for i, cam in enumerate(flight_static_dynamic_no_sync.cameras): + undistort_image(cam, output_dir=output_dir, title='static_dynamic_unsync_cam{}.png'.format(i)) + + # Analysis + # 2D reprojection error + print('\n#################################################################\n') + print("Plot reprojection error") + + _, axs1 = plt.subplots(3, 1, sharey=True, tight_layout=True) + _, axs2 = plt.subplots(3, 1, sharey=True, tight_layout=True) + _, axs3 = plt.subplots(3, 1, sharey=True, tight_layout=True) + + print('\n#################################################################\n') + print("Reconstructions from static-only setting") + + # add detections to static-only + flight_static.detections = flight_static_dynamic_no_sync.detections + # flight_static.detections = [gt_dynamic[x].copy() for x in flight_static.sequence] + flight_static.settings['cf_exact'] = True + flight_static.cut_detection(second=flight_static.settings['cut_detection_second']) + flight_static.init_alpha() + flight_static.time_shift() + # Convert raw detections into the global timeline + flight_static.detection_to_global() + gt_dynamic2 = [flight_static.detections_global[x] for x in flight_static.sequence] + gt_err1, gt_label1, traj_err1, traj_label1, match_err1, match_label1 = reproject_ground_truth(flight_static.cameras, gt_static, gt_dynamic2, res_json,ref_cam=flight_static_dynamic.ref_cam, output_dir=output_dir, prefix='static_only_', flight=flight_static, ax=[axs1[0],axs2[0],axs3[0]]) + plt.show() + print('\n#################################################################\n') + print("Reconstructions from static-dynamic setting") + gt_dynamic3 = [flight_static_dynamic.detections[x].copy() for x in flight_static_dynamic.sequence] + flight_static_dynamic.detections = flight_static_dynamic_no_sync.detections + # flight_static_dynamic.detections = [gt_dynamic[x].copy() for x in flight_static_dynamic.sequence] + flight_static_dynamic.detection_to_global() + gt_dynamic3 = [flight_static_dynamic.detections_global[x] for x in flight_static_dynamic.sequence] + gt_err2, gt_label2, traj_err2, traj_label2, match_err2, match_label2 = reproject_ground_truth(flight_static_dynamic.cameras, gt_static, gt_dynamic3, res_json,ref_cam=flight_static_dynamic.ref_cam, output_dir=output_dir, prefix='static_dynamic_sync_', flight=flight_static_dynamic, ax=[axs1[1],axs2[1],axs3[1]]) + plt.show() + print('\n#################################################################\n') + print("Reconstructions from static-dynamic-no-sync setting") + # flight_static_dynamic_no_sync.detections = [gt_dynamic[x].copy() for x in flight_static_dynamic_no_sync.sequence] + flight_static_dynamic_no_sync.detection_to_global() + gt_dynamic4 = [flight_static_dynamic_no_sync.detections_global[x] for x in flight_static_dynamic_no_sync.sequence] + gt_err3, gt_label3, traj_err3, traj_label3, match_err3, match_label3 = reproject_ground_truth(flight_static_dynamic_no_sync.cameras, gt_static, gt_dynamic4, res_json,ref_cam=flight_static_dynamic_no_sync.ref_cam, output_dir=output_dir, prefix='static_dynamic_unsync_', flight=flight_static_dynamic_no_sync, ax=[axs1[2],axs2[2],axs3[2]]) + + # save result + with open(os.path.join(output_dir, 'result.json'), 'w') as fj: + json.dump(res_json, fj) + print('\n\n#################################################################\n') + print("Visualization") + + print('\tVisualize reprojection box plot') + # plot reprojection errors + # reorder error terms + gt_repro_errs = gt_err1+gt_err3+gt_err2 + gt_repro_labels = gt_label1+gt_label3+gt_label2 + gt_repro_errs = gt_repro_errs[::2] + gt_repro_errs[1:][::2] + gt_repro_labels = gt_repro_labels[::2] + gt_repro_labels[1:][::2] + + match_repro_errs = match_err1+match_err3+match_err2 + match_repro_labels = match_label1+match_label3+match_label2 + match_repro_errs = match_repro_errs[::2] + match_repro_errs[1:][::2] + match_repro_labels = match_repro_labels[::2] + match_repro_labels[1:][::2] + + traj_repro_errs = traj_err1+traj_err3+traj_err2 + traj_repro_labels = traj_label1+traj_label3+traj_label2 + traj_repro_errs = traj_repro_errs[::2] + traj_repro_errs[1:][::2] + traj_repro_labels = traj_repro_labels[::2] + traj_repro_labels[1:][::2] + + vis.error_boxplot(gt_repro_errs, gt_repro_labels, title='reprojection_error_static_ground_truth.png', output_dir=output_dir) + # plt.show() + vis.error_boxplot(traj_repro_errs, traj_repro_labels, title='reprojection_error_dynamic.png', output_dir=output_dir) + # plt.show() + vis.error_boxplot(match_repro_errs, match_repro_labels, title='reprojection_error_static.png', output_dir=output_dir) + # plt.show() + + traj_repro_errs2 = traj_err2+traj_err3 + traj_repro_labels2 = traj_label2+traj_label3 + traj_repro_errs2 = traj_repro_errs2[::2] + traj_repro_errs2[1:][::2] + traj_repro_labels2 = traj_repro_labels2[::2] + traj_repro_labels2[1:][::2] + vis.error_boxplot(traj_repro_errs2, traj_repro_labels2, title='reprojection_error_dynamic2.png', output_dir=output_dir) + # plt.show() + + print('\tVisualize reprojection histogram') + # plot error histograms + width = 1 + fig_gt_hist, gt_hist_axs = plt.subplots(2, 1, sharex=True, sharey=True, dpi=300) + xlim = np.max(np.hstack(gt_repro_errs)) + for i, (cam_err1, cam_label1, cam_err2, cam_label2, cam_err3, cam_label3) in enumerate(zip(gt_err1, gt_label1, gt_err2, gt_label2, gt_err2, gt_label3)): + vis.error_histogram(cam_err1, cam_err2, cam_err3, labels=[cam_label1, cam_label2, cam_label3],ax=gt_hist_axs[i],title='cam '+str(i),xlim=80, num_bins=20, bin_width=width) + xticks = np.arange(0,90,10) + l2 = ["{:.0f}".format(x) for x in xticks] + loc2 = xticks + l2[-1] += '+' + gt_hist_axs[0].set_xticks(loc2) + gt_hist_axs[1].set_xticks(loc2) + gt_hist_axs[1].set_xticklabels(l2) + fig_gt_hist.suptitle('reprojection_error_histogram_static_ground_truth') + fig_gt_hist.savefig(os.path.join(output_dir,'reprojection_error_histogram_static_ground_truth.png')) + # plt.show() + + fig_traj_hist, traj_hist_axs = plt.subplots(2, 1, sharex=True, sharey=True, dpi=300) + xlim = np.max(np.hstack(traj_repro_errs)) + for i, (cam_err1, cam_label1, cam_err2, cam_label2, cam_err3, cam_label3) in enumerate(zip(traj_err1, traj_label1, traj_err2, traj_label2, traj_err3, traj_label3)): + vis.error_histogram(cam_err1, cam_err2, cam_err3, labels=[cam_label1, cam_label2, cam_label3],ax=traj_hist_axs[i],title='cam '+str(i),xlim=80, num_bins=20, bin_width=width) + traj_hist_axs[0].set_xticks(loc2) + traj_hist_axs[1].set_xticks(loc2) + traj_hist_axs[1].set_xticklabels(l2) + fig_traj_hist.suptitle('reprojection_error_histogram_dynamic') + fig_traj_hist.savefig(os.path.join(output_dir,'reprojection_error_histogram_dynamic.png')) + # plt.show() + + fig_match_hist, match_hist_axs = plt.subplots(2, 1, sharex=True, sharey=True, dpi=300) + xlim = np.max(np.hstack(match_repro_errs)) + for i, (cam_err1, cam_label1, cam_err2, cam_label2, cam_err3, cam_label3) in enumerate(zip(match_err1, match_label1, match_err2, match_label2, match_err3, match_label3)): + vis.error_histogram(cam_err1, cam_err2, cam_err3, labels=[cam_label1, cam_label2, cam_label3],ax=match_hist_axs[i],title='cam '+str(i),xlim=80, num_bins=20, bin_width=width) + match_hist_axs[0].set_xticks(loc2) + match_hist_axs[1].set_xticks(loc2) + match_hist_axs[1].set_xticklabels(l2) + fig_match_hist.suptitle('reprojection_error_histogram_static') + fig_match_hist.savefig(os.path.join(output_dir,'reprojection_error_histogram_static.png')) + # plt.show() + + print('Finish!') + +if __name__ == '__main__': + a = argparse.ArgumentParser() + a.add_argument('--results', nargs='+', required=True, help='list of result files') + a.add_argument('--gt_path', type=str, required=True, help='path to ground truth folder') + a.add_argument('--output_dir', type=str, required=True, help='path to output folder') + + opt = a.parse_args() + + # read result files + assert len(opt.results) == 3, 'Wrong number of result files.' + data_file_static, data_file_static_dynamic, data_file_static_dynamic_no_sync = opt.results + + # read gt control points + gt_static_file = sorted(glob(os.path.join(opt.gt_path, '*.txt'))) + assert len(gt_static_file) >= 2, 'Not enough control point files found.' + + # specify output directory + output_dir = opt.output_dir + if not os.path.exists(output_dir): + os.makedirs(output_dir, exist_ok=True) + + main(data_file_static, data_file_static_dynamic, data_file_static_dynamic_no_sync, gt_static_file, output_dir) \ No newline at end of file diff --git a/multiviewunsynch/analysis/compare_gt.py b/multiviewunsynch/analysis/compare_gt.py index a6a24aa..a12c35d 100644 --- a/multiviewunsynch/analysis/compare_gt.py +++ b/multiviewunsynch/analysis/compare_gt.py @@ -150,6 +150,93 @@ def align_gt(flight, f_gt, gt_path, visualize=False): return out +def align_gt_static(flight): + ''' + Function: + find the transformation between the reconstructed static 3d points and the ground truth static points + Input: + flight = the scene object + Output: + gt_static_aligned = the aligned ground truth static points + ''' + # compute the affine transformation between the ground truth static points and the reconstructed ones + M = transformation.affine_matrix_from_points(flight.gt_static[flight.inlier_mask > 0, :].T, flight.static[:, flight.inlier_mask > 0], shear=False, scale=True) + tran = np.dot(M, util.homogeneous(flight.gt_static.T)) + tran /= tran[-1] + + return tran[:-1] + +def align_detections(flight, visualize=False): + + for i, cam in enumerate(flight.cameras): + gt_ori = flight.detections[i] + + if gt_ori.shape[0] == 3 or gt_ori.shape[0] == 4: + pass + elif gt_ori.shape[1] == 3 or gt_ori.shape[1] == 4: + gt_ori = gt_ori.T + else: + raise Exception('Ground truth data have an invalid shape') + + # Pre-processing + alpha = 1 + + reconst = flight.spline_to_traj(sampling_rate=alpha) + t0 = reconst[0,0] + reconst = np.vstack(((reconst[0]-t0)/alpha,reconst[1:])) + if gt_ori.shape[0] == 3: + gt = np.vstack((np.arange(len(gt_ori[0])),gt_ori)) + else: + gt = np.vstack((gt_ori[0]-gt_ori[0,0],gt_ori[1:])) + + # Coarse search + thres = int(reconst[0,-1] / 2) + if int(gt[0,-1]-thres) < 0: + raise Exception('Ground truth too short!') + + error_min = np.inf + for i in range(-thres, int(gt[0,-1]-thres)): + reconst_i = np.vstack((reconst[0]+i,reconst[1:])) + p1, p2 = util.match_overlap(reconst_i, gt) + M = transformation.affine_matrix_from_points(p1[1:], p2[1:], shear=False, scale=True) + + tran = np.dot(M, util.homogeneous(p1[1:])) + tran /= tran[-1] + error_all = np.sqrt((p2[1]-tran[0])**2 + (p2[2]-tran[1])**2 + (p2[3]-tran[2])**2) + error = np.mean(error_all) + if error < error_min: + error_min = error + error_coarse = error_all + j = i + beta = t0-alpha*j + + # Fine optimization + ls, res = optimize(alpha,beta,flight,gt_ori) + + # Remove outliers by relative thresholding + thres = 10 + error_ = res[3] + idx = error_ <= thres*np.mean(error_) + reconst_, gt_, error_ = res[0][:,idx], res[1][:,idx], error_[idx] + + # Result + out = {'align_param':ls.x, 'reconst_tran':reconst_, 'gt':gt_, 'tran_matrix':res[2], 'error':error_} + print('The mean error (distance) is {:.5f} meter\n'.format(np.mean(out['error']))) + print('The median error (distance) is {:.5f} meter\n'.format(np.median(out['error']))) + + print(ls.x) + + if visualize: + # Compare the trajectories + vis.show_trajectory_2D(out['reconst_tran'][1:], out['gt'], line=False, title='Reconstruction(left) vs Ground Truth(right)') + + # Error histogram + vis.error_hist(out['error']) + + # Error over the trajectory + vis.error_traj(out['reconst_tran'][1:], out['error']) + + return out if __name__ == "__main__": diff --git a/multiviewunsynch/analysis/verify_detections.py b/multiviewunsynch/analysis/verify_detections.py index b6fc16b..7fd995b 100644 --- a/multiviewunsynch/analysis/verify_detections.py +++ b/multiviewunsynch/analysis/verify_detections.py @@ -2,34 +2,66 @@ # License, v. 2.0. If a copy of the MPL was not distributed with this # file, You can obtain one at https://mozilla.org/MPL/2.0/. +import re import numpy as np import cv2 +import argparse ''' This script verifies drone detections of a given video by playing the video with the detections in background ''' +a = argparse.ArgumentParser() +a.add_argument("--detection_path", required=True, type=str, help="Path to the detections") +a.add_argument("--video_path", required=True, type=str, help="Path to the video") + +args = a.parse_args() + # Read a video and its corresponding detection -detection_path = '' -video_path = '' +# detection_path = '' +# video_path = '' +detection_path = args.detection_path +video_path = args.video_path # Create a mask for detections in the same size of the video -detection = np.loadtxt(detection_path[1:],usecols=(2,0,1)).T.astype(int) +detection = np.loadtxt(detection_path,usecols=(2,0,1)).T.astype(int) cap = cv2.VideoCapture(video_path) -cap.set(cv2.CAP_PROP_POS_FRAMES,100) +# cap.set(cv2.CAP_PROP_POS_FRAMES,100) frame_0 = cap.read()[1] traj = np.zeros_like(frame_0) for i in range(detection.shape[1]): traj[detection[2,i],detection[1,i],0] = 1 # Plot all detections in each frame +# while cap.isOpened(): +# ret, frame = cap.read() +# if ret: +# # frame = cv2.flip(frame,0) +# frame[traj[:,:,0]==1]=np.array([0,0,255]) # Color of the traj can be specified +# frame = cv2.resize(frame,(1400,570)) +# cv2.imshow('Check Dectections, Press \'q\' to end',frame) + +# if cv2.waitKey(1)==ord('q'): +# break +# cap.release() +# cv2.destroyAllWindows() +frame_id = 0 while cap.isOpened(): ret, frame = cap.read() if ret: # frame = cv2.flip(frame,0) - frame[traj[:,:,0]==1]=np.array([255,0,0]) # Color of the traj can be specified + frame[traj[:,:,0]==1]=np.array([0,0,255]) # Color of the traj can be specified + dets = detection[:,(detection[0] == frame_id) | (detection[0] == frame_id + 500)] + if dets.shape[1] > 0: + for det in dets.T: + if det[0] <= 500: + cv2.circle(frame, (int(det[1]), int(det[2])), radius=5, color=(0,0,255), thickness=-1) + else: + cv2.circle(frame, (int(det[1]), int(det[2])), radius=5, color=(0,255,0), thickness=-1) frame = cv2.resize(frame,(1400,570)) cv2.imshow('Check Dectections, Press \'q\' to end',frame) + # print(frame_id) + frame_id +=1 if cv2.waitKey(1)==ord('q'): break diff --git a/multiviewunsynch/eval.py b/multiviewunsynch/eval.py new file mode 100644 index 0000000..a0a7571 --- /dev/null +++ b/multiviewunsynch/eval.py @@ -0,0 +1,27 @@ +from analysis import analysis_reconstruction as recon +import argparse +from glob import glob +import os + +if __name__ == '__main__': + a = argparse.ArgumentParser() + a.add_argument('--results', nargs='+', required=True, help='list of result files') + a.add_argument('--gt_path', type=str, required=True, help='path to ground truth folder') + a.add_argument('--output_dir', type=str, required=True, help='path to output folder') + + opt = a.parse_args() + + # read result files + assert len(opt.results) == 3, 'Wrong number of result files.' + data_file_static, data_file_static_dynamic, data_file_static_dynamic_no_sync = opt.results + + # read gt control points + gt_static_file = sorted(glob(os.path.join(opt.gt_path, '*.txt'))) + assert len(gt_static_file) >= 2, 'Not enough control point files found.' + + # specify output directory + output_dir = opt.output_dir + if not os.path.exists(output_dir): + os.makedirs(output_dir, exist_ok=True) + + recon.main(data_file_static, data_file_static_dynamic, data_file_static_dynamic_no_sync, gt_static_file, output_dir) \ No newline at end of file diff --git a/multiviewunsynch/main.py b/multiviewunsynch/main.py index 0b5a2e7..561ea75 100644 --- a/multiviewunsynch/main.py +++ b/multiviewunsynch/main.py @@ -9,6 +9,7 @@ from reconstruction import common from analysis.compare_gt import align_gt import sys +import os if len(sys.argv) < 2: print( "Please provide a path to a proper config file") @@ -30,7 +31,7 @@ flight.detection_to_global() # Initialize the first 3D trajectory -flight.init_traj(error=flight.settings['thres_Fmatix']) +flight.init_traj(inlier_only=True, error=flight.settings['thres_Fmatix']) # Convert discrete trajectory to spline representation flight.traj_to_spline(smooth_factor=flight.settings['smooth_factor']) @@ -52,7 +53,7 @@ rs_bounds=flight.settings['rs_bounds']) print('\nMean error of each camera after first BA: ', np.asarray([np.mean(flight.error_cam(x)) for x in flight.sequence[:cam_temp]])) - + flight.remove_outliers(flight.sequence[:cam_temp],thres=flight.settings['thres_outlier']) # Bundle adjustment after outlier removal @@ -62,12 +63,12 @@ rs_bounds=flight.settings['rs_bounds']) print('\nMean error of each camera after second BA: ', np.asarray([np.mean(flight.error_cam(x)) for x in flight.sequence[:cam_temp]])) - + num_end = flight.numCam if flight.find_order else len(flight.sequence) if cam_temp == num_end: print('\nTotal time: {}\n\n\n'.format(datetime.now()-start)) break - + # Select the next camera if not pre-defined flight.select_most_overlap() @@ -84,10 +85,17 @@ flight.spline_to_traj(sampling_rate=1) # Visualize the 3D trajectory -#vis.show_trajectory_3D(flight.traj[1:],line=False) +vis.show_trajectory_3D(flight.traj[1:],line=False) +# visualize the 2d trajectories +for i, cam in enumerate(flight.cameras): + x_res = cam.dist_point3d(flight.traj[1:]) + x_ori = flight.detections[i][1:] + # visualize the reprojection of the reconstructed trajectories + vis.show_2D_all(x_ori, x_res, title='cam'+str(i)+' trajectories', color=True, line=False, bg=cam.img, label=['extracted dynamic features', 'reconstructed trajectories']) # Align with the ground truth data if available -flight.out = align_gt(flight, flight.gt['frequency'], flight.gt['filepath'], visualize=False) +if len(flight.gt) > 0: + flight.out = align_gt(flight, flight.gt['frequency'], flight.gt['filepath'], visualize=False) with open(flight.settings['path_output'],'wb') as f: pickle.dump(flight, f) diff --git a/multiviewunsynch/main_static.py b/multiviewunsynch/main_static.py new file mode 100644 index 0000000..77f0176 --- /dev/null +++ b/multiviewunsynch/main_static.py @@ -0,0 +1,126 @@ +# This Source Code Form is subject to the terms of the Mozilla Public +# License, v. 2.0. If a copy of the MPL was not distributed with this +# file, You can obtain one at https://mozilla.org/MPL/2.0/. + +import numpy as np +import pickle +from tools import visualization as vis +from datetime import datetime +from reconstruction import common +from analysis.compare_gt import align_gt, align_gt_static +import sys +from tools.util import unpack_sift_kp + +import cv2 +from reconstruction import epipolar as ep +import argparse +import os + +# parse the input +a = argparse.ArgumentParser() +a.add_argument("--config_file", type=str, help="path to the proper config file", required=True) +a.add_argument("--debug", action="store_true", help="debug mode: run with ground truth)") +a.add_argument("--scale", action="store_true", help="scale variable in BA") +args = a.parse_args() + +print('Reconstruct with only static part of the scene.\n') + +if args.debug: + print("RUN ON DEBUG MODE WITH GROUND TRUTH STATIC POINTS") + +# Initialize a scene from the json template +flight = common.create_scene(args.config_file) + +# Initialize the static part +flight.init_static(inlier_only=True, debug=args.debug) + +'''---------------Incremental reconstruction----------------''' +start = datetime.now() +np.set_printoptions(precision=4) + +cam_temp = 2 +while True: + # print('\nRemove outliers far away from the center') + # flight.remove_outliers_3d(flight.sequence[:cam_temp], mode='camera', verbose=True, debug=args.debug) + + print('\n----------------- Bundle Adjustment with {} cameras -----------------'.format(cam_temp)) + print('\nMean error of the static part in each camera before BA: ', np.asarray([np.mean(flight.error_cam_static(x, debug=args.debug)) for x in flight.sequence[:cam_temp]])) + + print('\nDoing the first BA') + # Bundle adjustment + res = flight.BA_static(cam_temp, debug=args.debug, scaling=args.scale) + + print('\nMean error of the static part in each camera after the first BA: ', np.asarray([np.mean(flight.error_cam_static(x, debug=args.debug)) for x in flight.sequence[:cam_temp]])) + + # remove outliers + print('\nRemove outliers after first BA') + flight.remove_outliers_static(flight.sequence[:cam_temp], thres=flight.settings['thres_outlier'], verbose=True, debug=args.debug) + + # print('\nRemove outliers far away from the center') + # flight.remove_outliers_3d(flight.sequence[:cam_temp], mode='camera', verbose=True, debug=args.debug) + + print('\nDoing the second BA') + # Bundle adjustment after outlier removal + res = flight.BA_static(cam_temp, debug=args.debug, scaling=args.scale) + + print('\nMean error of the static part in each camera after the second BA: ', np.asarray([np.mean(flight.error_cam_static(x, debug=args.debug)) for x in flight.sequence[:cam_temp]])) + + print('\nRemove outliers far away from the center') + flight.remove_outliers_3d(flight.sequence[:cam_temp], mode='camera', verbose=True, debug=args.debug) + + num_end = flight.numCam if flight.find_order else len(flight.sequence) + if cam_temp == num_end: + print('\nTotal time: {}\n\n\n'.format(datetime.now()-start)) + break + + # find the next camera to be added if not the order is not specified + flight.select_next_camera_static(debug=args.debug) + + # Add the next camera and get its pose + flight.get_camera_pose_static(flight.sequence[cam_temp], flight.sequence[:cam_temp], debug=args.debug) + + # Triangulate new points and update the static scene + flight.triangulate_static(flight.sequence[cam_temp], flight.sequence[:cam_temp], thres=flight.settings['thres_triangulation']) + + print('\nTotal time: {}\n\n\n'.format(datetime.now()-start)) + cam_temp += 1 + +# Align with the ground truth static points if available +if flight.gt_static is not None: + # Transform the ground truth static 3d points + static_ref = align_gt_static(flight) + # Visualize the reconstructed 3D static points and the ground truth static points + vis.show_3D_all(static_ref, flight.static[:, flight.inlier_mask > 0], color=True, line=False, flight=flight) + for i, cam in enumerate(flight.cameras): + # x_res = cam.projectPoint(flight.static[:, cam.index_2d_3d])[:-1] + x_res = cam.dist_point3d(flight.static[:, cam.index_2d_3d]) + x_ori = cam.get_gt_pts() + vis.show_2D_all(x_ori, x_res, title='cam'+str(i), color=True, line=False, bg=cam.img, label=['ground truth features', 'reconstructed ground truth features']) +else: + # Visualize the 3D static points + vis.show_3D_all(flight.static[:, flight.inlier_mask > 0], color=False, line=False, flight=flight) + # no ground truth exists, plot the reprojection in 2d + for i, cam in enumerate(flight.cameras): + x_res = cam.dist_point3d(flight.static[:, cam.index_2d_3d]) + # x_res = cam.projectPoints(flight.static[:, cam.index_2d_3d])[:-1] + if args.debug: + x_ori = cam.get_gt_pts() + else: + x_ori = cam.get_points() + vis.show_2D_all(x_ori, x_res, title='cam'+str(i), color=True, line=False, bg=cam.img, label=['extracted static features', 'reconstructed static features']) + +# Align with the ground truth data if available +if len(flight.gt) > 0: + flight.out = align_gt(flight, flight.gt['frequency'], flight.gt['filepath'], visualize=False) + +if not os.path.exists(os.path.dirname(flight.settings['path_output'])): + os.makedirs(os.path.dirname(flight.settings['path_output']), exist_ok=True) + +with open(flight.settings['path_output'],'wb') as f: + # unpack sift features if used + if flight.settings['include_static']: + for cam in flight.cameras: + cam.kp = unpack_sift_kp(cam.kp) + pickle.dump(flight, f) + +print('Finished!') \ No newline at end of file diff --git a/multiviewunsynch/main_static_dynamic.py b/multiviewunsynch/main_static_dynamic.py new file mode 100644 index 0000000..cc45ae6 --- /dev/null +++ b/multiviewunsynch/main_static_dynamic.py @@ -0,0 +1,165 @@ +# This Source Code Form is subject to the terms of the Mozilla Public +# License, v. 2.0. If a copy of the MPL was not distributed with this +# file, You can obtain one at https://mozilla.org/MPL/2.0/. + +import numpy as np +import pickle + +from numpy.linalg.linalg import det +from tools import visualization as vis +from datetime import datetime +from reconstruction import common +from analysis.compare_gt import align_gt, align_gt_static, align_detections +import sys +from tools.util import unpack_sift_kp + +import cv2 +from reconstruction import epipolar as ep +import argparse +import os + +# parse the input +a = argparse.ArgumentParser() +a.add_argument("--config_file", type=str, help="path to the proper config file", required=True) +a.add_argument("--debug", action="store_true", help="debug mode: run with ground truth)") +a.add_argument("--scale", action="store_true", help="scale variable in BA") + +args = a.parse_args() + +print('Reconstruct with both static part and dynamic part of the scene.\n') + +if args.debug: + print("RUN ON DEBUG MODE WITH GROUND TRUTH STATIC POINTS") + +# Initialize a scene from the json template +flight = common.create_scene(args.config_file) + +# Truncate detections +flight.cut_detection(second=flight.settings['cut_detection_second']) + +# Add prior alpha +flight.init_alpha() + +# Compute time shift for each camera +flight.time_shift() + +# Convert raw detections into the global timeline +flight.detection_to_global() + +# Initialize the first 3D trajectory +flight.init_traj(error=flight.settings['thres_Fmatix'], inlier_only=True, debug=args.debug) + +# Convert discrete trajectory to spline representation +flight.traj_to_spline(smooth_factor=flight.settings['smooth_factor']) + +'''---------------Incremental reconstruction----------------''' +start = datetime.now() +np.set_printoptions(precision=4) + +cam_temp = 2 +while True: + # print('\nRemove outliers far away from the center') + # flight.remove_outliers_3d(flight.sequence[:cam_temp], verbose=True, debug=args.debug) + + print('\n----------------- Bundle Adjustment with {} cameras -----------------'.format(cam_temp)) + print('\nMean error of each camera before BA: ', np.asarray([np.mean(flight.error_cam(x)) for x in flight.sequence[:cam_temp]])) + print('\nMean error of the static part in each camera before BA: ', np.asarray([np.mean(flight.error_cam_static(x, debug=args.debug)) for x in flight.sequence[:cam_temp]])) + + print('\nDoing the first BA') + # Bundle adjustment + # res = flight.BA_static(cam_temp, debug=args.debug) + res = flight.BA(cam_temp, rs=flight.settings['rolling_shutter'],\ + motion_reg=flight.settings['motion_reg'],\ + motion_weights=flight.settings['motion_weights'],\ + rs_bounds=flight.settings['rs_bounds'],debug=args.debug,scaling=args.scale) + + print('\nMean error of each camera after the first BA: ', np.asarray([np.mean(flight.error_cam(x)) for x in flight.sequence[:cam_temp]])) + print('\nMean error of the static part in each camera after the first BA: ', np.asarray([np.mean(flight.error_cam_static(x, debug=args.debug)) for x in flight.sequence[:cam_temp]])) + + # remove outliers + print('\nRemove outliers after first BA') + # flight.remove_outliers_static(flight.sequence[:cam_temp], thres=flight.settings['thres_outlier'], verbose=True, debug=args.debug) + flight.remove_outliers(flight.sequence[:cam_temp], thres=flight.settings['thres_outlier'], verbose=True, debug=args.debug) + + print('\nRemove outliers far away from the center') + flight.remove_outliers_3d(flight.sequence[:cam_temp], verbose=True, debug=args.debug) + + print('\nDoing the second BA') + # Bundle adjustment after outlier removal + # res = flight.BA_static(cam_temp, debug=args.debug) + res = flight.BA(cam_temp, rs=flight.settings['rolling_shutter'],\ + motion_reg=flight.settings['motion_reg'],\ + motion_weights=flight.settings['motion_weights'],\ + rs_bounds=flight.settings['rs_bounds'],debug=args.debug, scaling=args.scale) + + print('\nMean error of each camera after the second BA: ', np.asarray([np.mean(flight.error_cam(x)) for x in flight.sequence[:cam_temp]])) + print('\nMean error of the static part in each camera after the second BA: ', np.asarray([np.mean(flight.error_cam_static(x, debug=args.debug)) for x in flight.sequence[:cam_temp]])) + + print('\nRemove outliers far away from the center') + flight.remove_outliers_3d(flight.sequence[:cam_temp], thres=flight.settings['thres_outlier'], mode='camera', verbose=True, debug=args.debug) + + num_end = flight.numCam if flight.find_order else len(flight.sequence) + if cam_temp == num_end: + print('\nTotal time: {}\n\n\n'.format(datetime.now()-start)) + break + + # Select the next camera if not pre-defined + flight.select_most_overlap() + + # Add the next camera and get its pose + flight.get_camera_pose(flight.sequence[cam_temp], debug=args.debug) + + # Triangulate new points and update the 3D spline + flight.triangulate(flight.sequence[cam_temp], flight.sequence[:cam_temp], thres=flight.settings['thres_triangulation'], factor_t2s=flight.settings['smooth_factor'], factor_s2t=flight.settings['sampling_rate']) + # Triangulate new points and update the static scene + flight.triangulate_static(flight.sequence[cam_temp], flight.sequence[:cam_temp], thres=flight.settings['thres_triangulation']) + + print('\nTotal time: {}\n\n\n'.format(datetime.now()-start)) + cam_temp += 1 + flight.traj_len = [] + +# Discretize trajectory +flight.spline_to_traj(sampling_rate=1) + +# Visualize the 3D trajectory +vis.show_trajectory_3D(flight.traj[1:],line=False) + +# Align with the ground truth static points if available +if flight.gt_static is not None: + # Transform the ground truth static 3d points + static_ref = align_gt_static(flight) + # Visualize the reconstructed 3D static points and the ground truth static points + vis.show_3D_all(static_ref, flight.static[:, flight.inlier_mask > 0], color=True, line=False, flight=flight) + for i, cam in enumerate(flight.cameras): + # x_res = cam.projectPoint(flight.static[:, cam.index_2d_3d])[:-1] + x_res = cam.dist_point3d(flight.static[:, cam.index_2d_3d]) + x_ori = cam.get_gt_pts() + x_res_traj = cam.dist_point3d(flight.traj[1:]) + vis.show_2D_all(x_ori, x_res, flight.detections[i][1:], x_res_traj, title='cam'+str(i), color=True, line=False, bg=cam.img, label=['ground truth features', 'reconstructed ground truth features', 'extracted dynamic features', 'reconstructed trajectories']) +else: + # Visualize the 3D static points + vis.show_3D_all(flight.static[:, flight.inlier_mask > 0], color=False, line=False, flight=flight) + # no ground truth exists, plot the reprojection in 2d + for i, cam in enumerate(flight.cameras): + x_res = cam.dist_point3d(flight.static[:, cam.index_2d_3d]) + x_res_traj = cam.dist_point3d(flight.traj[1:]) + # x_res = cam.projectPoints(flight.static[:, cam.index_2d_3d])[:-1] + if args.debug: + x_ori = cam.get_gt_pts() + else: + x_ori = cam.get_points() + vis.show_2D_all(x_ori, x_res, flight.detections[i][1:], x_res_traj, title='cam'+str(i), color=True, line=False, bg=cam.img, label=['extracted static features', 'reconstructed static features', 'extracted dynamic features', 'reconstructed trajectories']) + +# Align with the ground truth data if available +if len(flight.gt) > 0: + flight.out = align_gt(flight, flight.gt['frequency'], flight.gt['filepath'], visualize=False) +if not os.path.exists(os.path.dirname(flight.settings['path_output'])): + os.makedirs(os.path.dirname(flight.settings['path_output']), exist_ok=True) +with open(flight.settings['path_output'],'wb') as f: + # unpack sift features if used + if flight.settings['include_static']: + for cam in flight.cameras: + cam.kp = unpack_sift_kp(cam.kp) + pickle.dump(flight, f) + +print('Finished!') \ No newline at end of file diff --git a/multiviewunsynch/main_static_then_dynamic.py b/multiviewunsynch/main_static_then_dynamic.py new file mode 100644 index 0000000..8b37515 --- /dev/null +++ b/multiviewunsynch/main_static_then_dynamic.py @@ -0,0 +1,204 @@ +# This Source Code Form is subject to the terms of the Mozilla Public +# License, v. 2.0. If a copy of the MPL was not distributed with this +# file, You can obtain one at https://mozilla.org/MPL/2.0/. + +import numpy as np +import pickle +from tools import visualization as vis +from datetime import datetime +from reconstruction import common +from analysis.compare_gt import align_gt, align_gt_static +import sys +from tools.util import unpack_sift_kp + +import cv2 +from reconstruction import epipolar as ep +import argparse +import os + +# parse the input +a = argparse.ArgumentParser() +a.add_argument("--config_file", type=str, help="path to the proper config file", required=True) +a.add_argument("--debug", action="store_true", help="debug mode: run with ground truth)") + +args = a.parse_args() + +print('Reconstruct with only static part of the scene.\n') + +if args.debug: + print("RUN ON DEBUG MODE WITH GROUND TRUTH STATIC POINTS") + +# Initialize a scene from the json template +flight = common.create_scene(args.config_file) + +# Initialize the static part +flight.init_static(inlier_only=True, debug=args.debug) + +# '''---------------Incremental reconstruction----------------''' +# start = datetime.now() +# np.set_printoptions(precision=4) + +# cam_temp = 2 +# while True: +# print('\n----------------- Bundle Adjustment with {} cameras -----------------'.format(cam_temp)) +# print('\nMean error of the static part in each camera before BA: ', np.asarray([np.mean(flight.error_cam_static(x, debug=args.debug)) for x in flight.sequence[:cam_temp]])) + +# print('\nDoing the first BA') +# # Bundle adjustment +# res = flight.BA_static(cam_temp, debug=args.debug) + +# print('\nMean error of the static part in each camera after the first BA: ', np.asarray([np.mean(flight.error_cam_static(x, debug=args.debug)) for x in flight.sequence[:cam_temp]])) + +# # remove outliers +# print('\nRemove outliers after first BA') +# flight.remove_outliers_static(flight.sequence[:cam_temp], thres=flight.settings['thres_outlier'], verbose=True, debug=args.debug) + +# print('\nDoing the second BA') +# # Bundle adjustment after outlier removal +# res = flight.BA_static(cam_temp, debug=args.debug) + +# print('\nMean error of the static part in each camera after the second BA: ', np.asarray([np.mean(flight.error_cam_static(x, debug=args.debug)) for x in flight.sequence[:cam_temp]])) + +# num_end = flight.numCam if flight.find_order else len(flight.sequence) +# if cam_temp == num_end: +# print('\nTotal time: {}\n\n\n'.format(datetime.now()-start)) +# break + +# # Add the next camera and get its pose +# flight.get_camera_pose_static(flight.sequence[cam_temp], flight.sequence[:cam_temp], debug=args.debug) + +# # Triangulate new points and update the static scene +# flight.triangulate_static(flight.sequence[cam_temp], flight.sequence[:cam_temp], thres=flight.settings['thres_triangulation']) + +# print('\nTotal time: {}\n\n\n'.format(datetime.now()-start)) +# cam_temp += 1 + +# # Align with the ground truth static points if available +# if flight.gt_static is not None: +# # Transform the ground truth static 3d points +# static_ref = align_gt_static(flight) +# # Visualize the reconstructed 3D static points and the ground truth static points +# vis.show_3D_all(static_ref, flight.static[:, flight.inlier_mask > 0], color=True, line=False, flight=flight) +# for i, cam in enumerate(flight.cameras): +# # x_res = cam.projectPoint(flight.static[:, cam.index_2d_3d])[:-1] +# x_res = cam.dist_point3d(flight.static[:, cam.index_2d_3d]) +# x_ori = cam.get_gt_pts() +# vis.show_2D_all(x_ori, x_res, title='cam'+str(i), color=True, line=False, bg=cam.img) +# else: +# # Visualize the 3D static points +# vis.show_3D_all(flight.static[:, flight.inlier_mask > 0], color=False, line=False, flight=flight) +# # no ground truth exists, plot the reprojection in 2d +# for i, cam in enumerate(flight.cameras): +# x_res = cam.dist_point3d(flight.static[:, cam.index_2d_3d]) +# # x_res = cam.projectPoints(flight.static[:, cam.index_2d_3d])[:-1] +# if args.debug: +# x_ori = cam.get_gt_pts() +# else: +# x_ori = cam.get_points() +# vis.show_2D_all(x_ori, x_res, title='cam'+str(i), color=True, line=False, bg=cam.img) + +print("####################### FINISH INITIALIZATION WITH THE STATIC PART #######################") +# initialize dynamic part +print("Initialize the dynamic part with the poses") +flight.init_traj_from_pose() + +# Convert discrete trajectory to spline representation +flight.traj_to_spline(smooth_factor=flight.settings['smooth_factor']) + +'''---------------Incremental reconstruction----------------''' +start = datetime.now() +np.set_printoptions(precision=4) + +cam_temp = 2 +while True: + print('\n----------------- Bundle Adjustment with {} cameras -----------------'.format(cam_temp)) + print('\nMean error of each camera before BA: ', np.asarray([np.mean(flight.error_cam(x)) for x in flight.sequence[:cam_temp]])) + print('\nMean error of the static part in each camera before BA: ', np.asarray([np.mean(flight.error_cam_static(x, debug=args.debug)) for x in flight.sequence[:cam_temp]])) + + print('\nDoing the first BA') + # Bundle adjustment + # res = flight.BA_static(cam_temp, debug=args.debug) + res = flight.BA(cam_temp, rs=flight.settings['rolling_shutter'],\ + motion_reg=flight.settings['motion_reg'],\ + motion_weights=flight.settings['motion_weights'],\ + rs_bounds=flight.settings['rs_bounds'],debug=args.debug) + + print('\nMean error of each camera after the first BA: ', np.asarray([np.mean(flight.error_cam(x)) for x in flight.sequence[:cam_temp]])) + print('\nMean error of the static part in each camera after the first BA: ', np.asarray([np.mean(flight.error_cam_static(x, debug=args.debug)) for x in flight.sequence[:cam_temp]])) + + # remove outliers + print('\nRemove outliers after first BA') + # flight.remove_outliers_static(flight.sequence[:cam_temp], thres=flight.settings['thres_outlier'], verbose=True, debug=args.debug) + flight.remove_outliers(flight.sequence[:cam_temp], thres=flight.settings['thres_outlier'], verbose=True, debug=args.debug) + + print('\nDoing the second BA') + # Bundle adjustment after outlier removal + # res = flight.BA_static(cam_temp, debug=args.debug) + res = flight.BA(cam_temp, rs=flight.settings['rolling_shutter'],\ + motion_reg=flight.settings['motion_reg'],\ + motion_weights=flight.settings['motion_weights'],\ + rs_bounds=flight.settings['rs_bounds'],debug=args.debug) + + print('\nMean error of each camera after the second BA: ', np.asarray([np.mean(flight.error_cam(x)) for x in flight.sequence[:cam_temp]])) + print('\nMean error of the static part in each camera after the second BA: ', np.asarray([np.mean(flight.error_cam_static(x, debug=args.debug)) for x in flight.sequence[:cam_temp]])) + + num_end = flight.numCam if flight.find_order else len(flight.sequence) + if cam_temp == num_end: + print('\nTotal time: {}\n\n\n'.format(datetime.now()-start)) + break + + # Add the next camera and get its pose + flight.get_camera_pose(flight.sequence[cam_temp], debug=args.debug) + + # Triangulate new points and update the 3D spline + flight.triangulate(flight.sequence[cam_temp], flight.sequence[:cam_temp], thres=flight.settings['thres_triangulation'], factor_t2s=flight.settings['smooth_factor'], factor_s2t=flight.settings['sampling_rate']) + # Triangulate new points and update the static scene + flight.triangulate_static(flight.sequence[cam_temp], flight.sequence[:cam_temp], thres=flight.settings['thres_triangulation']) + + print('\nTotal time: {}\n\n\n'.format(datetime.now()-start)) + cam_temp += 1 + flight.traj_len = [] + +# Discretize trajectory +flight.spline_to_traj(sampling_rate=1) + +# Visualize the 3D trajectory +vis.show_trajectory_3D(flight.traj[1:],line=False) + +# Align with the ground truth static points if available +if flight.gt_static is not None: + # Transform the ground truth static 3d points + static_ref = align_gt_static(flight) + # Visualize the reconstructed 3D static points and the ground truth static points + vis.show_3D_all(static_ref, flight.static[:, flight.inlier_mask > 0], color=True, line=False, flight=flight) + for i, cam in enumerate(flight.cameras): + # x_res = cam.projectPoint(flight.static[:, cam.index_2d_3d])[:-1] + x_res = cam.dist_point3d(flight.static[:, cam.index_2d_3d]) + x_ori = cam.get_gt_pts() + x_res_traj = cam.dist_point3d(flight.traj[1:]) + vis.show_2D_all(x_ori, x_res, flight.detections[i][1:], x_res_traj, title='cam'+str(i), color=True, line=False, bg=cam.img) +else: + # Visualize the 3D static points + vis.show_3D_all(flight.static[:, flight.inlier_mask > 0], color=False, line=False, flight=flight) + # no ground truth exists, plot the reprojection in 2d + for i, cam in enumerate(flight.cameras): + x_res = cam.dist_point3d(flight.static[:, cam.index_2d_3d]) + x_res_traj = cam.dist_point3d(flight.traj[1:]) + # x_res = cam.projectPoints(flight.static[:, cam.index_2d_3d])[:-1] + if args.debug: + x_ori = cam.get_gt_pts() + else: + x_ori = cam.get_points() + vis.show_2D_all(x_ori, x_res, flight.detections[i][1:], x_res_traj, title='cam'+str(i), color=True, line=False, bg=cam.img) + +# Align with the ground truth data if available +if len(flight.gt) > 0: + flight.out = align_gt(flight, flight.gt['frequency'], flight.gt['filepath'], visualize=False) +with open(flight.settings['path_output'],'wb') as f: + # unpack sift features if used + if flight.settings['include_static']: + for cam in flight.cameras: + cam.kp = unpack_sift_kp(cam.kp) + pickle.dump(flight, f) + +print('Finished!') \ No newline at end of file diff --git a/multiviewunsynch/plot_detections.py b/multiviewunsynch/plot_detections.py new file mode 100644 index 0000000..d8d1882 --- /dev/null +++ b/multiviewunsynch/plot_detections.py @@ -0,0 +1,70 @@ +import numpy as np +import pickle +from datetime import datetime +from reconstruction import synchronization as sync + +from tools.util import match_overlap, homogeneous +from matplotlib import pyplot as plt +import os +import cv2 +import argparse +from glob import glob +import json + +def plot_detections(img, raw_dets, used_dets, figname): + # plt.figure(figsize=(12, 10)) + plt.figure() + if img is not None: + img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB) + plt.imshow(img) + h,w,_ = img.shape + plt.xlim([0,w]) + plt.ylim([h,0]) + + # plot raw detections + plt.scatter(raw_dets[0], raw_dets[1], c='yellow', marker='o', s=50) + plt.scatter(used_dets[0], used_dets[1], c='orange', marker='o', s=50) + + plt.axis('off') + plt.savefig(figname,bbox_inches='tight',pad_inches = 0) + +def main(): + # detection path + det_path = ['/scratch2/wuti/Repos/mvus/experiments/webcam-datasets/nyc_set17/det_opencv/cam0_set17.txt', '/scratch2/wuti/Repos/mvus/experiments/webcam-datasets/nyc_set17/det_opencv/cam2_set17.txt'] + + # output dir + output_dir = '/scratch2/wuti/Repos/mvus/experiments/webcam-datasets/nyc_set17/teaser' + if not os.path.exists(output_dir): + os.makedirs(output_dir) + + # result path + result_path = '/scratch2/wuti/Repos/mvus/experiments/webcam-datasets/nyc_set17/static_dynamic/nyc_static_dynamic_superglue_10.pkl' + + # load result + with open(result_path, 'rb') as file: + flight = pickle.load(file) + + raw_dets = [flight.detections[x] for x in flight.sequence] + flight.settings['undist_points'] = False + flight.detection_to_global() + sync_dets = [flight.detections_global[x] for x in flight.sequence] + + timestamp = raw_dets[flight.ref_cam][0] + used_dets = [] + + for sync_det in sync_dets: + # sample to the ref camera + _, used_det = match_overlap(raw_dets[flight.ref_cam], sync_det) + used_dets.append(used_det) + timestamp = np.intersect1d(timestamp, used_det[0]) + + used_dets = list(map(lambda x: x[1:,np.isin(x[0], timestamp)], used_dets)) + + for i, (cam, raw_det, used_det) in enumerate(zip(flight.cameras, raw_dets, used_dets)): + print(f'Plot the trajectory of camera {i}...') + # print(used_det.shape) + plot_detections(cam.img, raw_det[1:], used_det, os.path.join(output_dir, f'traj_cam{i}.png')) + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/multiviewunsynch/reconstruction/common.py b/multiviewunsynch/reconstruction/common.py index 301397c..40d25bd 100644 --- a/multiviewunsynch/reconstruction/common.py +++ b/multiviewunsynch/reconstruction/common.py @@ -4,6 +4,7 @@ # Classes that are common to the entire project import numpy as np +from scipy.optimize._lsq.least_squares import FROM_MINPACK_TO_COMMON from tools import util import cv2 import json @@ -12,11 +13,12 @@ from datetime import datetime from scipy.optimize import least_squares from scipy import interpolate -from scipy.sparse import lil_matrix, vstack +from scipy.sparse import lil_matrix, vstack, hstack from matplotlib import pyplot as plt from mpl_toolkits.mplot3d import Axes3D from tools import visualization as vis from tools import util +import pickle class Scene: @@ -59,7 +61,26 @@ def __init__(self): self.rs = [] self.ref_cam = 0 self.find_order = True - + + # save data regarding static part + # the reconstructed 3D points for the static scene + self.static = np.empty([3, 0]) + # the inlier mask of the reconstructed 3D static points (to keep a track of the inliers ) + self.inlier_mask = np.empty([0]) + # the ground truth static 3d points + self.gt_static = None + # the dictionary stores the matching result + self.feature_dict = {} + # feature_dict[cam_id] = np.array((n_cam, n_kp)) feature_dict[cam1][cam2]: values=indices of matched features in cam2, col=indices of matched features in cam1 + # e.g. feature_dict[0] = [[-1,-1,-1,-1,-1,-1], + # [-1,-1, 2, 1,-1, 3], + # [ 3,-1,-1,-1, 0, -1]] + # feature_dict[1] = [[-1, 3, 2, 5,-1,-1], + # [-1,-1,-1,-1,-1,-1], + # [ 1,-1, 4, 0, 2, 3]] + # feature_dict[2] = [[ 4,-1,-1, 0,-1,-1], + # [ 3, 0, 4, 5, 2,-1], + # [-1,-1,-1,-1,-1,-1]] def addCamera(self,*camera): """ @@ -123,7 +144,8 @@ def detection_to_global(self,*cam,motion_prior=False): for i in cams: timestamp = self.alpha[i] * (self.detections[i][0] + self.rs[i] * self.detections[i][2] / self.cameras[i].resolution[1]) + self.beta[i] - detect = self.cameras[i].undist_point(self.detections[i][1:]) if self.settings['undist_points'] else self.detections[i][1:] + method = self.settings['undist_method'] if 'unidst_method' in self.settings.keys() else 'opencv' + detect = self.cameras[i].undist_point(self.detections[i][1:], method) if self.settings['undist_points'] else self.detections[i][1:] self.detections_global[i] = np.vstack((timestamp, detect)) if motion_prior: @@ -175,9 +197,13 @@ def cut_detection(self,second=1): self.detections[i], _ = util.sampling(detect,interval_long) - def init_traj(self,error=10,inlier_only=False): + def init_traj(self,error=10,inlier_only=False, debug=False): ''' - Select the first two cams in the sequence, compute fundamental matrix, triangulate points + Function: + Select the first two cams in the sequence, compute fundamental matrix, triangulate points + Input: + debug = True -- use the ground truth matches for the static part; + False -- use the extracted features ''' self.select_most_overlap(init=True) @@ -191,35 +217,393 @@ def init_traj(self,error=10,inlier_only=False): else: d2, d1 = util.match_overlap(self.detections_global[t2], self.detections_global[t1]) - # Compute fundamental matrix - F,inlier = ep.computeFundamentalMat(d1[1:],d2[1:],error=error) - E = np.dot(np.dot(K2.T,F),K1) - - if not inlier_only: - inlier = np.ones(len(inlier)) - x1, x2 = util.homogeneous(d1[1:,inlier==1]), util.homogeneous(d2[1:,inlier==1]) - - # Find corrected corresponding points for optimal triangulation - N = d1[1:,inlier==1].shape[1] - pts1=d1[1:,inlier==1].T.reshape(1,-1,2) - pts2=d2[1:,inlier==1].T.reshape(1,-1,2) - m1,m2 = cv2.correctMatches(F,pts1,pts2) - x1,x2 = util.homogeneous(np.reshape(m1,(-1,2)).T), util.homogeneous(np.reshape(m2,(-1,2)).T) - - mask = np.logical_not(np.isnan(x1[0])) - x1 = x1[:,mask] - x2 = x2[:,mask] - - # Triangulte points - X, P = ep.triangulate_from_E(E,K1,K2,x1,x2) - self.traj = np.vstack((d1[0][inlier==1][mask],X[:-1])) + # draw matches between dections + if self.settings['undist_points']: + # the background images are the original ones and are not undistorted, the detections need to be distorted + d1_dist = self.cameras[t1].dist_point2d(d1[1:], method=self.settings['undist_method']) + d2_dist = self.cameras[t2].dist_point2d(d2[1:], method=self.settings['undist_method']) + + vis.draw_detection_matches(self.cameras[t1].img, np.vstack([d1[0], d1_dist]), self.cameras[t2].img, np.vstack([d2[0],d2_dist])) + else: + vis.draw_detection_matches(self.cameras[t1].img, d1, self.cameras[t2].img, d2) + + # add the static part + if 'include_static' in self.settings.keys() and self.settings['include_static']: + # if feature_dict is empty + if not self.feature_dict: + if debug: + # in debug, use static ground truth as 2d featues + pts1 = self.cameras[t1].gt_pts.T + pts2 = self.cameras[t2].gt_pts.T + + vis.draw_detection_matches(self.cameras[t1].img, np.vstack([np.zeros(pts1.shape[1]),pts1]), self.cameras[t2].img, np.vstack([np.zeros(pts2.shape[1]),pts2])) + + # save the matching result, the indices of the ground truth matches are shared across all cameras + query_ids = np.arange(self.cameras[t1].gt_pts.shape[0]) + train_ids = np.arange(self.cameras[t2].gt_pts.shape[0]) + + # initialize the matrix for storing matching result + match_res1 = -np.ones((self.numCam, self.cameras[t1].gt_pts.shape[0])) + match_res2 = -np.ones((self.numCam, self.cameras[t2].gt_pts.shape[0])) + + else: + # Match features with sift + if self.settings['feature_method'][0] == 'sift': + pts1, pts2, matches, matchesMask = ep.matching_feature(self.cameras[t1].kp, self.cameras[t2].kp, self.cameras[t1].des, self.cameras[t2].des, ratio=0.8) + # draw matches + vis.draw_matches(self.cameras[t1].img, self.cameras[t1].kp, self.cameras[t2].img, self.cameras[t2].kp, matches, matchesMask) + # get the valid matches + match_ids = np.where(np.array(matchesMask)[:, 0] == 1)[0] + + # get the valid matches + query_ids = np.array([m[0].queryIdx for m in matches]) + train_ids = np.array([m[0].trainIdx for m in matches]) + query_ids = query_ids[match_ids] + train_ids = train_ids[match_ids] + + # initialize the matrix for storing matching result + match_res1 = -np.ones((self.numCam, len(self.cameras[t1].kp))) + match_res2 = -np.ones((self.numCam, len(self.cameras[t2].kp))) + + pts1, pts2 = np.array(pts1).T, np.array(pts2).T + else: + raise Exception("Superglue matches are not properly loaded.") + + # save the matching result to feature_dict + match_res1[t2,query_ids] = train_ids + match_res2[t1,train_ids] = query_ids + self.feature_dict[t1] = match_res1 + self.feature_dict[t2] = match_res2 + + # if feature_dict is already specified, get the corresponding pts + else: + match_res = self.feature_dict[t1][t2] + query_ids = np.where(match_res > -1)[0] + train_ids = match_res[match_res > -1].astype(int) + + pts1 = self.cameras[t1].get_points(indices=query_ids) + pts2 = self.cameras[t2].get_points(indices=train_ids) + + # draw matches + vis.draw_detection_matches(self.cameras[t1].img, np.vstack([np.zeros(pts1.shape[1]),pts1]), self.cameras[t2].img, np.vstack([np.zeros(pts2.shape[1]),pts2])) + + + # undistort the matched keypoints + if self.settings['undist_points']: + pts1 = self.cameras[t1].undist_point(pts1, self.settings['undist_method']) + pts2 = self.cameras[t2].undist_point(pts2, self.settings['undist_method']) + + # stack the static features with the detections and use them together for initial pose extraction + fp1 = np.hstack([np.int32(pts1), d1[1:]]) + fp2 = np.hstack([np.int32(pts2), d2[1:]]) + + X, P, inlier, mask = ep.epipolar_pipeline(fp1, fp2, K1, K2, error, inlier_only, self.cameras[t1].img, self.cameras[t2].img) + + # split into traj and static + idx = np.where(inlier == 1)[0] + idx_mask = idx[mask] + inlier_static = idx_mask[idx_mask < pts1.shape[1]] + inlier_traj = idx_mask[idx_mask >= pts1.shape[1]] - pts1.shape[1] + + # save static part + self.static = X[:-1, idx_mask < pts1.shape[1]] + self.inlier_mask = np.ones(self.static.shape[1]) + + # also draw the inlier matches + if not debug: + if self.settings['feature_method'][0] == 'sift': + matchesMask_inliers = np.zeros((len(matches), 2)) + match_ids_inliers = match_ids[inlier_static] + matchesMask_inliers[match_ids_inliers] = [1, 0] + vis.draw_matches(self.cameras[t1].img, self.cameras[t1].kp, self.cameras[t2].img, self.cameras[t2].kp, matches, matchesMask_inliers) + + elif self.settings['feature_method'][0] == 'superglue': + # pts1_inlier = pts1[:,inlier_static] + # pts2_inlier = pts2[:,inlier_static] + pts1_inlier = self.cameras[t1].get_points(indices=query_ids[inlier_static]) + pts2_inlier = self.cameras[t2].get_points(indices=train_ids[inlier_static]) + + vis.draw_detection_matches(self.cameras[t1].img, np.vstack([np.zeros(pts1_inlier.shape[1]), pts1_inlier]), self.cameras[t2].img, np.vstack([np.zeros(pts2_inlier.shape[1]), pts2_inlier])) + + # # get the matching indices + # if debug: + # # query_ids -- the index of the features in cam1 + # query_ids = np.arange(len(pts1)) + # # train_ids -- index of the features in cam2 + # train_ids = np.arange(len(pts2)) + + # # initialize the matching result to be stored to the dict + # match_res1 = -np.ones((self.numCam, len(pts1))) + # match_res2 = -np.ones((self.numCam, len(pts2))) + # else: + # query_ids = np.array([m[0].queryIdx for m in matches]) + # train_ids = np.array([m[0].trainIdx for m in matches]) + + # # draw matches + # matchesMask_inliers = np.zeros((len(matches), 2)) + # match_ids = np.where(np.array(matchesMask)[:, 0] == 1)[0] + # match_ids_inliers = match_ids[inlier_static] + # matchesMask_inliers[match_ids_inliers] = [1, 0] + + # vis.draw_matches(self.cameras[t1].img, self.cameras[t1].kp, self.cameras[t2].img, self.cameras[t2].kp, matches, matchesMask_inliers) + + # query_ids = query_ids[match_ids] + # train_ids = train_ids[match_ids] + + # # initialize the matching result to be stored to the dict + # match_res1 = -np.ones((self.numCam, len(self.cameras[t1].kp))) + # match_res2 = -np.ones((self.numCam, len(self.cameras[t2].kp))) + + # save static 2d to cameras + self.cameras[t1].index_registered_2d = query_ids[inlier_static] + self.cameras[t1].index_2d_3d = np.arange(self.static.shape[1]) + self.cameras[t2].index_registered_2d = train_ids[inlier_static] + self.cameras[t2].index_2d_3d = np.arange(self.static.shape[1]) + + # match_res1[t2, query_ids] = train_ids + # match_res2[t1, train_ids] = query_ids + + # self.feature_dict[t1] = match_res1 + # self.feature_dict[t2] = match_res2 + + # save trajectory + self.traj = np.vstack((d1[0][inlier_traj], X[:-1, idx_mask >= pts1.shape[1]])) + + # only uses the detections for pose estimation + else: + X, P, inlier, mask = ep.epipolar_pipeline(d1[1:], d2[1:], K1, K2, error, inlier_only, self.cameras[t1].img, self.cameras[t2].img) + self.traj = np.vstack((d1[0][inlier==1][mask],X[:-1])) + + # # Compute fundamental matrix + # F,inlier = ep.computeFundamentalMat(d1[1:],d2[1:],error=error) + # E = np.dot(np.dot(K2.T,F),K1) + + # if not inlier_only: + # inlier = np.ones(len(inlier)) + # x1, x2 = util.homogeneous(d1[1:,inlier==1]), util.homogeneous(d2[1:,inlier==1]) + + # # Find corrected corresponding points for optimal triangulation + # N = d1[1:,inlier==1].shape[1] + # pts1=d1[1:,inlier==1].T.reshape(1,-1,2) + # pts2=d2[1:,inlier==1].T.reshape(1,-1,2) + # m1,m2 = cv2.correctMatches(F,pts1,pts2) + # x1,x2 = util.homogeneous(np.reshape(m1,(-1,2)).T), util.homogeneous(np.reshape(m2,(-1,2)).T) + + # mask = np.logical_not(np.isnan(x1[0])) + # x1 = x1[:,mask] + # x2 = x2[:,mask] + + # # Triangulte points + # X, P = ep.triangulate_from_E(E,K1,K2,x1,x2) + # self.traj = np.vstack((d1[0][inlier==1][mask],X[:-1])) # Assign the camera matrix for these two cameras self.cameras[t1].P = np.dot(K1,np.array([[1,0,0,0],[0,1,0,0],[0,0,1,0]])) self.cameras[t2].P = np.dot(K2,P) self.cameras[t1].decompose() self.cameras[t2].decompose() + + def init_traj_from_pose(self): + t1, t2 = self.sequence[0], self.sequence[1] + c1, c2 = -np.dot(self.cameras[t1].R.T, self.cameras[t1].t.reshape(-1,1)), -np.dot(self.cameras[t2].R.T, self.cameras[t2].t.reshape(-1,1)) + R12 = np.dot(self.cameras[t1].R.T, self.cameras[t2].R) + t12 = -np.dot(R12, c2-c1).ravel() + E = np.dot(self.cameras[t2].R, ep.skew(self.cameras[t2].t)) + E1 = np.dot(R12, ep.skew(t12)) + K1, K2 = self.cameras[t1].K, self.cameras[t2].K + + F = np.dot(np.linalg.inv(K2).T, np.dot(E, np.linalg.inv(K1))) + + # x1, x2 = util.homogeneous(self.cameras[t1].get_gt_pts()), util.homogeneous(self.cameras[t2].get_gt_pts()) + # l1 = F.dot(x1).T + # # compute distance between l1 and x2 + # dist12 = np.dot(l1, x2) + # dist12 /= np.sqrt(dist12[:,0]**2+dist12[:,1]**2).reshape(-1,1) + # matched_ids1 = np.argmin(dist12, axis=0) + # total_dist12 = np.mean(dist12[matched_ids1, np.arange(x2.shape[1])]) + + # l2 = F.T.dot(x2).T + # # compute distance between l1 and x2 + # dist21 = np.dot(l2, x1) + # dist21 /= np.sqrt(dist21[:,0]**2+dist21[:,1]**2).reshape(-1,1) + # matched_ids2 = np.argmin(dist21, axis=0) + # total_dist12 = np.mean(dist21[matched_ids2, np.arange(x1.shape[1])]) + # serr = ep.Sampson_error(x1,x2,F) + # vis.plot_epipolar_line(self.cameras[t1].img, self.cameras[t2].img, F, util.homogeneous(self.cameras[t1].get_gt_pts()), util.homogeneous(self.cameras[t2].get_gt_pts())) + pts1, pts2 = self.cameras[t1].get_gt_pts(), self.cameras[t2].get_gt_pts() + if self.settings['undist_points']: + # undistort the ground truth matches + pts1 = self.cameras[t1].undist_point(pts1, self.settings['undist_method']).T + pts2 = self.cameras[t2].undist_point(pts2, self.settings['undist_method']).T + + vis.plotEpiline(self.cameras[t1].img, self.cameras[t2].img, pts1.astype(int), pts2.astype(int), F) + # vis.plot_epipolar_line(self.cameras[t1].img, self.cameras[t2].img, F, util.homogeneous(self.detections[t1][1:]), util.homogeneous(self.detections[t2][1:])) + + # synchronization + # Truncate detections + self.cut_detection(second=self.settings['cut_detection_second']) + # Add prior alpha + self.init_alpha() + # synchronize between detections + # self.time_shift_from_F(F) + # self.time_shift_from_pose() + self.time_shift() + # convert detection timestamps to global + self.detection_to_global() + + # t1, t2 = self.sequence[0], self.sequence[1] + + # Find correspondences + if self.cameras[t1].fps > self.cameras[t2].fps: + d1, d2 = util.match_overlap(self.detections_global[t1], self.detections_global[t2]) + else: + d2, d1 = util.match_overlap(self.detections_global[t2], self.detections_global[t1]) + + # draw matches between dections + if self.settings['undist_points']: + # the background images are the original ones and are not undistorted, the detections need to be distorted + d1_dist = self.cameras[t1].dist_point2d(d1[1:], method=self.settings['undist_method']) + d2_dist = self.cameras[t2].dist_point2d(d2[1:], method=self.settings['undist_method']) + + vis.draw_detection_matches(self.cameras[t1].img, np.vstack([d1[0], d1_dist]), self.cameras[t2].img, np.vstack([d2[0],d2_dist])) + else: + vis.draw_detection_matches(self.cameras[t1].img, d1, self.cameras[t2].img, d2) + + # X, P, inlier, mask = ep.epipolar_pipeline_from_E(d1[1:], d2[1:], K1, K2, E) + X, P, inlier, mask = ep.epipolar_pipeline_from_F(d1[1:], d2[1:], K1, K2, F) + + self.traj = np.vstack((d1[0][inlier==1][mask],X[:-1])) + + def init_static(self, error=10, inlier_only=False, debug=False): + + self.select_next_camera_static(init=True, debug=debug) + + t1, t2 = self.sequence[0], self.sequence[1] + K1, K2 = self.cameras[t1].K, self.cameras[t2].K + + # if feature_dict is empty + if not self.feature_dict: + if debug: + # in debug, use static ground truth as 2d featues + pts1 = self.cameras[t1].gt_pts.T + pts2 = self.cameras[t2].gt_pts.T + + vis.draw_detection_matches(self.cameras[t1].img, np.vstack([np.zeros(pts1.shape[1]),pts1]), self.cameras[t2].img, np.vstack([np.zeros(pts2.shape[1]),pts2])) + + # save the matching result, the indices of the ground truth matches are shared across all cameras + query_ids = np.arange(self.cameras[t1].gt_pts.shape[0]) + train_ids = np.arange(self.cameras[t2].gt_pts.shape[0]) + + # initialize the matrix for storing matching result + match_res1 = -np.ones((self.numCam, self.cameras[t1].gt_pts.shape[0])) + match_res2 = -np.ones((self.numCam, self.cameras[t2].gt_pts.shape[0])) + + else: + # Match features with sift + if self.settings['feature_method'][0] == 'sift': + pts1, pts2, matches, matchesMask = ep.matching_feature(self.cameras[t1].kp, self.cameras[t2].kp, self.cameras[t1].des, self.cameras[t2].des, ratio=0.8) + # draw matches + vis.draw_matches(self.cameras[t1].img, self.cameras[t1].kp, self.cameras[t2].img, self.cameras[t2].kp, matches, matchesMask) + # get the valid matches + match_ids = np.where(np.array(matchesMask)[:, 0] == 1)[0] + + # get the valid matches + query_ids = np.array([m[0].queryIdx for m in matches]) + train_ids = np.array([m[0].trainIdx for m in matches]) + query_ids = query_ids[match_ids] + train_ids = train_ids[match_ids] + + # initialize the matrix for storing matching result + match_res1 = -np.ones((self.numCam, len(self.cameras[t1].kp))) + match_res2 = -np.ones((self.numCam, len(self.cameras[t2].kp))) + + pts1, pts2 = np.array(pts1).T, np.array(pts2).T + else: + raise Exception("Superglue matches are not properly loaded.") + + # save the matching result to feature_dict + match_res1[t2,query_ids] = train_ids + match_res2[t1,train_ids] = query_ids + self.feature_dict[t1] = match_res1 + self.feature_dict[t2] = match_res2 + + # if feature_dict is already specified, get the corresponding pts + else: + match_res = self.feature_dict[t1][t2] + query_ids = np.where(match_res > -1)[0] + train_ids = match_res[match_res > -1].astype(int) + + # # create the matches, and matchesMask for visualization (all the matches specify here are valid matches) + # matches = [cv2.DMatch(i, i, 0) for i in range(len(self.cameras[t1].kp))] + # matchesMask = [[0,0] for i in range(len(self.cameras[t1].kp))] + + # for q, t in zip(query_ids, train_ids): + # matches[q] = cv2.DMatch(q, t, 0) + # matchesMask[q] = [1,0] + + # # matchesMask = [[1,0] for i in range(len(query_ids))] + + pts1 = self.cameras[t1].get_points(indices=query_ids) + pts2 = self.cameras[t2].get_points(indices=train_ids) + + # draw matches + # vis.draw_matches(self.cameras[t1].img, self.cameras[t1].kp, self.cameras[t2].img, self.cameras[t2].kp, matches, matchesMask) + + # draw matches + vis.draw_detection_matches(self.cameras[t1].img, np.vstack([np.zeros(pts1.shape[1]),pts1]), self.cameras[t2].img, np.vstack([np.zeros(pts2.shape[1]),pts2])) + + + # undistort the matched keypoints + if self.settings['undist_points']: + pts1 = self.cameras[t1].undist_point(pts1, self.settings['undist_method']) + pts2 = self.cameras[t2].undist_point(pts2, self.settings['undist_method']) + + pts1 = np.int32(pts1) + pts2 = np.int32(pts2) + + # go through the epipolar pipeline for pose estimation and initial scene reconstruction + # if debug: + # X, P, inlier, mask = ep.epipolar_pipeline(pts1, pts2, K1, K2, error, False, self.cameras[t1].img, self.cameras[t2].img) + # else: + # X, P, inlier, mask = ep.epipolar_pipeline(pts1, pts2, K1, K2, error, inlier_only, self.cameras[t1].img, self.cameras[t2].img) + + X, P, inlier, mask = ep.epipolar_pipeline(pts1, pts2, K1, K2, error, inlier_only, self.cameras[t1].img, self.cameras[t2].img) + + # save the static part + self.static = X[:-1] + self.inlier_mask = np.ones(self.static.shape[1]) + + # ids of the inliers + inlier_ids = np.where(inlier == 1)[0] + inlier_ids_masked = inlier_ids[mask] + + # also draw the inlier matches + if not debug: + if self.settings['feature_method'][0] == 'sift': + matchesMask_inliers = np.zeros((len(matches), 2)) + match_ids_inliers = match_ids[inlier_ids_masked] + matchesMask_inliers[match_ids_inliers] = [1, 0] + vis.draw_matches(self.cameras[t1].img, self.cameras[t1].kp, self.cameras[t2].img, self.cameras[t2].kp, matches, matchesMask_inliers) + + elif self.settings['feature_method'][0] == 'superglue': + # pts1_inlier = pts1[:,inlier_ids_masked] + # pts2_inlier = pts2[:,inlier_ids_masked] + pts1_inlier = self.cameras[t1].get_points(indices=query_ids[inlier_ids_masked]) + pts2_inlier = self.cameras[t2].get_points(indices=train_ids[inlier_ids_masked]) + + vis.draw_detection_matches(self.cameras[t1].img, np.vstack([np.zeros(pts1_inlier.shape[1]),pts1_inlier]), self.cameras[t2].img, np.vstack([np.zeros(pts2_inlier.shape[1]),pts2_inlier])) + # register these points and store their indices to the cameras + self.cameras[t1].index_registered_2d = query_ids[inlier_ids_masked] + self.cameras[t1].index_2d_3d = np.arange(self.static.shape[1]) + self.cameras[t2].index_registered_2d = train_ids[inlier_ids_masked] + self.cameras[t2].index_2d_3d = np.arange(self.static.shape[1]) + + # construct projections + self.cameras[t1].P = np.dot(K1,np.array([[1,0,0,0],[0,1,0,0],[0,0,1,0]])) + self.cameras[t2].P = np.dot(K2,P) + self.cameras[t1].decompose() + self.cameras[t2].decompose() def traj_to_spline(self,smooth_factor): ''' @@ -358,6 +742,55 @@ def error_cam(self,cam_id,mode='dist',motion_prior=False,norm=False): error_y[idx.astype(bool)] = abs(x_cal[1]-x[1]) return np.concatenate((error_x, error_y)) + def error_cam_static(self, cam_id, mode='dist', norm=False, debug=False): + ''' + Compute the reprojection error for static scene + ''' + # get the 3D static points reconstructed from this camera + point_3D = np.empty([3, 0]) + if len(self.cameras[cam_id].index_2d_3d) > 0: + point_3D = np.hstack([point_3D, self.static[:, self.cameras[cam_id].index_2d_3d]]) + X = util.homogeneous(point_3D) + + # get the corresponding 2d static points + if debug: + # use the ground truth static poitns + x = self.cameras[cam_id].get_gt_pts() + else: + # use the extracted static features + x = self.cameras[cam_id].get_points() + + if X.shape[1] > 0: + if self.settings['undist_points']: + # undistort 2d points + x = self.cameras[cam_id].undist_point(x, self.settings['undist_method']) + + x_cal = self.cameras[cam_id].projectPoint(X) + else: + x_cal = np.empty([3,0]) + + # # distort point + # x_cal = self.cameras[cam_id].dist_point3d(point_3D, self.settings['undist_method']) + # print(cam_id,self.cameras[cam_id].index_2d_3d) + + #Normalize Tracks + if norm: + x_cal = np.dot(np.linalg.inv(self.cameras[cam_id].K), x_cal) + x = np.dot(np.linalg.inv(self.cameras[cam_id].K), util.homogeneous(x)) + + if mode == 'dist': + return ep.reprojection_error(x, x_cal) + elif mode == 'xy_1D': + return np.concatenate((abs(x_cal[0] - x[0]), abs(x_cal[1] - x[1]))) + elif mode == 'xy_2D': + return np.vstack((abs(x_cal[0] - x[0]), abs(x_cal[1] - x[1]))) + elif mode == 'each': + error_x = np.zeros_like(x[0]) + error_y = np.zeros_like(x[0]) + error_x = abs(x_cal[0] - x[0]) + error_y = abs(x_cal[1] - x[1]) + return np.concatenate((error_x, error_y)) + def error_motion(self,cams,mode='dist',norm=False,motion_weights=0,motion_reg = False,motion_prior = False): ''' @@ -438,7 +871,7 @@ def compute_visibility(self): self.visible.append(visible) - def BA(self, numCam, max_iter=10, rs=False, motion_prior=False,motion_reg=False,motion_weights=1,norm=False,rs_bounds=False): + def BA(self, numCam, max_iter=10, rs=False, motion_prior=False,motion_reg=False,motion_weights=1,norm=False,rs_bounds=False, debug=False, sync=True, scaling=False): ''' Bundle Adjustment with multiple splines @@ -450,9 +883,16 @@ def error_BA(x): Input is the model (parameters that need to be optimized) ''' + # if the static part are included, they are added to the beginning of the columns. + # first parse the pararmeters for the static points + if 'include_static' in self.settings.keys() and self.settings['include_static']: + # the rest of the parameters are the 3d positions of the static scene + self.static[:, self.inlier_mask > 0] = x[:num_3d_points * 3].reshape(-1, 3).T + # Assign parameters to the class attributes sections = [numCam, numCam*2, numCam*3, numCam*3+numCam*num_camParam] - model_parts = np.split(x, sections) + # parse the rest of the parameters terms excluding the static parts + model_parts = np.split(x[num_3d_points * 3:], sections) self.alpha[self.sequence[:numCam]], self.beta[self.sequence[:numCam]], self.rs[self.sequence[:numCam]] = model_parts[0], model_parts[1], model_parts[2] cams = np.split(model_parts[3],numCam) @@ -484,12 +924,19 @@ def error_BA(x): error_motion_reg = self.error_motion(self.sequence[:numCam],motion_reg=True,motion_weights=motion_weights) error = np.concatenate((error, error_motion_reg)) + # also add the errors regarding the static part + if 'include_static' in self.settings.keys() and self.settings['include_static']: + for i in range(numCam): + error_each_static = self.error_cam_static(self.sequence[i], mode='each',debug=debug) + error = np.concatenate((error, error_each_static)) + return error def jac_BA(near=3,motion_offset=10): - num_param = len(model) + # exclude the parts regarding the static points for now + num_param = len(model) - 3*num_3d_points self.compute_visibility() jac = lil_matrix((1, num_param),dtype=int) @@ -603,7 +1050,43 @@ def jac_BA(near=3,motion_offset=10): m_jac[j,m_traj_idx[m_traj_idx < num_param]] = 1 jac = vstack((jac, m_jac)) + + # add jacobian matrix for the static error terms + if 'include_static' in self.settings.keys() and self.settings['include_static']: + jac_parts = [] + # inlier ids of the static points used for BA + inlier_ids = np.where(self.inlier_mask > 0)[0] + # loop through all cameras + for i in range(numCam): + # get camera id + cam_id = self.sequence[i] + # get the number of registered 2d points in this camera + num_pts_2d = self.cameras[cam_id].index_registered_2d.shape[0] + # the index of the registered 2d in 3d static points in the inlier set + registered_ids = np.in1d(inlier_ids, self.cameras[cam_id].index_2d_3d).nonzero()[0] + + # initialize the jac mat (the structures of the sparsity matrix are the same in x and y direction) + jac_part = lil_matrix((num_pts_2d, len(model))) + + # mark all entries relate to this camera as 1 (the first num_3d_points*3 columns are for the static 3d points) + start, end = num_3d_points * 3 + 3*numCam + i * num_camParam, num_3d_points * 3 + 3*numCam + (i+1) * num_camParam + jac_part[:, start:end] = 1 + + # mark all entries relate to the static 3d points as 1 + jac_part[np.arange(num_pts_2d), registered_ids * 3] = 1 + jac_part[np.arange(num_pts_2d), registered_ids * 3 + 1] = 1 + jac_part[np.arange(num_pts_2d), registered_ids * 3 + 2] = 1 + + # append mat for x direction and y direction + jac_parts.append(jac_part) + jac_parts.append(jac_part) + + # FIXME: check if the dimension is the same + jac_static = vstack(jac_parts) + static_empty = lil_matrix((jac.shape[0], 3*num_3d_points)) + jac = hstack((static_empty, jac)) + jac = vstack((jac, jac_static)) # fix the first camera # jac[:,[0,numCam]], jac[:,2*numCam+4:2*numCam+10] = 0, 0 #return jac @@ -649,6 +1132,15 @@ def jac_BA(near=3,motion_offset=10): idx_spline_sum = idx_spline + len(model_other) model = np.concatenate((model_other, model_spline)) assert idx_spline_sum[-1,-1] == len(model), 'Error in spline indices' + + # add the static part to BA + num_3d_points = 0 + if 'include_static' in self.settings.keys() and self.settings['include_static']: + num_3d_points = self.static[:, self.inlier_mask > 0].shape[1] + model_static = self.static[:, self.inlier_mask > 0].T.ravel() + # concatenate the static point_3d to the beginning + model = np.concatenate((model_static, model)) + print('Number of BA parameters is {}'.format(len(model))) # constrain rs params to between 0 and 1 @@ -667,12 +1159,27 @@ def jac_BA(near=3,motion_offset=10): '''Compute BA''' print('Doing BA with {} cameras...\n'.format(numCam)) fn = lambda x: error_BA(x) - res = least_squares(fn,model,jac_sparsity=A,tr_solver='lsmr',xtol=1e-12,max_nfev=max_iter,verbose=0,bounds=bounds_rs) + # ignore jac_sparsity matrix for now for the static part + # if 'include_static' in self.settings.keys() and self.settings['include_static']: + # res = least_squares(fn,model,tr_solver='lsmr',xtol=1e-12,max_nfev=max_iter,verbose=1,bounds=bounds_rs) + # else: + if scaling: + print('apply x_scaling during BA') + res = least_squares(fn,model,jac_sparsity=A,tr_solver='lsmr',x_scale='jac',xtol=1e-12,max_nfev=max_iter,verbose=1,bounds=bounds_rs) + else: + res = least_squares(fn,model,jac_sparsity=A,tr_solver='lsmr',xtol=1e-12,max_nfev=max_iter,verbose=1,bounds=bounds_rs) '''After BA''' + # if the static part are included, they are added to the beginning of the columns. + # first parse the pararmeters for the static points + if 'include_static' in self.settings.keys() and self.settings['include_static']: + # update the 3d positions of the static scene + self.static[:, self.inlier_mask > 0] = res.x[:num_3d_points * 3].reshape(-1, 3).T + # Assign the optimized model to alpha, beta, cam, and spline sections = [numCam, numCam*2, numCam*3, numCam*3+numCam*num_camParam] - model_parts = np.split(res.x, sections) + # exclude the parameters regarding the static part and parse the results + model_parts = np.split(res.x[num_3d_points * 3:], sections) self.alpha[self.sequence[:numCam]], self.beta[self.sequence[:numCam]], self.rs[self.sequence[:numCam]] = model_parts[0], model_parts[1], model_parts[2] cams = np.split(model_parts[3],numCam) @@ -695,9 +1202,125 @@ def jac_BA(near=3,motion_offset=10): self.detection_to_global() return res + + def BA_static(self, numCam, max_iter=10, debug=False, scaling=True): + ''' + Function: + standard BA with static points and camera models + ''' + + def error_BA(x): + ''' + Function: + define the error terms of BA + Input: + x = parameters to be optimized + ''' + model_static, model_cam = x[:num_3d_points * 3], x[num_3d_points * 3:] + # parse the 3d static + self.static[:,self.inlier_mask > 0] = model_static.reshape(-1,3).T - def remove_outliers(self, cams, thres=30, verbose=False): + # parse the new camera parameters and poses + cams = np.split(model_cam, numCam) + for i in range(numCam): + self.cameras[self.sequence[i]].vector2P(cams[i], calib=self.settings['opt_calib']) + + # compute the error terms + error = np.array([]) + for i in range(numCam): + error_static_each = self.error_cam_static(self.sequence[i], mode='each', debug=debug) + error = np.concatenate((error, error_static_each)) + + return error + + def jac_BA(): + ''' + Function: + define the sparse jaccobian matrix + ''' + + num_param = len(model) + + jac_parts = [] + # inlier ids of the static points used for BA + inlier_ids = np.where(self.inlier_mask > 0)[0] + # loop through all cameras + for i in range(numCam): + # get camera id + cam_id = self.sequence[i] + # get the number of registered 2d points in this camera + num_pts_2d = self.cameras[cam_id].index_registered_2d.shape[0] + # the index of the registered 2d in 3d static points in the inlier set + registered_ids = np.in1d(inlier_ids, self.cameras[cam_id].index_2d_3d).nonzero()[0] + + # initialize the jac mat (the structures of the sparsity matrix are the same in x and y direction) + jac_part = lil_matrix((num_pts_2d, num_param)) + + # mark all entries relate to this camera as 1 (the first num_3d_points*3 columns are for the static 3d points) + start, end = num_3d_points * 3 + i * num_camParam, num_3d_points * 3 + (i+1) * num_camParam + jac_part[:, start:end] = 1 + + # mark all entries relate to the static 3d points as 1 + jac_part[np.arange(num_pts_2d), registered_ids * 3] = 1 + jac_part[np.arange(num_pts_2d), registered_ids * 3 + 1] = 1 + jac_part[np.arange(num_pts_2d), registered_ids * 3 + 2] = 1 + + # append mat for x direction and y direction + jac_parts.append(jac_part) + jac_parts.append(jac_part) + + # FIXME: check if the dimension is the same + + jac = vstack(jac_parts) + return jac.toarray() + + ''' BEFORE BA ''' + # camera model + model_cam = np.array([]) + num_camParam = 15 if self.settings['opt_calib'] else 6 + for i in self.sequence[:numCam]: + model_cam = np.concatenate((model_cam, self.cameras[i].P2vector(calib=self.settings['opt_calib']))) + + # 3d static points + num_3d_points = self.static[:, self.inlier_mask > 0].shape[1] + model_static = self.static[:, self.inlier_mask > 0].T.ravel() + # concatenate the static point_3d to the beginning + model = np.concatenate((model_static, model_cam)) + + print('Number of BA parameters is {}'.format(len(model))) + + ''' BA ''' + # set the jacobian matrix + A = jac_BA() + print('Doing BA with {} cameras and {} static points...\n'.format(numCam, num_3d_points)) + + # define the error function + fn = lambda x: error_BA(x) + + # least-sqaure optimization for BA + # res = least_squares(fn, model, tr_solver='lsmr', xtol=1e-12, max_nfev=max_iter, verbose=1) + if scaling: + print('apply x_scaling during BA') + res = least_squares(fn, model, jac_sparsity=A, tr_solver='lsmr', x_scale='jac', xtol=1e-12, max_nfev=max_iter, verbose=1) + else: + res = least_squares(fn, model, jac_sparsity=A, tr_solver='lsmr', xtol=1e-12, max_nfev=max_iter, verbose=1) + + ''' AFTER BA ''' + # parse the result of BA + res_static, res_cam = res.x[:num_3d_points * 3], res.x[num_3d_points * 3:] + + # update the 3d static points + self.static[:, self.inlier_mask > 0] = res_static.reshape(-1,3).T + + # update the camera parameters + cams = np.split(res_cam, numCam) + for i in range(numCam): + self.cameras[self.sequence[i]].vector2P(cams[i], calib=self.settings['opt_calib']) + print(self.cameras[self.sequence[i]].K) + print(self.cameras[self.sequence[i]].d) + + def remove_outliers(self, cams, thres=30, verbose=False, debug=False): ''' Remove raw detections that have large reprojection errors. @@ -715,9 +1338,230 @@ def remove_outliers(self, cams, thres=30, verbose=False): if verbose: print('{} out of {} detections are removed for camera {}'.format(sum(error>=thres),sum(error!=0),i)) + + # if the static part is included, also remove outliers in the static part + if 'include_static' in self.settings.keys() and self.settings['include_static']: + self.remove_outliers_static(cams, thres=thres, verbose=verbose, debug=debug) + # # filter out the outliers from the static scene + # error_static = self.error_cam_static(i, mode='dist', debug=debug) + + # # indices of the outliers in the reconstructed 3D points + # outlier_ids = self.cameras[i].index_2d_3d[error_static >= self.settings['thres_outlier_static']] + # # maskout these points in the inlier_mask + # self.inlier_mask[outlier_ids] = 0 + # # remove feature ids corresponds to the outliers from the registered list + # self.cameras[i].index_2d_3d = self.cameras[i].index_2d_3d[error_static < self.settings['thres_outlier_static']] + # self.cameras[i].index_registered_2d = self.cameras[i].index_registered_2d[error_static < self.settings['thres_outlier_static']] + + # # also update the registered 2d index of the other cameras + # for j in cams: + # if j == i: + # continue + + # # find the ids of the outlier in the camera + # cond = np.in1d(self.cameras[j].index_2d_3d, outlier_ids) + # self.cameras[j].index_2d_3d = self.cameras[j].index_2d_3d[~cond] + # self.cameras[j].index_registered_2d = self.cameras[j].index_registered_2d[~cond] + + # if verbose: + # print('{} out of {} static points are removed for camera {}'.format(len(outlier_ids), len(error_static), i)) + + def remove_outliers_static(self, cams, thres=30, verbose=False, debug=False): + ''' + Function: + remove the outliers from the static scene + ''' + if thres: + for i in cams: + # filter out the outliers from the static scene + error_static = self.error_cam_static(i, mode='dist', debug=debug) + + # indices of the outliers in the reconstructed 3D points + outlier_ids = self.cameras[i].index_2d_3d[error_static >= self.settings['thres_outlier_static']] + # maskout these points in the inlier_mask + self.inlier_mask[outlier_ids] = 0 + # remove feature ids corresponds to the outliers from the registered list + self.cameras[i].index_2d_3d = self.cameras[i].index_2d_3d[error_static < self.settings['thres_outlier_static']] + self.cameras[i].index_registered_2d = self.cameras[i].index_registered_2d[error_static < self.settings['thres_outlier_static']] + + # also update the registered 2d index of the other cameras + for j in cams: + if j == i: + continue + + # find the ids of the outlier in the camera + cond = np.in1d(self.cameras[j].index_2d_3d, outlier_ids) + self.cameras[j].index_2d_3d = self.cameras[j].index_2d_3d[~cond] + self.cameras[j].index_registered_2d = self.cameras[j].index_registered_2d[~cond] + + if verbose: + print('{} out of {} static points are removed for camera {}'.format(len(outlier_ids), len(error_static), i)) + + def remove_outliers_3d(self, cams, mode='center', thres=100, verbose=False, debug=False): + ''' + Function: + remove the outliers from the static scene + ''' + if mode == 'camera': + cam_centers = [-self.cameras[i].R.T @ self.cameras[i].t.reshape((-1,1)) for i in cams] + origin = np.mean(np.hstack(cam_centers),axis=1).reshape(-1) + else: + # remove points far from point center + origin = np.mean(self.static[:, self.inlier_mask > 0], axis=1) + inlier_ids = np.where(self.inlier_mask > 0)[0] + distance = np.linalg.norm(self.static[:, self.inlier_mask > 0].T - origin, axis=1) + + # mask = distance > np.mean(distance) + sigma*np.std(distance) + mask = distance > thres + + # mask out point outside range + outlier_ids = inlier_ids[mask] + self.inlier_mask[outlier_ids] = 0 + + for i in cams: + id_mask = np.isin(self.cameras[i].index_2d_3d, outlier_ids, invert=True) + self.cameras[i].index_2d_3d = self.cameras[i].index_2d_3d[id_mask] + self.cameras[i].index_registered_2d = self.cameras[i].index_registered_2d[id_mask] + + if verbose: + print('{} out of {} static points are removed from the scene'.format(np.sum(mask), len(inlier_ids))) + + + + def register_new_camera_static(self, cam_id, cams, debug=False): + ''' + Function: + find 2d - 3d correspondences between the features in the new camera and existing 3d + Input: + cam_id = the id of the new camera + cams = a sequence of existing cameras + Output: + pts_2d = the features in the new camera that match the existing 3d points (does not need to undistort the pts, this will be taken care when solving pnp) + pts_3d = the matched existing 3d points + ''' + if debug: + # use the ground truth matches + # if have not yet initialize the cam_id in the feature dict, do so + if cam_id not in self.feature_dict.keys(): + # initialize the matching result of this new camera to be stored to the dict + self.feature_dict[cam_id] = -np.ones((self.numCam, len(self.cameras[cam_id].gt_pts))) + # match between the features of this new camera and all other cameras + for i in cams: + if i == cam_id: + continue + # all the ground truth matches are mutually valid, so directly update + self.feature_dict[cam_id][i] = np.arange(len(self.cameras[i].gt_pts)) + self.feature_dict[i][cam_id] = np.arange(len(self.cameras[cam_id].gt_pts)) + + # get the registered indices based on matches in feature dict + for i in cams: + if i == cam_id: + continue + # update the registered indices (all ground truth matches share the same indices) + self.cameras[cam_id].index_2d_3d = self.cameras[i].index_2d_3d + self.cameras[cam_id].index_registered_2d = self.cameras[i].index_registered_2d + # add the ground truth + pts_2d = self.cameras[cam_id].get_gt_pts() + + else: + # if have not yet initialize cam_id in feature_dict, do so + if cam_id not in self.feature_dict.keys(): + + # this only happens when using sift extractor + assert self.settings['feature_method'][0] == 'sift', 'Currently only support sift, and Superglue; however, the Superglue matches are not properly loaded' + + # get the features and descriptors of the new camera + kp1, des1 = self.cameras[cam_id].kp, self.cameras[cam_id].des + # initilize the matching result of the new camera to be stored in feature_dict + match_res = -np.ones((self.numCam, len(kp1))) + # loop through all the cameras + for i in cams: + if i == cam_id: + continue + # get the features of the old camera + kp2, des2 = self.cameras[i].kp, self.cameras[i].des + # match the features of the new camera and the old camera + _, _, matches, matchesMask = ep.matching_feature(kp1, kp2, des1, des2, ratio=0.8) + # draw matches + vis.draw_matches(self.cameras[cam_id].img, self.cameras[cam_id].kp, self.cameras[i].img, self.cameras[i].kp, matches, matchesMask) + + # get the matched indices + query_ids = np.array([m[0].queryIdx for m in matches]) + train_ids = np.array([m[0].trainIdx for m in matches]) + + # get the valid matches + match_ids = np.where(np.array(matchesMask)[:, 0] == 1)[0] + query_ids = query_ids[match_ids] + train_ids = train_ids[match_ids] + + # save the matching result to feature_dict + match_res[i, query_ids] = train_ids + self.feature_dict[cam_id] = match_res + self.feature_dict[i][cam_id, train_ids] = query_ids + + # otherwise get the matching results from feature dict + for i in cams: + if i == cam_id: + continue + + # get the matching results between cam_id and i + match_res = self.feature_dict[cam_id][i] + train_ids = match_res[match_res >= 0] + query_ids = np.where(match_res >= 0)[0] + # find the indices of the old features that has been used + _, train_in_ids, registered_ids = np.intersect1d(train_ids, self.cameras[i].index_registered_2d, return_indices=True) + + # # register corresponding query_ids of the new camera + # self.cameras[cam_id].index_registered_2d = np.union1d(self.cameras[cam_id].index_registered_2d, query_ids[train_in_ids]).astype(int) + # self.cameras[cam_id].index_2d_3d = np.union1d(self.cameras[cam_id].index_2d_3d, self.cameras[i].index_2d_3d[registered_ids]).astype(int) + + # find the indices of the point that has not been added + newids = np.isin(self.cameras[i].index_2d_3d[registered_ids], self.cameras[cam_id].index_2d_3d, invert=True) + self.cameras[cam_id].index_2d_3d = np.concatenate([self.cameras[cam_id].index_2d_3d, self.cameras[i].index_2d_3d[registered_ids[newids]]]).astype(int) + self.cameras[cam_id].index_registered_2d = np.concatenate([self.cameras[cam_id].index_registered_2d, query_ids[train_in_ids[newids]]]).astype(int) + + # get the registered 2d features from the new camera + pts_2d = self.cameras[cam_id].get_points() + + # get the registered 3d static point from the new camera + if len(self.cameras[cam_id].index_2d_3d) == 0: + pts_3d = np.empty((3,0)) + else: + pts_3d = self.static[:, self.cameras[cam_id].index_2d_3d] + + return pts_2d, pts_3d + + def get_camera_pose_static(self, cam_id, cams, error=8, verbose=0, debug=False): + ''' + Function: + solve the PnP to get the pose of the new camera + the distortion model is taken care by PnP + Input: + cam_id = the index of the new camera + pts_2d = the matched raw 2d feature positions + pts_3d = the matched reconstructed 3d points + ''' + pts_2d, pts_3d = self.register_new_camera_static(cam_id, cams, debug) + # PnP solution from OpenCV + N = pts_3d.shape[1] + objectPoints = np.ascontiguousarray(pts_3d.T).reshape((N,1,3)) + imagePoints = np.ascontiguousarray(pts_2d.T).reshape((N,1,2)) + distCoeffs = self.cameras[cam_id].d + retval, rvec, tvec, inliers = cv2.solvePnPRansac(objectPoints, imagePoints, self.cameras[cam_id].K, distCoeffs, reprojectionError=error) + + # update the indices of the registered 2d features, removing the outliers + self.cameras[cam_id].index_registered_2d = self.cameras[cam_id].index_registered_2d[inliers.ravel()] + self.cameras[cam_id].index_2d_3d = self.cameras[cam_id].index_2d_3d[inliers.ravel()] + + self.cameras[cam_id].R = cv2.Rodrigues(rvec)[0] + self.cameras[cam_id].t = tvec.reshape(-1,) + self.cameras[cam_id].compose() - def get_camera_pose(self, cam_id, error=8, verbose=0): + if verbose: + print('{} out of {} points are inliers for PnP'.format(inliers.shape[0], N)) + + def get_camera_pose(self, cam_id, error=8, verbose=0, debug=False): ''' Get the absolute pose of a camera by solving the PnP problem. @@ -735,6 +1579,16 @@ def get_camera_pose(self, cam_id, error=8, verbose=0): if detect_part.size: detect = np.hstack((detect,detect_part)) point_3D = np.hstack((point_3D, np.asarray(interpolate.splev(detect_part[0], tck[i])))) + # FIXME: SHOULD USE THE RAW DETECTION? INSTEAD OF THE UNDISTORTED ONES?? + + num_detect = detect.shape[1] + # if the static part is also included, add the static part to the points as well + if 'include_static' in self.settings.keys() and self.settings['include_static']: + pts_2d, pts_3d = self.register_new_camera_static(cam_id, self.sequence[:cam_id], debug) + + # stack the static points with the detections + detect = np.hstack([detect, np.vstack([np.zeros(pts_2d.shape[1]),pts_2d])]) + point_3D = np.hstack([point_3D, pts_3d]) # PnP solution from OpenCV N = point_3D.shape[1] @@ -743,6 +1597,10 @@ def get_camera_pose(self, cam_id, error=8, verbose=0): distCoeffs = self.cameras[cam_id].d retval, rvec, tvec, inliers = cv2.solvePnPRansac(objectPoints, imagePoints, self.cameras[cam_id].K, distCoeffs, reprojectionError=error) + # update the index of the registered 2d static points to remove the outliers + self.cameras[cam_id].index_registered_2d = self.cameras[cam_id].index_registered_2d[inliers[inliers >= num_detect] - num_detect] + self.cameras[cam_id].index_2d_3d = self.cameras[cam_id].index_2d_3d[inliers[inliers >= num_detect] - num_detect] + self.cameras[cam_id].R = cv2.Rodrigues(rvec)[0] self.cameras[cam_id].t = tvec.reshape(-1,) self.cameras[cam_id].compose() @@ -813,7 +1671,86 @@ def triangulate(self, cam_id, cams, factor_t2s, factor_s2t=0.02, thres=0, refit= self.traj_to_spline(smooth_factor=factor_t2s) return X_new + + def triangulate_static(self, cam_id, cams, thres=0, verbose=0): + ''' + Triangulate new points from the static scene to the existing 3D scene + + cam_id is the new camera + cams must be an iterable that contains cameras that have been processed to build the 3D static scene + ''' + + assert self.cameras[cam_id].P is not None, 'The camera pose must be computed first' + + # find the features of this camera that have not yet been triangulated + all_ids = np.arange(len(self.cameras[cam_id].kp)) + cand_ids = all_ids[~np.in1d(all_ids, self.cameras[cam_id].index_registered_2d)] + + X_new = np.empty([3, 0]) + added_cand_ids = np.empty(0) + # loop through all old cameras, and triangulate the matched features + for i in cams: + if i == cam_id: + continue + # get the matched features in this camera + new_ids, matched_ids, _ = np.intersect1d(self.feature_dict[i][cam_id], cand_ids, return_indices=True) + new_ids = new_ids.astype(int) + # get the 2d points from the two cameras + pts1 = self.cameras[cam_id].get_points(new_ids) + pts2 = self.cameras[i].get_points(matched_ids) + + # undistort the 2d points if needed + if self.settings['undist_points']: + pts1 = self.cameras[cam_id].undist_point(pts1, self.settings['undist_method']) + pts2 = self.cameras[i].undist_point(pts2, self.settings['undist_method']) + + # get the poses of the two cameras + P1, P2 = self.cameras[cam_id].P, self.cameras[i].P + + # triangulate the points + X_i = ep.triangulate_matlab(pts1, pts2, P1, P2) + + # Check reprojection error directly after triangulation, preserve those with small error + if thres: + err_1 = ep.reprojection_error(pts1, self.cameras[cam_id].projectPoint(X_i)) + err_2 = ep.reprojection_error(pts2, self.cameras[i].projectPoint(X_i)) + mask = np.logical_and(err_1 < thres, err_2 < thres) + X_i = X_i[:, mask] + new_ids = new_ids[mask] + matched_ids = matched_ids[mask] + + if verbose: + print('{} out of {} points are triangulated'.format(sum(mask), len(err_1))) + + added_mask = np.in1d(new_ids, added_cand_ids) + # assign ids to the points to be added + ids_3d_new = np.arange(new_ids[~added_mask].shape[0]) + X_new.shape[1] + int(np.sum(self.inlier_mask)) + if verbose: + print('{} new points are added'.format(ids_3d_new.shape[0])) + + # registered the points that have not yet been added + self.cameras[cam_id].index_registered_2d = np.concatenate((self.cameras[cam_id].index_registered_2d, new_ids[~added_mask])) + self.cameras[cam_id].index_2d_3d = np.concatenate((self.cameras[cam_id].index_2d_3d, ids_3d_new)) + + # register the new points in the old camera + # first add the points which have previously been added + self.cameras[i].index_registered_2d = np.concatenate((self.cameras[i].index_registered_2d, matched_ids[added_mask])) + # get the corresponding 3d point indices + ids_3d_old = np.in1d(added_cand_ids, new_ids).nonzero()[0] + int(np.sum(self.inlier_mask)) + self.cameras[i].index_2d_3d = np.concatenate((self.cameras[i].index_2d_3d, ids_3d_old)) + # then add the points which have not yet been added + self.cameras[i].index_registered_2d = np.concatenate((self.cameras[i].index_registered_2d, matched_ids[~added_mask])) + self.cameras[i].index_2d_3d = np.concatenate((self.cameras[i].index_2d_3d, ids_3d_new)) + + # keep a track of the new ids + add_cand_ids = np.concatenate((added_cand_ids, new_ids[~added_mask])) + # add the new points to the record + X_new = np.hstack([X_new, X_i[:-1, ~added_mask]]) + + # add these new points to the static scene + self.static = np.hstack([self.static, X_new]) + self.inlier_mask = np.concatenate((self.inlier_mask, np.ones(X_new.shape[1]))) def plot_reprojection(self,interval=np.array([[-np.inf],[np.inf]]),match=True): ''' @@ -847,7 +1784,6 @@ def plot_reprojection(self,interval=np.array([[-np.inf],[np.inf]]),match=True): plt.show() - def select_most_overlap(self,init=False): ''' Select either the initial pair of cameras or the next best camera with largest overlap @@ -882,7 +1818,112 @@ def select_most_overlap(self,init=False): overlap_max = len(overlap) next_cam = i self.sequence.append(next_cam) + + def select_next_camera_static(self, init=False, debug=False): + ''' + Function: + find to the next camera to be registered based on the static part only + Input: + init = whether in the initialization or not + ''' + assert self.numCam >= 2 + + if not self.find_order: + return + + if init: + # if already have matching results, pick the pair of cameras that have most matches + if self.feature_dict: + max_num_matches = 0 + for k, v in self.feature_dict.items(): + match_res = self.feature_dict[k] + num_matches = np.count_nonzero(match_res+1,axis=1) + k2 = np.argmax(num_matches) + if num_matches[k2] > max_num_matches: + max_num_matches = num_matches[k2] + self.sequence = [k, k2] + else: + # take the first two cameras as initial pairs + self.sequence = [0,1] + else: + # get the candidate new cameras + candidate = [] + for i in range(self.numCam): + if self.cameras[i].P is None: + candidate.append(i) + + # find the next camera with the largest overlap + max_overlap = 0 + for cam_id in candidate: + num_overlap = 0 + overlap_ids = np.empty(0) + + # in case of superglue, find next camera based on the pre-computed matching result + if cam_id in self.feature_dict.keys(): + for i in self.sequence: + match_res = self.feature_dict[cam_id][i] + train_ids = match_res[match_res > -1] + _, registered_ids, _ = np.intersect1d(train_ids, self.cameras[i].index_registered_2d, return_indices=True) + + overlap_ids = np.union1d(overlap_ids, self.cameras[i].index_2d_3d[registered_ids]) + num_overlap = len(overlap_ids) + # initialize feature_dict + elif debug: + # use the ground truth matches + # initialize the matching result of this new camera to be stored to the dict + self.feature_dict[cam_id] = -np.ones((self.numCam, len(self.cameras[cam_id].gt_pts))) + # match between the features of this new camera and all other cameras + for i in self.sequence: + # all ground truth matches are mutually valid, directly populate with indices + self.feature_dict[cam_id][i] = np.arange(len(self.cameras[i].gt_pts)) + self.feature_dict[i][cam_id] = np.arange(len(self.cameras[cam_id].gt_pts)) + + # get the registered indices in 3d + overlap_ids = np.union1d(overlap_ids,self.cameras[i].index_2d_3d) + # sum the number of registered indices + num_overlap = len(overlap_ids) + else: + # get the features and descriptors of the new camera + kp1, des1 = self.cameras[cam_id].kp, self.cameras[cam_id].des + + # initilize the matching result of the new camera to be stored in feature_dict + match_res = -np.ones((self.numCam, len(kp1))) + # loop through all the cameras + for i in self.sequence: + # get the features of the old camera + kp2, des2 = self.cameras[i].kp, self.cameras[i].des + # match the features of the new camera and the old camera + _, _, matches, matchesMask = ep.matching_feature(kp1, kp2, des1, des2, ratio=0.8) + # draw matches + vis.draw_matches(self.cameras[cam_id].img, self.cameras[cam_id].kp, self.cameras[i].img, self.cameras[i].kp, matches, matchesMask) + + # get the matched indices + query_ids = np.array([m[0].queryIdx for m in matches]) + train_ids = np.array([m[0].trainIdx for m in matches]) + + # get the valid matches + match_ids = np.where(np.array(matchesMask)[:, 0] == 1)[0] + query_ids = query_ids[match_ids] + train_ids = train_ids[match_ids] + + # save the matching result to feature_dict + match_res[i, query_ids] = train_ids + self.feature_dict[cam_id] = match_res + self.feature_dict[i][cam_id, train_ids] = query_ids + + # find the indices of the old features that has been used + _, _, registered_ids = np.intersect1d(train_ids, self.cameras[i].index_registered_2d, return_indices=True) + # find the corresponding indices in 3D points + overlap_ids = np.union1d(overlap_ids, self.cameras[i].index_2d_3d[registered_ids]) + num_overlap = len(overlap_ids) + + if num_overlap > max_overlap: + next_cam = cam_id + + # add the next cam to the sequence + self.sequence.append(next_cam) + def all_detect_to_traj(self,*cam): #global_traj = np.empty() @@ -1032,10 +2073,61 @@ def time_shift(self, iter=False): beta[j], _ = sync_fun(self.cameras[i].fps, self.cameras[j].fps, self.detections[i], self.detections[j], self.cf[i], self.cf[j]) +# FIXME: threshold=20 print('Status: {} from {} cam finished'.format(j+1,self.numCam)) self.beta = beta self.beta_after_Fbeta = beta.copy() + + def time_shift_from_F(self, F): + ''' + This function computes relative time shifts of each camera to the ref camera using the given corresponding frame numbers + If the given frame indices are precise, then the time shifts are directly transformed from them. + ''' + + assert len(self.cf)==self.numCam, 'The number of frame indices should equal to the number of cameras' + + if self.settings['cf_exact']: + self.beta = self.cf[self.ref_cam] - self.alpha*self.cf + print('The given corresponding frames are directly exploited as temporal synchronization\n') + else: + + print('Computing temporal synchronization...\n') + beta = np.zeros(self.numCam) + i = self.ref_cam + for j in range(self.numCam): + if j==i: + beta[j] = 0 + else: + beta[j], _ = sync.sync_bf_from_F(self.cameras[i].fps, self.cameras[j].fps, + self.detections[i], self.detections[j], + self.cf[i], self.cf[j], F) + print('Status: {} from {} cam finished'.format(j+1,self.numCam)) + self.beta = beta + self.beta_after_Fbeta = beta.copy() + + def time_shift_from_pose(self): + ''' + This function computes relative time shifts of each camera to the ref camera using the given corresponding frame numbers + + If the given frame indices are precise, then the time shifts are directly transformed from them. + ''' + + assert len(self.cf)==self.numCam, 'The number of frame indices should equal to the number of cameras' + + print('Computing temporal synchronization...\n') + beta = np.zeros(self.numCam) + i = self.ref_cam + for j in range(self.numCam): + if j==i: + beta[j] = 0 + else: + beta[j], _ = sync.sync_bf_from_pose(self.cameras[i].fps, self.cameras[j].fps, + self.detections[i], self.detections[j], + self.cf[i], self.cf[j], self.cameras[i], self.cameras[j]) + print('Status: {} from {} cam finished'.format(j+1,self.numCam)) + self.beta = beta + self.beta_after_Fbeta = beta.copy() class Camera: """ @@ -1067,7 +2159,18 @@ def __init__(self,**kwargs): self.c = kwargs.get('c') self.fps = kwargs.get('fps') self.resolution = kwargs.get('resolution') - + + # information for the static part + self.img_path = kwargs.get('img_path') + self.img = None + self.kp = [] + self.des = [] + # the indices of the features used for 3D static point reconstruction + self.index_registered_2d = np.empty(0, dtype=int) + # the indices of the 3D static points that corresponds to the used feautures + self.index_2d_3d = np.empty(0, dtype=int) + # ground truth static matches for debugging + self.gt_pts = None def projectPoint(self,X): @@ -1142,9 +2245,25 @@ def vector2P(self, vector, calib=False): self.compose() return self.P + + def undist_point(self, points, method='opencv'): + ''' + Function: + A wrapper function for the undistort point function with different models + Input: + points = points to be undistorted + method = distortion model ['opencv', 'division'] + Output: + a sets of undistorted points + ''' + + if method == 'division': + return self.undist_point_div(points) + else: + return self.undist_point_opencv(points) - def undist_point(self,points): + def undist_point_opencv(self,points): assert points.shape[0]==2, 'Input must be a 2D array' @@ -1155,6 +2274,26 @@ def undist_point(self,points): dst_unnorm = np.dot(self.K, util.homogeneous(dst.reshape((num,2)).T)) return dst_unnorm[:2] + + def undist_point_div(self, points): + ''' + CURRENTLY NOT AVAILABLE YET + ''' + return points[:2] + + def dist_point3d(self, points, method='opencv'): + return self.dist_point3d_opencv(points) + + def dist_point3d_opencv(self, points): + pts_dist,_ = cv2.projectPoints(points, self.R, self.t, self.K, self.d) + return pts_dist.reshape(-1,2).T + + def dist_point2d(self, points, method='opencv'): + return self.dist_point2d_opencv(points) + + def dist_point2d_opencv(self, points): + pts_dist, _ = cv2.projectPoints(np.dot(np.linalg.inv(self.K), util.homogeneous(points)), np.eye(3), np.zeros((3,1)), self.K, self.d) + return pts_dist.reshape(-1,2).T def info(self): @@ -1166,7 +2305,45 @@ def info(self): print(self.R) print('\n t:') print(self.t) + + def read_img(self): + self.img = cv2.imread(self.img_path) + # self.img = cv2.cvtColor(self.img, cv2.COLOR_BGR2RGB) + + def extract_features(self, img=None, method='sift'): + ''' + Function: + extract features from the images and store the featues to the class + Input: + img = image of which features are to be extracted + (if None given, then extract the features of this camera) + method = feature extractor + ''' + if img is None: + self.read_img() + self.kp, self.des = ep.extract_SIFT_feature(self.img) + else: + print("extract features from given img") + self.kp, self.des = ep.extract_SIFT_feature(img) + def get_points(self, indices=None): + ''' + get the registered 2d points (2, n) + ''' + point_2d = np.empty([2, 0]) + if indices is not None: + return np.hstack([point_2d, np.array([self.kp[idx].pt for idx in indices]).T.reshape(2,-1)]) + return np.hstack([ + point_2d, + np.array([self.kp[idx].pt + for idx in self.index_registered_2d]).T.reshape(2, -1) + ]) + + def get_gt_pts(self): + ''' + get the registered 2d ground truth points (2, n) + ''' + return self.gt_pts[self.index_registered_2d].T def create_scene(path_input): ''' @@ -1189,10 +2366,22 @@ def create_scene(path_input): for i in path_detect: detect = np.loadtxt(i,usecols=(2,0,1))[:flight.settings['num_detections']].T flight.addDetection(detect) + + # Load superglue matches or ground truth + if 'include_static' in config['settings'].keys() and config['settings']['include_static']: + feature_method = config['settings']['feature_method'] + # if use superglue, load the preprocessed superpoints and matching result + if feature_method[0] == 'superglue': + try: + with open(feature_method[-1], 'rb') as f: + flight.feature_dict = pickle.load(f) + kpt_list = pickle.load(f) + except: + raise Exception('Failed to read superglue matching results') # Load cameras path_cam = config['necessary inputs']['path_cameras'] - for path in path_cam: + for i, path in enumerate(path_cam): try: with open(path, 'r') as file: cam = json.load(file) @@ -1201,9 +2390,29 @@ def create_scene(path_input): if len(cam['distCoeff']) == 4: cam['distCoeff'].append(0) + + # load camera information + camera = Camera(K=np.asfarray(cam['K-matrix']), d=np.asfarray(cam['distCoeff']), fps=cam['fps'], resolution=cam['resolution']) + # extract features + if 'include_static' in config['settings'].keys() and config['settings']['include_static']: + # if using sift as feature_method, also extract sift features + assert 'img_path' in cam.keys(), "Please specify the path to images" + camera.img_path = cam['img_path'] + if feature_method[0] == 'sift': + camera.extract_features(method=feature_method[0]) + elif feature_method[0] == 'superglue': + camera.read_img() + camera.kp = util.convert_kpts(kpt_list[i]) + else: + raise Exception("Unsupported feature extraction and matching method") + elif 'img_path' in cam.keys(): + camera.read_img() + + # load the ground truth static matches if given + if 'optional inputs' in config.keys() and 'static_ground_truth' in config['optional inputs'].keys(): + camera.gt_pts = np.loadtxt(config['optional inputs']['static_ground_truth'][i]) - flight.addCamera(Camera(K=np.asfarray(cam['K-matrix']), d=np.asfarray(cam['distCoeff']), - fps=cam['fps'], resolution=cam['resolution'])) + flight.addCamera(camera) # Load sequence flight.ref_cam = config['settings']['ref_cam'] @@ -1222,7 +2431,11 @@ def create_scene(path_input): flight.rs = np.asfarray([init_rs for i in range(flight.numCam)]) # Load ground truth setting (optinal) - flight.gt = config['optional inputs']['ground_truth'] + if 'optional inputs' in config.keys(): + if 'ground_truth' in config['optional inputs'].keys(): + flight.gt = config['optional inputs']['ground_truth'] + if 'static_ground_truth_3d' in config['optional inputs'].keys(): + flight.gt_static = np.loadtxt(config['optional inputs']['static_ground_truth_3d']) print('Input data are loaded successfully, a scene is created.\n') return flight diff --git a/multiviewunsynch/reconstruction/epipolar.py b/multiviewunsynch/reconstruction/epipolar.py index 9ec21a6..f68613e 100644 --- a/multiviewunsynch/reconstruction/epipolar.py +++ b/multiviewunsynch/reconstruction/epipolar.py @@ -7,6 +7,7 @@ import cv2 from tools import ransac from tools import visualization as vis +from tools import util from scipy.optimize import least_squares, root @@ -89,7 +90,7 @@ def matching_feature(kp1, kp2, des1, des2, method=1, ratio=0.7): return pts1, pts2, matches -def computeFundamentalMat(pts1, pts2, method=cv2.FM_RANSAC, error=3, inliers=True): +def computeFundamentalMat(pts1, pts2, method=cv2.USAC_MAGSAC, error=3, inliers=True): ''' Function: compute fundamental matrix given correspondences (at least 8) @@ -98,6 +99,7 @@ def computeFundamentalMat(pts1, pts2, method=cv2.FM_RANSAC, error=3, inliers=Tru method = cv2.FM_RANSAC: Using RANSAC algorithm (default) cv2.FM_LMEDS: Using least-median algorithm cv2.FM_8POINT: Using 8 points algorithm + cv2.USAC_MAGSAC: Using MAGSAC++ error = reprojection threshold that describes maximal distance from a point to a epipolar line inlier = True: return F and the mask for inliers @@ -508,6 +510,32 @@ def triangulate_matlab(x1,x2,P1,P2): X[:,i] = V[-1]/V[-1,-1] return X + +def triangulate_matlab_mv(xs, Ps): + ''' + triangulate multiple points in multiple views + Input: + xs = (nViews*2)xN + Ps = a vector of projection matrices + ''' + num_views = len(Ps) + X = np.zeros((4, xs.shape[1])) + for i in range(xs.shape[1]): + pts = np.split(xs[:,i], num_views) + A = [] + for j, pt in enumerate(pts): + P = Ps[j] + r1 = pt[0]*P[2] - P[0] + r2 = pt[1]*P[2] - P[1] + A.append(r1) + A.append(r2) + A = np.vstack(A) + U,S,V = np.linalg.svd(A) + X[:,i] = V[-1]/V[-1,-1] + + return X + + def compute_Rt_from_E(E): @@ -639,6 +667,113 @@ def dist_model(x,*arg): def reprojection_error(x,x_p): return np.sqrt((x[0]-x_p[0])**2 + (x[1]-x_p[1])**2) +def epipolar_pipeline(d1, d2, K1, K2, error, inlier_only, img1, img2): + ''' + Function: + Basic epipolar pipeline that goes from the matching pairs to the E esitmation and triangulation of 3D points + Input: + d1, d2: = matched keypoints in two images + K1, K2 = intrinsics of the two cameras + error = error threshold for calculating fundamental matrix + inlier_only = if only using inliers for fundamental matrix computation + img1, img2 = two images + Output: + X = the triangulated 3D points from the matched pairs + P = the projection of the second camera + inlier = the inlier mask of the matched pairs used for fundamental matrix computation + mask = mask of the 2D keypoints used for triangulation + ''' + + # Compute fundamental matrix with the given error theshold + F, inlier = computeFundamentalMat(d1, d2, error=error) + + # use very small error threshold + # F, inlier = computeFundamentalMat(d1, d2, error=1) + + # use 8-point algorithm + # F, inlier = computeFundamentalMat(d1, d2, error=error, method=cv2.FM_8POINT) + + # use least square on all points + # F = compute_fundamental(d1, d2) + # inlier = np.ones(d1.shape[1]) + + # print(inlier) + E = np.dot(np.dot(K2.T, F), K1) + + if not inlier_only: + inlier = np.ones(len(inlier)) + x1, x2 = util.homogeneous(d1[:, inlier == 1]), util.homogeneous( + d2[:, inlier == 1]) + # vis.plot_epipolar_line(img1[:,:,0], img2[:,:,0], F, x1, x2) + + # Find corrected corresponding points for optimal triangulation + N = d1[:, inlier == 1].shape[1] + pts1 = d1[:, inlier == 1].T.reshape(1, -1, 2) + pts2 = d2[:, inlier == 1].T.reshape(1, -1, 2) + m1, m2 = cv2.correctMatches(F, pts1, pts2) + x1, x2 = util.homogeneous(np.reshape(m1, (-1, 2)).T), util.homogeneous( + np.reshape(m2, (-1, 2)).T) + + mask = np.logical_not(np.isnan(x1[0])) + x1 = x1[:, mask] + x2 = x2[:, mask] + + # print(img1[:,:,0].shape) + + # vis.plotEpiline(img1[:,:,0], img2[:,:,0], np.int32(d1[:,inlier==1]).T, np.int32(d2[:,inlier==1]).T, F) + + # Triangulte points + X, P = triangulate_from_E(E, K1, K2, x1, x2) + + return X, P, inlier, mask + +def epipolar_pipeline_from_F(d1, d2, K1, K2, F, inlier_only=False,thres=5): + ''' + Function: + Basic epipolar pipeline that goes from the matching pairs to the E esitmation and triangulation of 3D points + Input: + d1, d2: = matched keypoints in two images + K1, K2 = intrinsics of the two cameras + error = error threshold for calculating fundamental matrix + inlier_only = if only using inliers for fundamental matrix computation + img1, img2 = two images + Output: + X = the triangulated 3D points from the matched pairs + P = the projection of the second camera + inlier = the inlier mask of the matched pairs used for fundamental matrix computation + mask = mask of the 2D keypoints used for triangulation + ''' + + # Compute sampson error and filter out outliers + if inlier_only: + serr = Sampson_error(util.homogeneous(d1),util.homogeneous(d2),F) + inlier = np.zeros(len(serr)) + inlier[serr < thres] = 1 + else: + inlier = np.ones(d1.shape[1]) + # print(inlier) + E = np.dot(np.dot(K2.T, F), K1) + # F = np.dot(np.linalg.inv(K2).T, np.dot(E, np.linalg.inv(K1))) + + x1, x2 = util.homogeneous(d1[:, inlier == 1]), util.homogeneous(d2[:, inlier == 1]) + # vis.plot_epipolar_line(img1[:,:,0], img2[:,:,0], F, x1, x2) + + # Find corrected corresponding points for optimal triangulation + N = d1[:, inlier == 1].shape[1] + pts1 = d1[:, inlier == 1].T.reshape(1, -1, 2) + pts2 = d2[:, inlier == 1].T.reshape(1, -1, 2) + m1, m2 = cv2.correctMatches(F, pts1, pts2) + x1, x2 = util.homogeneous(np.reshape(m1, (-1, 2)).T), util.homogeneous(np.reshape(m2, (-1, 2)).T) + + mask = np.logical_not(np.isnan(x1[0])) + x1 = x1[:, mask] + x2 = x2[:, mask] + + # Triangulte points + X, P = triangulate_from_E(E, K1, K2, x1, x2) + + return X, P, inlier, mask + if __name__ == "__main__": diff --git a/multiviewunsynch/reconstruction/synchronization.py b/multiviewunsynch/reconstruction/synchronization.py index bca9477..59e4149 100644 --- a/multiviewunsynch/reconstruction/synchronization.py +++ b/multiviewunsynch/reconstruction/synchronization.py @@ -173,6 +173,105 @@ def search(beta_list, thres=8): return beta, overlap_second +def sync_bf_from_F(fps1, fps2, detect1, detect2, frame1, frame2, F, r=10): + ''' + Brute-force method for temporal synchronization of two series of detections + + r is the half length of the search interval in unit second + + Function returns both the time shift (beta) and the temporal overlap of the two series of detections in unit second + ''' + + + def search(beta_list, thres=5): + maxInlier = 0 + beta_est = 0 + for beta in beta_list: + detect2_temp = np.vstack((detect2[0]+beta,detect2[1:])) + pts1, pts2 = util.match_overlap(detect1_temp, detect2_temp) + + serr = epipolar.Sampson_error(util.homogeneous(pts1[1:]), util.homogeneous(pts2[1:]), F) + + inlier = np.sum(serr < thres) + + if inlier > maxInlier: + maxInlier = inlier + beta_est = beta + + return beta_est, maxInlier + + + # Pre-processing + alpha = fps1 / fps2 + detect1_temp = np.vstack((detect1[0]/alpha,detect1[1:])) + beta_prior = frame1/alpha - frame2 + + # Two-stage search + beta_coarse = np.arange(beta_prior-r*fps2, beta_prior+r*fps2, fps2/10) + beta_est, _ = search(beta_coarse) + beta_fine = np.arange(beta_est-fps2/2, beta_est+fps2/2, fps2/20) + beta_est, numInlier = search(beta_fine) + + # Result + beta = beta_est * alpha + overlap_second = numInlier/fps1 + + return beta, overlap_second + +def sync_bf_from_pose(fps1, fps2, detect1, detect2, frame1, frame2, cam1, cam2, r=10): + ''' + Brute-force method for temporal synchronization of two series of detections + + r is the half length of the search interval in unit second + + Function returns both the time shift (beta) and the temporal overlap of the two series of detections in unit second + ''' + + + def search(beta_list, thres=5): + maxInlier = 0 + beta_est = 0 + minErr = np.inf + for beta in beta_list: + detect2_temp = np.vstack((detect2[0]+beta,detect2[1:])) + pts1, pts2 = util.match_overlap(detect1_temp, detect2_temp) + + X = epipolar.triangulate_matlab(pts1[1:], pts2[1:], cam1.P, cam2.P) + + err1 = epipolar.reprojection_error(pts1[1:], cam1.projectPoint(X)) + err2 = epipolar.reprojection_error(pts2[1:], cam2.projectPoint(X)) + + + # serr = epipolar.Sampson_error(util.homogeneous(pts1[1:]), util.homogeneous(pts2[1:]), F) + + # inlier = np.sum((err1 < thres) & (err2 < thres)) + + # if inlier > maxInlier: + # maxInlier = inlier + # beta_est = beta + mean_err = np.mean(err1)+np.mean(err2) + if mean_err < minErr: + best_est = beta + + return beta_est, maxInlier + + + # Pre-processing + alpha = fps1 / fps2 + detect1_temp = np.vstack((detect1[0]/alpha,detect1[1:])) + beta_prior = frame1/alpha - frame2 + + # Two-stage search + beta_coarse = np.arange(beta_prior-r*fps2, beta_prior+r*fps2, fps2) + beta_est, _ = search(beta_coarse) + beta_fine = np.arange(beta_est-fps2/2, beta_est+fps2/2, fps2/1000) + beta_est, numInlier = search(beta_fine) + + # Result + beta = beta_est * alpha + overlap_second = numInlier/fps1 + + return beta, overlap_second if __name__ == "__main__": diff --git a/multiviewunsynch/thirdparty/camera_calibration_show_extrinsics.py b/multiviewunsynch/thirdparty/camera_calibration_show_extrinsics.py new file mode 100755 index 0000000..ab102b3 --- /dev/null +++ b/multiviewunsynch/thirdparty/camera_calibration_show_extrinsics.py @@ -0,0 +1,235 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +# Python 2/3 compatibility +from __future__ import print_function + +import numpy as np +import cv2 as cv + +from numpy import linspace + +def inverse_homogeneoux_matrix(M): + R = M[0:3, 0:3] + T = M[0:3, 3] + M_inv = np.identity(4) + M_inv[0:3, 0:3] = R.T + M_inv[0:3, 3] = -(R.T).dot(T) + + return M_inv + +def transform_to_matplotlib_frame(cMo, X, inverse=False): + M = np.identity(4) + # M[1,1] = 0 + # M[1,2] = 1 + # M[2,1] = -1 + # M[2,2] = 0 + + if inverse: + return M.dot(inverse_homogeneoux_matrix(cMo).dot(X)) + else: + return M.dot(cMo.dot(X)) + +def create_camera_model(camera_matrix, width, height, scale_focal, draw_frame_axis=False): + fx = camera_matrix[0,0] + fy = camera_matrix[1,1] + focal = 2 / (fx + fy) + # f_scale = scale_focal * focal + f_scale = scale_focal + width *= focal*scale_focal + height *= focal*scale_focal + + # draw image plane + X_img_plane = np.ones((4,5)) + X_img_plane[0:3,0] = [-width, height, f_scale] + X_img_plane[0:3,1] = [width, height, f_scale] + X_img_plane[0:3,2] = [width, -height, f_scale] + X_img_plane[0:3,3] = [-width, -height, f_scale] + X_img_plane[0:3,4] = [-width, height, f_scale] + + # draw triangle above the image plane + X_triangle = np.ones((4,3)) + X_triangle[0:3,0] = [-width, -height, f_scale] + X_triangle[0:3,1] = [0, -2*height, f_scale] + X_triangle[0:3,2] = [width, -height, f_scale] + + # draw camera + X_center1 = np.ones((4,2)) + X_center1[0:3,0] = [0, 0, 0] + X_center1[0:3,1] = [-width, height, f_scale] + + X_center2 = np.ones((4,2)) + X_center2[0:3,0] = [0, 0, 0] + X_center2[0:3,1] = [width, height, f_scale] + + X_center3 = np.ones((4,2)) + X_center3[0:3,0] = [0, 0, 0] + X_center3[0:3,1] = [width, -height, f_scale] + + X_center4 = np.ones((4,2)) + X_center4[0:3,0] = [0, 0, 0] + X_center4[0:3,1] = [-width, -height, f_scale] + + # draw camera frame axis + X_frame1 = np.ones((4,2)) + X_frame1[0:3,0] = [0, 0, 0] + X_frame1[0:3,1] = [f_scale/2, 0, 0] + + X_frame2 = np.ones((4,2)) + X_frame2[0:3,0] = [0, 0, 0] + X_frame2[0:3,1] = [0, f_scale/2, 0] + + X_frame3 = np.ones((4,2)) + X_frame3[0:3,0] = [0, 0, 0] + X_frame3[0:3,1] = [0, 0, f_scale/2] + + # if draw_frame_axis: + # return [X_img_plane, X_triangle, X_center1, X_center2, X_center3, X_center4, X_frame1, X_frame2, X_frame3] + # else: + # return [X_img_plane, X_triangle, X_center1, X_center2, X_center3, X_center4] + + if draw_frame_axis: + return [X_img_plane, X_center1, X_center2, X_center3, X_center4, X_frame1, X_frame2, X_frame3] + else: + return [X_img_plane, X_center1, X_center2, X_center3, X_center4] + +def create_board_model(extrinsics, board_width, board_height, square_size, draw_frame_axis=False): + width = board_width*square_size + height = board_height*square_size + + # draw calibration board + X_board = np.ones((4,5)) + #X_board_cam = np.ones((extrinsics.shape[0],4,5)) + X_board[0:3,0] = [0,0,0] + X_board[0:3,1] = [width,0,0] + X_board[0:3,2] = [width,height,0] + X_board[0:3,3] = [0,height,0] + X_board[0:3,4] = [0,0,0] + + # draw board frame axis + X_frame1 = np.ones((4,2)) + X_frame1[0:3,0] = [0, 0, 0] + X_frame1[0:3,1] = [height/2, 0, 0] + + X_frame2 = np.ones((4,2)) + X_frame2[0:3,0] = [0, 0, 0] + X_frame2[0:3,1] = [0, height/2, 0] + + X_frame3 = np.ones((4,2)) + X_frame3[0:3,0] = [0, 0, 0] + X_frame3[0:3,1] = [0, 0, height/2] + + if draw_frame_axis: + return [X_board, X_frame1, X_frame2, X_frame3] + else: + return [X_board] + +def draw_camera_boards(ax, camera_matrix, cam_width, cam_height, scale_focal, + extrinsics, board_width, board_height, square_size, + patternCentric): + from matplotlib import cm + + min_values = np.zeros((3,1)) + min_values = np.inf + max_values = np.zeros((3,1)) + max_values = -np.inf + + if patternCentric: + X_moving = create_camera_model(camera_matrix, cam_width, cam_height, scale_focal) + X_static = create_board_model(extrinsics, board_width, board_height, square_size) + else: + X_static = create_camera_model(camera_matrix, cam_width, cam_height, scale_focal, True) + X_moving = create_board_model(extrinsics, board_width, board_height, square_size) + + cm_subsection = linspace(0.0, 1.0, extrinsics.shape[0]) + colors = [ cm.jet(x) for x in cm_subsection ] + + for i in range(len(X_static)): + X = np.zeros(X_static[i].shape) + for j in range(X_static[i].shape[1]): + X[:,j] = transform_to_matplotlib_frame(np.eye(4), X_static[i][:,j]) + ax.plot3D(X[0,:], X[1,:], X[2,:], color='r') + min_values = np.minimum(min_values, X[0:3,:].min(1)) + max_values = np.maximum(max_values, X[0:3,:].max(1)) + + for idx in range(extrinsics.shape[0]): + R, _ = cv.Rodrigues(extrinsics[idx,0:3]) + cMo = np.eye(4,4) + cMo[0:3,0:3] = R + cMo[0:3,3] = extrinsics[idx,3:6] + for i in range(len(X_moving)): + X = np.zeros(X_moving[i].shape) + for j in range(X_moving[i].shape[1]): + X[0:4,j] = transform_to_matplotlib_frame(cMo, X_moving[i][0:4,j], patternCentric) + ax.plot3D(X[0,:], X[1,:], X[2,:], color=colors[idx]) + min_values = np.minimum(min_values, X[0:3,:].min(1)) + max_values = np.maximum(max_values, X[0:3,:].max(1)) + + return min_values, max_values + +def main(): + import argparse + + parser = argparse.ArgumentParser(description='Plot camera calibration extrinsics.', + formatter_class=argparse.ArgumentDefaultsHelpFormatter) + parser.add_argument('--calibration', type=str, default='left_intrinsics.yml', + help='YAML camera calibration file.') + parser.add_argument('--cam_width', type=float, default=0.064/2, + help='Width/2 of the displayed camera.') + parser.add_argument('--cam_height', type=float, default=0.048/2, + help='Height/2 of the displayed camera.') + parser.add_argument('--scale_focal', type=float, default=40, + help='Value to scale the focal length.') + parser.add_argument('--patternCentric', action='store_true', + help='The calibration board is static and the camera is moving.') + args = parser.parse_args() + + fs = cv.FileStorage(cv.samples.findFile(args.calibration), cv.FILE_STORAGE_READ) + board_width = int(fs.getNode('board_width').real()) + board_height = int(fs.getNode('board_height').real()) + square_size = fs.getNode('square_size').real() + camera_matrix = fs.getNode('camera_matrix').mat() + extrinsics = fs.getNode('extrinsic_parameters').mat() + + import matplotlib.pyplot as plt + from mpl_toolkits.mplot3d import Axes3D # pylint: disable=unused-variable + + fig = plt.figure() + ax = fig.gca(projection='3d') + ax.set_aspect("equal") + + cam_width = args.cam_width + cam_height = args.cam_height + scale_focal = args.scale_focal + min_values, max_values = draw_camera_boards(ax, camera_matrix, cam_width, cam_height, + scale_focal, extrinsics, board_width, + board_height, square_size, args.patternCentric) + + X_min = min_values[0] + X_max = max_values[0] + Y_min = min_values[1] + Y_max = max_values[1] + Z_min = min_values[2] + Z_max = max_values[2] + max_range = np.array([X_max-X_min, Y_max-Y_min, Z_max-Z_min]).max() / 2.0 + + mid_x = (X_max+X_min) * 0.5 + mid_y = (Y_max+Y_min) * 0.5 + mid_z = (Z_max+Z_min) * 0.5 + ax.set_xlim(mid_x - max_range, mid_x + max_range) + ax.set_ylim(mid_y - max_range, mid_y + max_range) + ax.set_zlim(mid_z - max_range, mid_z + max_range) + + ax.set_xlabel('x') + ax.set_ylabel('z') + ax.set_zlabel('-y') + ax.set_title('Extrinsic Parameters Visualization') + + plt.show() + print('Done') + + +if __name__ == '__main__': + print(__doc__) + main() + cv.destroyAllWindows() diff --git a/multiviewunsynch/tools/util.py b/multiviewunsynch/tools/util.py index 3f0ece5..e4990cd 100644 --- a/multiviewunsynch/tools/util.py +++ b/multiviewunsynch/tools/util.py @@ -7,6 +7,7 @@ from scipy import interpolate from thirdparty import transformation from tools import ransac +import cv2 # from numba import jit @@ -55,7 +56,7 @@ def homogeneous(x): return np.vstack((x,np.ones(x.shape[1]))) # @jit -def find_intervals(x,gap=5,idx=False): +def find_intervals(x,gap=15,idx=False): ''' Given indices of detections, return a matrix that contains the start and the end of each continues part. @@ -205,6 +206,23 @@ def umeyama(src, dst, estimate_scale): return T +def convert_kpts(kpts): + ''' + Function: + convert superglue keypoints to cv.KeyPoint format + Input: + kpts = a numpy array of superglue keypoints (n,2) + Output: + kpts_cv = a list of cv.KeyPoint features + ''' + return [cv2.KeyPoint(k[0],k[1],8) for k in kpts] + +def unpack_sift_kp(kpts): + ''' + Function: + unpack sift keypoints + ''' + return np.array([kp.pt for kp in kpts]) if __name__ == "__main__": R = rotation(0.38,-176.3,100) diff --git a/multiviewunsynch/tools/visualization.py b/multiviewunsynch/tools/visualization.py index 3e6f0de..e6e91dc 100644 --- a/multiviewunsynch/tools/visualization.py +++ b/multiviewunsynch/tools/visualization.py @@ -7,7 +7,11 @@ from tools import util import pickle from matplotlib import pyplot as plt +from matplotlib import cm from mpl_toolkits.mplot3d import Axes3D +from thirdparty.camera_calibration_show_extrinsics import create_camera_model, transform_to_matplotlib_frame +from .util import homogeneous +import os def drawlines(img1,img2,lines,pts1,pts2): ''' @@ -22,7 +26,7 @@ def drawlines(img1,img2,lines,pts1,pts2): img1 = image1 with lines and circles img2 = image2 with circles ''' - r,c = img1.shape + r,c = img1.shape[:2] # Convert grayscale to BGR if needed if len(img1.shape)==2: @@ -52,6 +56,8 @@ def plotEpiline(img1, img2, pts1, pts2, F): Output: plot epipolar lines on image pair ''' + img1 = img1.copy() + img2 = img2.copy() # Find epilines corresponding to points in right image (second image) and # drawing its lines on left image lines1 = cv2.computeCorrespondEpilines(pts2.reshape(-1,1,2), 2,F) @@ -65,24 +71,28 @@ def plotEpiline(img1, img2, pts1, pts2, F): img5 = drawlines(img2,img1,lines2,pts2,pts1)[0] # Show results - cv2.namedWindow('Epipolar lines in img1',cv2.WINDOW_NORMAL) - cv2.resizeWindow('Epipolar lines in img1',500,300) - cv2.imshow('Epipolar lines in img1',img3) - cv2.namedWindow('Epipolar lines in img2',cv2.WINDOW_NORMAL) - cv2.resizeWindow('Epipolar lines in img2',500,300) - cv2.imshow('Epipolar lines in img2',img5) - cv2.waitKey(0) + # cv2.namedWindow('Epipolar lines in img1',cv2.WINDOW_NORMAL) + # cv2.resizeWindow('Epipolar lines in img1',500,300) + # cv2.imshow('Epipolar lines in img1',img3) + cv2.imwrite('epipolar_lines_img1.png', img3) + # cv2.namedWindow('Epipolar lines in img2',cv2.WINDOW_NORMAL) + # cv2.resizeWindow('Epipolar lines in img2',500,300) + # cv2.imshow('Epipolar lines in img2',img5) + cv2.imwrite('epipolar_lines_img2.png', img5) + + # cv2.waitKey(0) def plot_epipolar_line(img1, img2, F, x1, x2): ''' Plot epipolar lines of point correspondences. ''' - + img1 = cv2.cvtColor(img1, cv2.COLOR_BGR2RGB) + img2 = cv2.cvtColor(img2, cv2.COLOR_BGR2RGB) # pre-steps num = x1.shape[1] - r1,c1 = img1.shape - r2,c2 = img2.shape + r1,c1 = img1.shape[:2] + r2,c2 = img2.shape[:2] x1_coord = np.linspace(0,c1-1,100) x2_coord = np.linspace(0,c2-1,100) @@ -92,7 +102,7 @@ def plot_epipolar_line(img1, img2, F, x1, x2): # plot epipolar lines in img1, which are calculated using key points in img2 plt.subplot(121),plt.imshow(img1,cmap='gray') - for i in range(num): + for i in range(x2.shape[1]): line = line2[:,i] y1_coord = np.array([(line[2]+line[0]*x)/(-line[1]) for x in x1_coord]) idx = (y1_coord>=0) & (y1_coord=0) & (y2_coord 0: + # plot reprojection with vector + assert num % 2 == 0, 'incomplete input pairs' + + for i in range(num // 2): + # plot base point + plt.scatter(x[2*i][0], x[2*i][1], c=c[2*i], marker=m[2*i], label=label[2*i], s=50) + # plot vector between base point and target + plt.quiver(x[2*i][0], x[2*i][1], vec*(x[2*i+1][0]-x[2*i][0]), vec*(x[2*i+1][1]-x[2*i][1]), color=c[2*i+1], angles='xy', scale_units='xy', scale=1, width=1e-2) + else: + for i in range(num): + # plt.subplot(1,num,i+1) + if len(label) == 0: + label = ['Raw points', 'Reconstruction points','Raw detections', 'Reconstructed trajectories'] + if color: + plt.scatter(x[i][0],x[i][1],c=c[i],marker=m[i],label=label[i]) + else: + plt.scatter(x[i][0],x[i][1],c=c[i]) + # plt.scatter(x[i][0],x[i][1],c=np.arange(x[i].shape[1])*color) + if line: + plt.plot(x[i][0],x[i][1]) + if text: + for j in range(len(x[i][0])): + plt.text(x[i][0,j], x[i][1,j], str(j), color='red',fontsize=12) + + # plt.gca().set_xlim([0,1920]) + # plt.gca().set_ylim([0,1080]) + # plt.gca().invert_yaxis() + + plt.xlabel('X') + plt.ylabel('Y') + + # if cam_center is not None and cam_center.shape[0] == 2 and cam_center.shape[1] > 0: + # # plot camera position + # plt.scatter(cam_center[0], cam_center[1], c='r',marker='*', label='Camera center') + + # plt.legend(loc=1) + plt.axis('off') - plt.xlabel('X') - plt.ylabel('Y') if title: - plt.suptitle(title) - plt.show() + # plt.suptitle(title) + plt.savefig(os.path.join(output_dir,title+'.png'),bbox_inches='tight',pad_inches = 0) + else: + plt.savefig(os.path.join(output_dir,'reprojected.png'),bbox_inches='tight',pad_inches = 0) + # plt.show() + + +def draw_camera_extrinsics(flight, ax, scale_focal=40): + colors = [ cm.jet(x) for x in 100*np.random.rand(flight.numCam)] + # loop through all the cameras + for i, cam in enumerate(flight.cameras): + # width and height of the camera + cam_height, cam_width, _ = cam.img.shape + # get the camera frame model + X_cam_model = create_camera_model(cam.K, cam_width/2, cam_height/2, scale_focal, draw_frame_axis=True) + cMo = np.eye(4) + cMo[:3,:3] = cam.R + cMo[:3,-1] = cam.t + + # print(len(X_cam_model)) + + for k, X_cam_part in enumerate(X_cam_model): + X = np.zeros_like(X_cam_part) + for j in range(X_cam_part.shape[1]): + X[0:4,j] = transform_to_matplotlib_frame(cMo, X_cam_part[0:4,j], True) + if len(X_cam_model) == 8 and k == 5: + ax.plot3D(X[0,:], X[1,:], X[2,:], color='r') + elif len(X_cam_model) == 8 and k == 6: + ax.plot3D(X[0,:], X[1,:], X[2,:], color='g') + elif len(X_cam_model) == 8 and k == 7: + ax.plot3D(X[0,:], X[1,:], X[2,:], color='b') + else: + ax.plot3D(X[0,:], X[1,:], X[2,:], color=colors[i]) + + C_cam = np.dot(-cam.R.T, cam.t.reshape(-1,1)).ravel() + + ax.scatter3D(C_cam[0], C_cam[1], C_cam[2], c=colors[i]) + ax.text(C_cam[0], C_cam[1], C_cam[2], 'Camera '+str(i), color=colors[i]) -def show_3D_all(*X,title=None,color=True,line=True): +def show_3D_all(*X, title=None,color=True,line=True,flight=None, output_dir='',label=[]): fig = plt.figure(figsize=(20, 15)) num = len(X) ax = fig.add_subplot(111,projection='3d') + # ax.set_zlim(-10,10) + # ax.set_xlim(-10,10) + # ax.set_ylim(-10,10) + for i in range(num): if color: - c = ['r','g'] - m = ['o','x'] - label = ['RTK ground truth', 'Reconstruction Spline'] + c = ['r','b','orange','g'] + m = ['o','x','o','+'] + if len(label) == 0: + label = ['RTK ground truth', 'Reconstruction Spline'] if i is 0: - ax.scatter3D(X[i][0],X[i][1],X[i][2],s=60,c=c[i],marker='o',label=label[i]) + # ax.scatter3D(X[i][0],X[i][1],X[i][2],s=60,c=c[i],marker='o',label=label[i]) + ax.scatter3D(X[i][0],X[i][1],X[i][2],s=60,c=c[i%len(c)],marker=m[i%len(m)],label=label[i%len(label)]) else: - ax.scatter3D(X[i][0],X[i][1],X[i][2],s=60,c=c[i],marker=m[i],label=label[i]) + ax.scatter3D(X[i][0],X[i][1],X[i][2],s=60,c=c[i%len(c)],marker=m[i%len(m)],label=label[i%len(label)]) # ax.plot(X[i][0],X[i][1],X[i][2],c=c[i]) else: @@ -205,22 +294,42 @@ def show_3D_all(*X,title=None,color=True,line=True): ax.plot(X[i][0],X[i][1],X[i][2]) plt.xlabel('X') plt.ylabel('Y') + + # if the flight is provided, also draw the cameras and the reconstructed trajectories + if flight is not None: + draw_camera_extrinsics(flight, ax, scale_focal=.5) + # if exists trajectory, also plot it + if len(flight.traj) > 0: + if color: + ax.scatter3D(flight.traj[1], flight.traj[2], flight.traj[3], c=np.arange(flight.traj.shape[1])*color) + else: + ax.scatter3D(flight.traj[1], flight.traj[2], flight.traj[3]) + if line: + ax.plot(flight.traj[1], flight.traj[2], flight.traj[3]) + if title: plt.suptitle(title) - ax.set_xlabel('East [m]',fontsize=20) - ax.set_ylabel('North [m]',fontsize=20) - ax.set_zlabel('Up [m]',fontsize=20) + # ax.set_xlabel('East [m]',fontsize=20) + # ax.set_ylabel('North [m]',fontsize=20) + # ax.set_zlabel('Up [m]',fontsize=20) + ax.set_xlabel('X',fontsize=20) + ax.set_ylabel('Y',fontsize=20) + ax.set_zlabel('Z',fontsize=20) ax.view_init(elev=30,azim=-50) - lgnd = ax.legend(loc=1, prop={'size': 30}) + lgnd = ax.legend(loc=8, prop={'size': 15}) for handle in lgnd.legendHandles: handle.set_sizes([100]) # plt.axis('off') + if title: + plt.savefig(os.path.join(output_dir,title+'.png')) + else: + plt.savefig(os.path.join(output_dir,'reconstructed_scene.png')) plt.show() -def show_spline(*spline,title=None): +def show_spline(*spline, title=None): num = len(spline) for i in range(num): @@ -287,6 +396,113 @@ def error_traj(traj,error,thres=0.5,title=None,colormap='Wistia',size=100, text= plt.title(title, fontsize=50) plt.show() +def error_boxplot(err, labels=[], title=None, ax=None, show_outliers=False, output_dir=''): + assert len(labels) == len(err), "The length of labels should be consistent with the length of the err vector" + + if ax is None: + fig, ax = plt.subplots(figsize=(50,15),sharey=True) + + ax.boxplot(err, labels=labels, showfliers=show_outliers) + + if title is not None: + ax.set_title(title) + plt.savefig(os.path.join(output_dir,title)) + else: + plt.savefig(os.path.join(output_dir,'error_boxplot.png')) + return ax + +def error_histogram(*errs, num_cams=2, labels=[], title=None, ax=None, xlim=40, num_bins=40, ylim=350, bin_width=0.1): + assert len(labels) == len(errs), "The length of labels should be consistent with the length of the err vector" + + if ax is None: + fig, ax = plt.subplots(sharex=True, sharey=True, dpi=300) + + # if xlim > 100: + # bins = np.arange(0,100,1) + # bins[-1] = xlim + # else: + # bins = np.arange(0,np.ceil(xlim),1) + + # for err, label in zip(errs,labels): + # print(label) + # ax.hist(err, bins=bins, label=label, alpha=0.5) + # # if ax.get_ylim()[-1] < np.max(n): + # # ax.set_ylim([0,np.max(n)+0.75]) + + bins = np.arange(0, xlim+1, (xlim+1)//num_bins) + + for i, (err, label) in enumerate(zip(errs, labels)): + h, _ = np.histogram(np.clip(err, bins[0], bins[-1]), bins=bins) + ax.bar(bins[:-1]+(i-1)*bin_width, h, bin_width, label=label, align='center') + # h, bins, patches = ax.hist([np.clip(err, bins[0], bins[-1]) for err in errs], bins=bins, range=(0,80), label=labels, alpha=.8) + # ax.set_ylim([0,0.8]) + + + # xlabels = bins[1:].astype(str) + # xlabels[-1] += '+' + # ax.set_xlim([0,xlim]) + # ax.set_xticks(np.arange(len(xlabels))+0.5) + # ax.set_xticklabels(xlabels) + + # ax.set_xlim([0, xlim]) + # loc = ax.get_xticks() + # xlabels = ["{:.0f}".format(x) for x in loc] + # loc[-1] = xlim+0.5 + # xlabels[-1] = "{:.0f}+".format(xlim) + # xlabels.append(str(xlim)+'+') + # ax.set_xticks(loc) + # ax.set_xticklabels(xlabels) + + if title is not None: + ax.set_title(title) + + ax.legend() + + return ax, bins[:-1] + + + +def draw_detection_matches(img1, d1, img2, d2, title='detection_mathches.png', output_dir=''): + ''' + Function: + Draw the corresponding detections in the camera views + Input: + img1, img2 = two images + d1, d2 = the matched detections + ''' + fig = plt.figure() + dp1 = [cv2.KeyPoint(d[1], d[2], 8) for d in d1.T] + dp2 = [cv2.KeyPoint(d[1], d[2], 8) for d in d2.T] + # print(dp1) + # print(dp2) + matches = [cv2.DMatch(i, i, 0) for i in range(len(dp1))] + + # outimg = cv2.drawMatches(img1, dp1, img2, dp2, matches, None, matchColor=(0,255,0)) + outimg = cv2.drawMatches(img1, dp1, img2, dp2, matches, None) + + + plt.imshow(outimg), plt.show() + cv2.imwrite(os.path.join(output_dir,title), outimg) + +def draw_matches(img1, kp1, img2, kp2, matches, matchesMask): + ''' + Function: + Draw the matching results (FLANN) of the two sets of keypoints + Input: + img1, img2 = two images + kp1, kp2 = two matched keypoints + matches = the matcher object returned from FLANN matcher + matchesMask = index of good matches + ''' + fig = plt.figure() + draw_params = dict(matchColor=(0, 255, 0), + singlePointColor=(255, 0, 0), + matchesMask=matchesMask, + flags=cv2.DrawMatchesFlags_DEFAULT) + out_img1 = cv2.drawMatchesKnn(img1, kp1, img2, kp2, matches, None, **draw_params) + cv2.imwrite('sift_match.png', out_img1) + plt.imshow(out_img1), plt.show() + if __name__ == "__main__": # # Synthetic trajectory data