Include static part into the pipeline

.gitignore

@@ -125,3 +125,7 @@ dmypy.json
 
 # Datas
 data/
+experiments/
+multiviewunsynch/drone-tracking-datasets
+multiviewunsynch/*.png
+multiviewunsynch/*-datasets.png

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__":
 

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

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)

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)
 

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