From 3000531db2474ea6f6fe233d0effc08a647a48f3 Mon Sep 17 00:00:00 2001
From: Tianyu Wu <t228wu@uwateroo.ca>
Date: Mon, 31 May 2021 16:30:51 +0200
Subject: [PATCH 01/25] add static part

---
 multiviewunsynch/main.py                    |   8 +-
 multiviewunsynch/reconstruction/common.py   | 516 ++++++++++++++++++--
 multiviewunsynch/reconstruction/epipolar.py |  61 +++
 multiviewunsynch/tools/util.py              |  38 ++
 4 files changed, 588 insertions(+), 35 deletions(-)

diff --git a/multiviewunsynch/main.py b/multiviewunsynch/main.py
index 0b5a2e7..019baf3 100644
--- a/multiviewunsynch/main.py
+++ b/multiviewunsynch/main.py
@@ -52,7 +52,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 +62,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,7 +84,7 @@
 
 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)
 
 # Align with the ground truth data if available
 flight.out = align_gt(flight, flight.gt['frequency'], flight.gt['filepath'], visualize=False)
diff --git a/multiviewunsynch/reconstruction/common.py b/multiviewunsynch/reconstruction/common.py
index 301397c..1d85b60 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
@@ -59,7 +60,24 @@ 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 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 +141,7 @@ 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:]
+            detect = self.cameras[i].undist_point(self.detections[i][1:], self.settings['undist_method']) if self.settings['undist_points'] else self.detections[i][1:]
             self.detections_global[i] = np.vstack((timestamp, detect))
 
             if motion_prior:
@@ -175,9 +193,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,28 +213,137 @@ 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)
+        # 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[t2].img, np.vstack([d2[0], d2_dist]), self.cameras[t1].img, np.vstack([d1[0],d1_dist]))
+        else:
+            vis.draw_detection_matches(self.cameras[t2].img, d2, self.cameras[t1].img, d1)
+        
+        # add the static part
+        if self.settings['include_static']:
+            if debug:
+                # in debug, use static ground truth as 2d featues
+                if self.settings['undist_points']:
+                    # undistort the ground truth matches
+                    pts1 = self.cameras[t1].undist_point(self.cameras[t1].gt_pts.T, self.settings['undist_method']).T
+                    pts2 = self.cameras[t1].undist_point(self.cameras[t2].gt_pts.T, self.settings['undist_method']).T
+
+                    # plot the ground truth matches
+                    # TODO: check pts_dist == gt_pts
+                    pts1_dist = self.cameras[t1].dist_point2d(pts1.T, method=self.settings['undist_method'])
+                    pts2_dist = self.cameras[t2].dist_point2d(pts2.T, method=self.settings['undist_method'])
+                    vis.draw_detection_matches(self.cameras[t1].img, np.vstack([np.zeros(pts1_dist.shape[1]),pts1_dist]), self.cameras[t2].img, np.vstack([np.zeros(pts2_dist.shape[1]),pts2_dist]))
+
+                else:
+                    pts1 = self.cameras[t1].gt_pts
+                    pts2 = self.cameras[t2].gt_pts
+                
+                    vis.draw_detection_matches(self.cameras[t1].img, np.vstack([np.zeros(pts1.shape[0]),pts1.T]), self.cameras[t2].img, np.vstack([np.zeros(pts2.shape[0]),pts2.T]))
+            else:
+                
+                # sp1, sp2 = util.match_features(self.cameras[t1].img, self.cameras[t2].img, 'sift', 'bf', 0.7)
+
+                # Match features
+                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)
+                
+                # undistort the matched keypoints
+                if self.settings['undist_points']:
+                    pts1 = self.cameras[t1].undist_point(np.array(pts1).T, self.settings['undist_method']).T
+                    pts2 = self.cameras[t1].undist_point(np.array(pts2).T, self.settings['undist_method']).T
+            
+            # stack the static features with the detections and use them together for initial pose extraction
+            fp1 = np.hstack([np.int32(pts1).T, d1[1:]])
+            fp2 = np.hstack([np.int32(pts2).T, 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 < len(pts1)]
+            inlier_traj = idx_mask[idx_mask >= len(pts1)] - len(pts1)
+
+            # save static part
+            self.static = X[:-1, idx_mask < len(pts1)]
+            self.inlier_mask = np.ones(self.static.shape[1])
+
+            # 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])
 
-        if not inlier_only:
-            inlier = np.ones(len(inlier))
-        x1, x2 = util.homogeneous(d1[1:,inlier==1]), util.homogeneous(d2[1:,inlier==1])
+            match_res1[t2, query_ids] = train_ids
+            match_res2[t1, train_ids] = query_ids
 
-        # 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)
+            self.feature_dict[t1] = match_res1
+            self.feature_dict[t2] = match_res2
 
-        mask = np.logical_not(np.isnan(x1[0]))
-        x1 = x1[:,mask]
-        x2 = x2[:,mask]
+            # save trajectory
+            self.traj = np.vstack((d1[0][inlier_traj], X[:-1, idx_mask >= len(pts1)]))
+        
+        # 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])
 
-        # Triangulte points
-        X, P = ep.triangulate_from_E(E,K1,K2,x1,x2)
-        self.traj = np.vstack((d1[0][inlier==1][mask],X[:-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]]))
@@ -358,6 +489,51 @@ 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])
+        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 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)
+
+        # # 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 +614,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):
         '''
         Bundle Adjustment with multiple splines
 
@@ -450,9 +626,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 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,6 +667,12 @@ 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 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
 
 
@@ -649,6 +838,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 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 +865,23 @@ 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 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:
+            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 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)
@@ -697,7 +906,7 @@ def jac_BA(near=3,motion_offset=10):
         return res
 
 
-    def remove_outliers(self, cams, thres=30, verbose=False):
+    def remove_outliers(self, cams, thres=30, verbose=False, debug=False):
         '''
         Remove raw detections that have large reprojection errors.
 
@@ -715,9 +924,33 @@ 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 self.settings['include_static']:
+                    # 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_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 get_camera_pose(self, cam_id, error=8, verbose=0):
+    def get_camera_pose(self, cam_id, cams, error=8, verbose=0, debug=False):
         '''
         Get the absolute pose of a camera by solving the PnP problem.
 
@@ -735,6 +968,68 @@ 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]))))
+        # TODO: SHOULD USE THE RAW DETECTION? INSTEAD OF THE UNDISTORTED ONES??
+        
+        # if the static part is also included, add the static part to the points as well
+        if self.settings['include_static']:
+            if 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 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))
+
+                    # 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 = self.cameras[cam_id].get_gt_points()
+            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 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])
+
+                    # 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
+                    _, 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])
+                    self.cameras[cam_id].index_2d_3d = np.union1d(self.cameras[cam_id].index_2d_3d, self.cameras[i].index_2d_3d[registered_ids])
+                
+                # get the registered 2d features from the new camera
+                pts = self.cameras[cam_id].get_points()
+            # get the registered 3d static point from the new camera
+            static_point_3D = self.static[:, self.cameras[cam_id].index_2d_3d]
+
+            # stack the static points with the detections
+            detect = np.hstack([detect, pts])
+            point_3D = np.hstack([point_3D, static_point_3D])
 
         # PnP solution from OpenCV
         N = point_3D.shape[1]
@@ -813,7 +1108,83 @@ 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)
+            # 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))
+            
+            # 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], 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(add_cand_ids, new_ids).nonzeros()[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((add_cand_ids, new_ids[~added_mask]))
+            # add the new points to the record
+            X_new = np.hstack([X_new, X_i[:, ~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):
         '''
@@ -1067,7 +1438,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)
+        # the indices of the 3D static points that corresponds to the used feautures
+        self.index_2d_3d = np.empty(0)
+        # ground truth static matches for debugging
+        self.gt_pts = None
 
     def projectPoint(self,X):
 
@@ -1142,9 +1524,25 @@ def vector2P(self, vector, calib=False):
 
         self.compose()
         return self.P
+
+    def undist_point(self, points, method):
+        '''
+        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 +1553,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,8 +1584,44 @@ 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):
+        '''
+        get the registered 2d points (2, n)
+        '''
+        point_2d = np.empty([2, 0])
+        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):
     '''
     Create a scene from the imput template in json format
diff --git a/multiviewunsynch/reconstruction/epipolar.py b/multiviewunsynch/reconstruction/epipolar.py
index 9ec21a6..b5c7ff3 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
 
 
@@ -639,6 +640,66 @@ 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
+
 
 if __name__ == "__main__":
 
diff --git a/multiviewunsynch/tools/util.py b/multiviewunsynch/tools/util.py
index 3f0ece5..7eb4a1f 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
 
 
@@ -205,6 +206,43 @@ def umeyama(src, dst, estimate_scale):
 
     return T
 
+def draw_detection_matches(img1, d1, img2, d2):
+    '''
+    Function:
+        Draw the corresponding detections in the camera views
+    Input:
+        img1, img2 = two images
+        d1, d2 = the matched detections
+    '''
+    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)
+    print(matplotlib.get_backend())
+
+    plt.imshow(outimg), plt.show()
+    cv2.imwrite('detection_mathches.png', 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
+    '''
+    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__":
     R = rotation(0.38,-176.3,100)

From fff9120e81dde77b8cbc004698cfb95f31f01170 Mon Sep 17 00:00:00 2001
From: Tianyu Wu <t228wu@uwateroo.ca>
Date: Tue, 1 Jun 2021 12:58:54 +0200
Subject: [PATCH 02/25] adapt code to run static part only

---
 multiviewunsynch/reconstruction/common.py | 378 +++++++++++++++++++++-
 1 file changed, 363 insertions(+), 15 deletions(-)

diff --git a/multiviewunsynch/reconstruction/common.py b/multiviewunsynch/reconstruction/common.py
index 1d85b60..7804a38 100644
--- a/multiviewunsynch/reconstruction/common.py
+++ b/multiviewunsynch/reconstruction/common.py
@@ -224,7 +224,7 @@ def init_traj(self,error=10,inlier_only=False, debug=False):
             vis.draw_detection_matches(self.cameras[t2].img, d2, self.cameras[t1].img, d1)
         
         # add the static part
-        if self.settings['include_static']:
+        if 'include_static' in self.settings.keys() and self.settings['include_static']:
             if debug:
                 # in debug, use static ground truth as 2d featues
                 if self.settings['undist_points']:
@@ -233,7 +233,7 @@ def init_traj(self,error=10,inlier_only=False, debug=False):
                     pts2 = self.cameras[t1].undist_point(self.cameras[t2].gt_pts.T, self.settings['undist_method']).T
 
                     # plot the ground truth matches
-                    # TODO: check pts_dist == gt_pts
+                    # FIXME: check pts_dist == gt_pts
                     pts1_dist = self.cameras[t1].dist_point2d(pts1.T, method=self.settings['undist_method'])
                     pts2_dist = self.cameras[t2].dist_point2d(pts2.T, method=self.settings['undist_method'])
                     vis.draw_detection_matches(self.cameras[t1].img, np.vstack([np.zeros(pts1_dist.shape[1]),pts1_dist]), self.cameras[t2].img, np.vstack([np.zeros(pts2_dist.shape[1]),pts2_dist]))
@@ -351,6 +351,96 @@ def init_traj(self,error=10,inlier_only=False, debug=False):
         self.cameras[t1].decompose()
         self.cameras[t2].decompose()
 
+    def init_static(self, error=10, inlier_only=False, debug=False):
+        t1, t2 = self.sequence[0], self.sequence[1]
+        K1, K2 = self.cameras[t1].K, self.cameras[t2].K
+
+        if debug:
+            # in debug, use static ground truth as 2d featues
+            if self.settings['undist_points']:
+                # undistort the ground truth matches
+                pts1 = self.cameras[t1].undist_point(self.cameras[t1].gt_pts.T, self.settings['undist_method']).T
+                pts2 = self.cameras[t1].undist_point(self.cameras[t2].gt_pts.T, self.settings['undist_method']).T
+
+                # plot the ground truth matches
+                # FIXME: check pts_dist == gt_pts
+                pts1_dist = self.cameras[t1].dist_point2d(pts1.T, method=self.settings['undist_method'])
+                pts2_dist = self.cameras[t2].dist_point2d(pts2.T, method=self.settings['undist_method'])
+                vis.draw_detection_matches(self.cameras[t1].img, np.vstack([np.zeros(pts1_dist.shape[1]),pts1_dist]), self.cameras[t2].img, np.vstack([np.zeros(pts2_dist.shape[1]),pts2_dist]))
+
+            else:
+                pts1 = self.cameras[t1].gt_pts
+                pts2 = self.cameras[t2].gt_pts
+            
+                vis.draw_detection_matches(self.cameras[t1].img, np.vstack([np.zeros(pts1.shape[0]),pts1.T]), self.cameras[t2].img, np.vstack([np.zeros(pts2.shape[0]),pts2.T]))
+
+            # 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
+            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]
+
+            # undistort the matched keypoints
+            if self.settings['undist_points']:
+                pts1 = self.cameras[t1].undist_point(np.array(pts1).T, self.settings['undist_method']).T
+                pts2 = self.cameras[t1].undist_point(np.array(pts2).T, self.settings['undist_method']).T
+
+            # 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)))
+
+        # 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
+
+        # go through the epipolar pipeline for pose estimation and initial scene reconstruction
+        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:
+            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)
+
+        # 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):
         '''
@@ -628,7 +718,7 @@ def error_BA(x):
 
             # if the static part are included, they are added to the beginning of the columns.
             # first parse the pararmeters for the static points
-            if self.settings['include_static']:
+            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
 
@@ -668,7 +758,7 @@ def error_BA(x):
                 error = np.concatenate((error, error_motion_reg))
             
             # also add the errors regarding the static part
-            if self.settings['include_static']:
+            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))
@@ -841,7 +931,7 @@ def jac_BA(near=3,motion_offset=10):
         
         # add the static part to BA
         num_3d_points = 0
-        if self.settings['include_static']:
+        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
@@ -866,7 +956,7 @@ def jac_BA(near=3,motion_offset=10):
         print('Doing BA with {} cameras...\n'.format(numCam))
         fn = lambda x: error_BA(x)
         # ignore jac_sparsity matrix for now for the static part
-        if self.settings['include_static']:
+        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:
             res = least_squares(fn,model,jac_sparsity=A,tr_solver='lsmr',xtol=1e-12,max_nfev=max_iter,verbose=1,bounds=bounds_rs)
@@ -874,7 +964,7 @@ def jac_BA(near=3,motion_offset=10):
         '''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 self.settings['include_static']:
+        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
 
@@ -904,7 +994,120 @@ def jac_BA(near=3,motion_offset=10):
         self.detection_to_global()
 
         return res
+    
+    def BA_static(self, numCam, max_iter=10, debug=False):
+        '''
+        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
+            '''
+            sections = [num_3d_points * 3, numCam * num_camParam]
+            model_static, model_cam = np.split(x, sections)
+
+            # parse the 3d static
+            self.static[:,self.inlier_mask > 0] = model_static.reshape(-1,3).T
+
+            # 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
+            '''
+
+            jac_parts = []
+            num_param = len(model)
+
+            # 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).nonzeros()[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] = 1
+                jac_part[np.arange(num_pts_2d), registered_ids + 1] = 1
+                jac_part[np.arange(num_pts_2d), registered_ids + 2] = 1
+
+                # append mat for x direction
+                jac_parts.append(jac_part)
+                # append mat for y direction
+                jac_parts.append(jac_part)
+
+                # FIXME: check if the dimension is the same
+            
+            jac = np.vstack(jac_parts)
+            
+            return jac
+
+        ''' 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, jac_sparsity=A, tr_solver='lsmr', xtol=1e-12, max_nfev=max_iter, verbose=1)
+
+        ''' AFTER BA '''
+        # parse the result of BA
+        sections = [num_3d_points * 3, numCam * num_camParam]
+        res_static, res_cam = np.split(res.x, sections)
+
+        # 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'])
 
     def remove_outliers(self, cams, thres=30, verbose=False, debug=False):
         '''
@@ -926,7 +1129,7 @@ def remove_outliers(self, cams, thres=30, verbose=False, debug=False):
                     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 self.settings['include_static']:
+                if 'include_static' in self.settings.keys() and self.settings['include_static']:
                     # filter out the outliers from the static scene
                     error_static = self.error_cam_static(i, mode='dist', debug=debug)
 
@@ -950,6 +1153,135 @@ def remove_outliers(self, cams, thres=30, verbose=False, debug=False):
                     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_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 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
+            # 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))
+
+                # 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_points()
+        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 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])
+
+                # 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
+                _, 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])
+                self.cameras[cam_id].index_2d_3d = np.union1d(self.cameras[cam_id].index_2d_3d, self.cameras[i].index_2d_3d[registered_ids])
+            
+            # 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
+        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]
+        self.cameras[cam_id].index_2d_3d = self.cameras[cam_id].index_2d_3d[inliers]
+
+        self.cameras[cam_id].R = cv2.Rodrigues(rvec)[0]
+        self.cameras[cam_id].t = tvec.reshape(-1,)
+        self.cameras[cam_id].compose()
+
+        if verbose:
+            print('{} out of {} points are inliers for PnP'.format(inliers.shape[0], N))
+
     def get_camera_pose(self, cam_id, cams, error=8, verbose=0, debug=False):
         '''
         Get the absolute pose of a camera by solving the PnP problem.
@@ -968,10 +1300,11 @@ def get_camera_pose(self, cam_id, cams, error=8, verbose=0, debug=False):
             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]))))
-        # TODO: SHOULD USE THE RAW DETECTION? INSTEAD OF THE UNDISTORTED ONES??
+        # 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 self.settings['include_static']:
+        if 'include_static' in self.settings.keys() and self.settings['include_static']:
             if debug:
                 # use the ground truth matches
                 # initialize the matching result of this new camera to be stored to the dict
@@ -1038,6 +1371,10 @@ def get_camera_pose(self, cam_id, cams, error=8, verbose=0, debug=False):
         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()
@@ -1162,6 +1499,8 @@ def triangulate_static(self, cam_id, cams, thres=0, verbose=0):
             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]))
@@ -1218,7 +1557,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
@@ -1621,7 +1959,7 @@ 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):
     '''
     Create a scene from the imput template in json format
@@ -1655,9 +1993,17 @@ 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'], img_path=cam['feature_extractor'])
+        # extract features
+        camera.extract_features(method=flight.settings['feature_extractor'])
+
+        # 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']
@@ -1676,7 +2022,9 @@ 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']
+    flight.gt = None
+    if 'optional inputs' in config.keys() and 'ground_truth' in config['optional inputs'].keys():
+        flight.gt = config['optional inputs']['ground_truth']
 
     print('Input data are loaded successfully, a scene is created.\n')
     return flight

From 253daf4bfed983412d43ad28595ecb9f2818ff01 Mon Sep 17 00:00:00 2001
From: Tianyu Wu <t228wu@uwateroo.ca>
Date: Tue, 1 Jun 2021 13:23:51 +0200
Subject: [PATCH 03/25] add script for running static part reconstruction

---
 multiviewunsynch/main_static.py | 85 +++++++++++++++++++++++++++++++++
 1 file changed, 85 insertions(+)
 create mode 100644 multiviewunsynch/main_static.py

diff --git a/multiviewunsynch/main_static.py b/multiviewunsynch/main_static.py
new file mode 100644
index 0000000..31a91ab
--- /dev/null
+++ b/multiviewunsynch/main_static.py
@@ -0,0 +1,85 @@
+# 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
+import sys
+
+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()
+
+'''---------------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(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'])
+
+    print('\nDoing the second BA')
+    # Bundle adjustment after outlier removal
+    res = flight.BA(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(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
+
+# Visualize the 3D static points
+vis.show_3D_all(flight.static[flight.inlier_mask > 0], color=False, line=False)
+# vis.show_trajectory_3D(flight.traj[1:],line=False)
+
+# Align with the ground truth data if available
+if flight.gt is not None:
+    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)
+
+print('Finished!')
\ No newline at end of file

From 7503adec895557540331b0f455048375e7a09083 Mon Sep 17 00:00:00 2001
From: Tianyu Wu <t228wu@uwateroo.ca>
Date: Tue, 1 Jun 2021 13:24:39 +0200
Subject: [PATCH 04/25] modify .gitignore

---
 .gitignore | 1 +
 1 file changed, 1 insertion(+)

diff --git a/.gitignore b/.gitignore
index 77ceb39..d7d7bbf 100644
--- a/.gitignore
+++ b/.gitignore
@@ -125,3 +125,4 @@ dmypy.json
 
 # Datas
 data/
+experiments/

From 763a288e158f55e689b632e55503f5e48a6329d4 Mon Sep 17 00:00:00 2001
From: Tianyu Wu <t228wu@uwateroo.ca>
Date: Tue, 1 Jun 2021 23:10:59 +0200
Subject: [PATCH 05/25] fix bugs with the static part

---
 .gitignore                                |   1 +
 multiviewunsynch/analysis/compare_gt.py   |  15 +++
 multiviewunsynch/main_static.py           |  39 ++++--
 multiviewunsynch/reconstruction/common.py | 146 ++++++++++------------
 multiviewunsynch/tools/util.py            |  38 ------
 multiviewunsynch/tools/visualization.py   |  52 +++++++-
 6 files changed, 165 insertions(+), 126 deletions(-)

diff --git a/.gitignore b/.gitignore
index d7d7bbf..c936980 100644
--- a/.gitignore
+++ b/.gitignore
@@ -126,3 +126,4 @@ dmypy.json
 # Datas
 data/
 experiments/
+multiviewunsynch/*.png
diff --git a/multiviewunsynch/analysis/compare_gt.py b/multiviewunsynch/analysis/compare_gt.py
index a6a24aa..4629494 100644
--- a/multiviewunsynch/analysis/compare_gt.py
+++ b/multiviewunsynch/analysis/compare_gt.py
@@ -150,6 +150,21 @@ 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.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]
 
 if __name__ == "__main__":
 
diff --git a/multiviewunsynch/main_static.py b/multiviewunsynch/main_static.py
index 31a91ab..f0072b1 100644
--- a/multiviewunsynch/main_static.py
+++ b/multiviewunsynch/main_static.py
@@ -7,7 +7,7 @@
 from tools import visualization as vis
 from datetime import datetime
 from reconstruction import common
-from analysis.compare_gt import align_gt
+from analysis.compare_gt import align_gt, align_gt_static
 import sys
 
 import cv2
@@ -31,7 +31,7 @@
 flight = common.create_scene(args.config_file)
 
 # Initialize the static part
-flight.init_static()
+flight.init_static(inlier_only=True, debug=args.debug)
 
 '''---------------Incremental reconstruction----------------'''
 start = datetime.now()
@@ -44,17 +44,17 @@
 
     print('\nDoing the first BA')
     # Bundle adjustment
-    res = flight.BA(cam_temp, debug=args.debug)
+    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'])
+    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(cam_temp, debug=args.debug)
+    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]]))
 
@@ -72,14 +72,35 @@
     print('\nTotal time: {}\n\n\n'.format(datetime.now()-start))
     cam_temp += 1
 
-# Visualize the 3D static points
-vis.show_3D_all(flight.static[flight.inlier_mask > 0], color=False, line=False)
-# 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)
+    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)
+    # 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]
+        x_ori = cam.get_points()
+        vis.show_2D_all(x_ori, x_res, title='cam'+str(i), color=True, line=False, bg=cam.img)
 
 # Align with the ground truth data if available
-if flight.gt is not None:
+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['feature_extractor'] == 'sift':
+        for cam in flight.cameras:
+            cam.unpack_sift_kp()
     pickle.dump(flight, f)
 
 print('Finished!')
\ No newline at end of file
diff --git a/multiviewunsynch/reconstruction/common.py b/multiviewunsynch/reconstruction/common.py
index 7804a38..79f9256 100644
--- a/multiviewunsynch/reconstruction/common.py
+++ b/multiviewunsynch/reconstruction/common.py
@@ -66,6 +66,8 @@ def __init__(self):
         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
@@ -366,7 +368,8 @@ def init_static(self, error=10, inlier_only=False, debug=False):
                 # FIXME: check pts_dist == gt_pts
                 pts1_dist = self.cameras[t1].dist_point2d(pts1.T, method=self.settings['undist_method'])
                 pts2_dist = self.cameras[t2].dist_point2d(pts2.T, method=self.settings['undist_method'])
-                vis.draw_detection_matches(self.cameras[t1].img, np.vstack([np.zeros(pts1_dist.shape[1]),pts1_dist]), self.cameras[t2].img, np.vstack([np.zeros(pts2_dist.shape[1]),pts2_dist]))
+                # vis.draw_detection_matches(self.cameras[t1].img, np.vstack([np.zeros(pts1_dist.shape[1]),pts1_dist]), self.cameras[t2].img, np.vstack([np.zeros(pts2_dist.shape[1]),pts2_dist]))
+                vis.draw_detection_matches(self.cameras[t1].img, np.vstack([np.zeros(pts1.shape[0]),self.cameras[t1].gt_pts.T]), self.cameras[t2].img, np.vstack([np.zeros(pts2.shape[0]),self.cameras[t2].gt_pts.T]))
 
             else:
                 pts1 = self.cameras[t1].gt_pts
@@ -410,6 +413,9 @@ def init_static(self, error=10, inlier_only=False, debug=False):
         match_res2[t1,train_ids] = query_ids
         self.feature_dict[t1] = match_res1
         self.feature_dict[t2] = match_res2
+        
+        pts1 = np.int32(pts1).T
+        pts2 = np.int32(pts2).T
 
         # go through the epipolar pipeline for pose estimation and initial scene reconstruction
         X, P, inlier, mask = ep.epipolar_pipeline(pts1, pts2, K1, K2, error, inlier_only, self.cameras[t1].img, self.cameras[t2].img)
@@ -882,7 +888,41 @@ 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, 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_static = vstack(jac_parts)
+                jac = vstack((jac, jac_static))
             # fix the first camera
             # jac[:,[0,numCam]], jac[:,2*numCam+4:2*numCam+10] = 0, 0
             #return jac
@@ -1008,8 +1048,7 @@ def error_BA(x):
             Input:
                 x = parameters to be optimized
             '''
-            sections = [num_3d_points * 3, numCam * num_camParam]
-            model_static, model_cam = np.split(x, sections)
+            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
@@ -1033,9 +1072,9 @@ def jac_BA():
                 define the sparse jaccobian matrix
             '''
 
-            jac_parts = []
             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
@@ -1045,7 +1084,7 @@ def jac_BA():
                 # 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).nonzeros()[0]
+                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))
@@ -1055,20 +1094,18 @@ def jac_BA():
                 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] = 1
-                jac_part[np.arange(num_pts_2d), registered_ids + 1] = 1
-                jac_part[np.arange(num_pts_2d), registered_ids + 2] = 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
+                # append mat for x direction and y direction
                 jac_parts.append(jac_part)
-                # append mat for y direction
                 jac_parts.append(jac_part)
 
                 # FIXME: check if the dimension is the same
             
-            jac = np.vstack(jac_parts)
-            
-            return jac
+            jac = vstack(jac_parts)
+            return jac.toarray()
 
         ''' BEFORE BA '''
         # camera model
@@ -1094,12 +1131,12 @@ def jac_BA():
         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)
         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
-        sections = [num_3d_points * 3, numCam * num_camParam]
-        res_static, res_cam = np.split(res.x, sections)
+        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
@@ -1108,7 +1145,8 @@ def jac_BA():
         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)
+    
     def remove_outliers(self, cams, thres=30, verbose=False, debug=False):
         '''
         Remove raw detections that have large reprojection errors.
@@ -1138,6 +1176,7 @@ def remove_outliers(self, cams, thres=30, verbose=False, debug=False):
                     # 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
@@ -1168,6 +1207,7 @@ def remove_outliers_static(self, cams, thres=30, verbose=False, debug=False):
                 # 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
@@ -1305,64 +1345,11 @@ def get_camera_pose(self, cam_id, cams, error=8, verbose=0, debug=False):
         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']:
-            if 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 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))
-
-                    # 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 = self.cameras[cam_id].get_gt_points()
-            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 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])
-
-                    # 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
-                    _, 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])
-                    self.cameras[cam_id].index_2d_3d = np.union1d(self.cameras[cam_id].index_2d_3d, self.cameras[i].index_2d_3d[registered_ids])
-                
-                # get the registered 2d features from the new camera
-                pts = self.cameras[cam_id].get_points()
-            # get the registered 3d static point from the new camera
-            static_point_3D = self.static[:, self.cameras[cam_id].index_2d_3d]
+            pts_2d, pts_3d = self.register_new_camera_static(cam_id, cams, debug)
 
             # stack the static points with the detections
-            detect = np.hstack([detect, pts])
-            point_3D = np.hstack([point_3D, static_point_3D])
+            detect = np.hstack([detect, pts_2d])
+            point_3D = np.hstack([point_3D, pts_3d])
 
         # PnP solution from OpenCV
         N = point_3D.shape[1]
@@ -1959,6 +1946,9 @@ def get_gt_pts(self):
         get the registered 2d ground truth points (2, n)
         '''
         return self.gt_pts[self.index_registered_2d].T
+    
+    def unpack_sift_kp(self):
+        self.kp = np.array([kp.pt for kp in self.kp])
 
 def create_scene(path_input):
     '''
@@ -1984,7 +1974,7 @@ def create_scene(path_input):
 
     # 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)
@@ -1995,7 +1985,7 @@ def create_scene(path_input):
             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'], img_path=cam['feature_extractor'])
+        camera = Camera(K=np.asfarray(cam['K-matrix']), d=np.asfarray(cam['distCoeff']), fps=cam['fps'], resolution=cam['resolution'], img_path=cam['img_path'])
         # extract features
         camera.extract_features(method=flight.settings['feature_extractor'])
 
@@ -2022,9 +2012,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 = None
-    if 'optional inputs' in config.keys() and 'ground_truth' in config['optional inputs'].keys():
-        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/tools/util.py b/multiviewunsynch/tools/util.py
index 7eb4a1f..6a33881 100644
--- a/multiviewunsynch/tools/util.py
+++ b/multiviewunsynch/tools/util.py
@@ -206,44 +206,6 @@ def umeyama(src, dst, estimate_scale):
 
     return T
 
-def draw_detection_matches(img1, d1, img2, d2):
-    '''
-    Function:
-        Draw the corresponding detections in the camera views
-    Input:
-        img1, img2 = two images
-        d1, d2 = the matched detections
-    '''
-    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)
-    print(matplotlib.get_backend())
-
-    plt.imshow(outimg), plt.show()
-    cv2.imwrite('detection_mathches.png', 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
-    '''
-    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__":
     R = rotation(0.38,-176.3,100)
     x,y,z = rotation_decompose(R)
diff --git a/multiviewunsynch/tools/visualization.py b/multiviewunsynch/tools/visualization.py
index 3e6f0de..892235e 100644
--- a/multiviewunsynch/tools/visualization.py
+++ b/multiviewunsynch/tools/visualization.py
@@ -156,14 +156,21 @@ def show_trajectory_3D(*X,title=None,color=True,line=False):
     plt.show()
 
 
-def show_2D_all(*x,title=None,color=True,line=True,text=False):
+def show_2D_all(*x,title=None,color=True,line=True,text=False, bg=None):
     plt.figure(figsize=(12, 10))
+    # if bg is not None:
+    #     plt.imshow(bg)
     num = len(x)
     for i in range(num):
         # plt.subplot(1,num,i+1)
 
         c = ['r','b']
-        plt.scatter(x[i][0],x[i][1],c=c[i])
+        m = ['o','x']
+        label = ['Raw points', 'Reconstruction points']
+        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])
@@ -179,6 +186,9 @@ def show_2D_all(*x,title=None,color=True,line=True,text=False):
         plt.ylabel('Y')
     if title:
         plt.suptitle(title)
+        plt.savefig(title+'.png')
+    else:
+        plt.savefig('reprojected.png')
     plt.show()
 
 
@@ -217,6 +227,7 @@ def show_3D_all(*X,title=None,color=True,line=True):
     for handle in lgnd.legendHandles:
         handle.set_sizes([100])
     # plt.axis('off')
+    plt.savefig('reconstructed_scene.png')
     plt.show()
 
 
@@ -287,6 +298,43 @@ def error_traj(traj,error,thres=0.5,title=None,colormap='Wistia',size=100, text=
     plt.title(title, fontsize=50)
     plt.show()
 
+def draw_detection_matches(img1, d1, img2, d2):
+    '''
+    Function:
+        Draw the corresponding detections in the camera views
+    Input:
+        img1, img2 = two images
+        d1, d2 = the matched detections
+    '''
+    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)
+
+    plt.imshow(outimg), plt.show()
+    cv2.imwrite('detection_mathches.png', 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
+    '''
+    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

From 5040a23c81e7a480be37bfb1ad5555f0bdf430fd Mon Sep 17 00:00:00 2001
From: Tianyu Wu <t228wu@uwateroo.ca>
Date: Thu, 3 Jun 2021 09:50:58 +0200
Subject: [PATCH 06/25] debug and test with static and dynamic part together

---
 multiviewunsynch/analysis/compare_gt.py   |  72 ++++++++++
 multiviewunsynch/main_static.py           |   7 +-
 multiviewunsynch/main_static_dynamic.py   | 167 ++++++++++++++++++++++
 multiviewunsynch/reconstruction/common.py |   3 +-
 multiviewunsynch/tools/visualization.py   |   4 +-
 5 files changed, 248 insertions(+), 5 deletions(-)
 create mode 100644 multiviewunsynch/main_static_dynamic.py

diff --git a/multiviewunsynch/analysis/compare_gt.py b/multiviewunsynch/analysis/compare_gt.py
index 4629494..78e9c87 100644
--- a/multiviewunsynch/analysis/compare_gt.py
+++ b/multiviewunsynch/analysis/compare_gt.py
@@ -166,6 +166,78 @@ def align_gt_static(flight):
 
     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_3D(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__":
 
     # Load the reconstructed trajectory
diff --git a/multiviewunsynch/main_static.py b/multiviewunsynch/main_static.py
index f0072b1..4ee7930 100644
--- a/multiviewunsynch/main_static.py
+++ b/multiviewunsynch/main_static.py
@@ -64,7 +64,7 @@
         break
 
     # Add the next camera and get its pose
-    flight.get_camera_pose(flight.sequence[cam_temp], flight.sequence[:cam_temp], debug=args.debug)
+    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'])
@@ -90,7 +90,10 @@
     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]
-        x_ori = cam.get_points()
+        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)
 
 # Align with the ground truth data if available
diff --git a/multiviewunsynch/main_static_dynamic.py b/multiviewunsynch/main_static_dynamic.py
new file mode 100644
index 0000000..925ad38
--- /dev/null
+++ b/multiviewunsynch/main_static_dynamic.py
@@ -0,0 +1,167 @@
+# 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
+
+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 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 with the static part
+# flight.init_static(inlier_only=True, debug=args.debug)
+
+# 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('\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], 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)
+# save the 2d trajectories
+if 'save_2d' in flight.settings.keys() and flight.settings['save_2d']:
+    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)
+
+        # # align with the raw detection
+        # _ =  align_detections(flight, visualize=True)
+
+        # save the reprojected trajectories
+        traj_res = np.vstack([x_res, flight.traj[0]]).T
+        # save the raw detection (replace the timestamp to the global timestamp)
+        det_ori_global = np.vstack([x_ori, flight.detections_global[i][0]]).T
+        np.savetxt(flight.settings['save_2d_path'].replace('.txt', '_cam'+str(i)+'.txt'), traj_res, delimiter=' ')
+        np.savetxt(flight.settings['save_2d_path'].replace('.txt', '_det_ori_global_cam'+str(i)+'.txt'), det_ori_global, delimiter=' ')
+
+# 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)
+    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)
+    # 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)
+
+# 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['feature_extractor'] == 'sift':
+        for cam in flight.cameras:
+            cam.unpack_sift_kp()
+    pickle.dump(flight, f)
+
+print('Finished!')
\ No newline at end of file
diff --git a/multiviewunsynch/reconstruction/common.py b/multiviewunsynch/reconstruction/common.py
index 79f9256..f7b0cdf 100644
--- a/multiviewunsynch/reconstruction/common.py
+++ b/multiviewunsynch/reconstruction/common.py
@@ -238,7 +238,8 @@ def init_traj(self,error=10,inlier_only=False, debug=False):
                     # FIXME: check pts_dist == gt_pts
                     pts1_dist = self.cameras[t1].dist_point2d(pts1.T, method=self.settings['undist_method'])
                     pts2_dist = self.cameras[t2].dist_point2d(pts2.T, method=self.settings['undist_method'])
-                    vis.draw_detection_matches(self.cameras[t1].img, np.vstack([np.zeros(pts1_dist.shape[1]),pts1_dist]), self.cameras[t2].img, np.vstack([np.zeros(pts2_dist.shape[1]),pts2_dist]))
+                    # vis.draw_detection_matches(self.cameras[t1].img, np.vstack([np.zeros(pts1_dist.shape[1]),pts1_dist]), self.cameras[t2].img, np.vstack([np.zeros(pts2_dist.shape[1]),pts2_dist]))
+                    vis.draw_detection_matches(self.cameras[t1].img, np.vstack([np.zeros(pts1.shape[0]),self.cameras[t1].gt_pts.T]), self.cameras[t2].img, np.vstack([np.zeros(pts2.shape[0]),self.cameras[t2].gt_pts.T]))
 
                 else:
                     pts1 = self.cameras[t1].gt_pts
diff --git a/multiviewunsynch/tools/visualization.py b/multiviewunsynch/tools/visualization.py
index 892235e..35b8fe3 100644
--- a/multiviewunsynch/tools/visualization.py
+++ b/multiviewunsynch/tools/visualization.py
@@ -158,8 +158,8 @@ def show_trajectory_3D(*X,title=None,color=True,line=False):
 
 def show_2D_all(*x,title=None,color=True,line=True,text=False, bg=None):
     plt.figure(figsize=(12, 10))
-    # if bg is not None:
-    #     plt.imshow(bg)
+    if bg is not None:
+        plt.imshow(bg)
     num = len(x)
     for i in range(num):
         # plt.subplot(1,num,i+1)

From d7622defde3adda09ccb710afc063a05732a590e Mon Sep 17 00:00:00 2001
From: Tianyu Wu <t228wu@uwateroo.ca>
Date: Thu, 3 Jun 2021 21:15:29 +0200
Subject: [PATCH 07/25] add visualization of camera extrinsics

---
 .../analysis/verify_detections.py             |  42 +++-
 multiviewunsynch/main_static.py               |   4 +-
 multiviewunsynch/main_static_dynamic.py       |   4 +-
 multiviewunsynch/reconstruction/common.py     |   4 +-
 .../camera_calibration_show_extrinsics.py     | 230 ++++++++++++++++++
 multiviewunsynch/tools/visualization.py       |  38 ++-
 6 files changed, 310 insertions(+), 12 deletions(-)
 create mode 100755 multiviewunsynch/thirdparty/camera_calibration_show_extrinsics.py

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/main_static.py b/multiviewunsynch/main_static.py
index 4ee7930..acd7c2b 100644
--- a/multiviewunsynch/main_static.py
+++ b/multiviewunsynch/main_static.py
@@ -77,7 +77,7 @@
     # 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)
+    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])
@@ -85,7 +85,7 @@
         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)
+    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])
diff --git a/multiviewunsynch/main_static_dynamic.py b/multiviewunsynch/main_static_dynamic.py
index 925ad38..55311ee 100644
--- a/multiviewunsynch/main_static_dynamic.py
+++ b/multiviewunsynch/main_static_dynamic.py
@@ -135,7 +135,7 @@
     # 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)
+    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])
@@ -143,7 +143,7 @@
         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)
+    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])
diff --git a/multiviewunsynch/reconstruction/common.py b/multiviewunsynch/reconstruction/common.py
index f7b0cdf..d829889 100644
--- a/multiviewunsynch/reconstruction/common.py
+++ b/multiviewunsynch/reconstruction/common.py
@@ -221,9 +221,9 @@ def init_traj(self,error=10,inlier_only=False, debug=False):
             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[t2].img, np.vstack([d2[0], d2_dist]), self.cameras[t1].img, np.vstack([d1[0],d1_dist]))
+            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[t2].img, d2, self.cameras[t1].img, d1)
+            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']:
diff --git a/multiviewunsynch/thirdparty/camera_calibration_show_extrinsics.py b/multiviewunsynch/thirdparty/camera_calibration_show_extrinsics.py
new file mode 100755
index 0000000..e9f1078
--- /dev/null
+++ b/multiviewunsynch/thirdparty/camera_calibration_show_extrinsics.py
@@ -0,0 +1,230 @@
+#!/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]
+
+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/visualization.py b/multiviewunsynch/tools/visualization.py
index 35b8fe3..54e863f 100644
--- a/multiviewunsynch/tools/visualization.py
+++ b/multiviewunsynch/tools/visualization.py
@@ -7,7 +7,10 @@
 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
 
 def drawlines(img1,img2,lines,pts1,pts2):
     ''' 
@@ -153,6 +156,9 @@ def show_trajectory_3D(*X,title=None,color=True,line=False):
     if title:
         plt.suptitle(title)
     # plt.axis('off')
+        plt.savefig(title+'.png')
+    else:
+        plt.savefig('reconstructed_trajectory.png')
     plt.show()
 
 
@@ -192,7 +198,26 @@ def show_2D_all(*x,title=None,color=True,line=True,text=False, bg=None):
     plt.show()
 
 
-def show_3D_all(*X,title=None,color=True,line=True):
+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)
+        cMo = np.eye(4)
+        cMo[:3,:3] = cam.R
+        cMo[:3,-1] = cam.t
+
+        for X_cam_part in 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)
+            ax.plot3D(X[0,:], X[1,:], X[2,:], color=colors[i])
+
+
+def show_3D_all(*X,title=None,color=True,line=True,flight=None):
     fig = plt.figure(figsize=(20, 15))
     num = len(X)
     ax = fig.add_subplot(111,projection='3d')
@@ -215,6 +240,17 @@ 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=1)
+        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)
     

From 2f8504062bbeac6cc0641fe05aa19dd308952086 Mon Sep 17 00:00:00 2001
From: Tianyu Wu <t228wu@uwateroo.ca>
Date: Sun, 6 Jun 2021 16:31:50 +0200
Subject: [PATCH 08/25] add evaluation of results

---
 .../analysis/analysis_reconstruction.py       | 96 +++++++++++++++++++
 multiviewunsynch/analysis/compare_gt.py       |  2 +-
 multiviewunsynch/eval.py                      |  3 +
 multiviewunsynch/main.py                      |  5 +-
 multiviewunsynch/main_static.py               |  4 +-
 multiviewunsynch/main_static_dynamic.py       |  6 +-
 multiviewunsynch/reconstruction/common.py     |  7 +-
 .../camera_calibration_show_extrinsics.py     |  6 +-
 multiviewunsynch/tools/visualization.py       | 11 ++-
 9 files changed, 125 insertions(+), 15 deletions(-)
 create mode 100644 multiviewunsynch/analysis/analysis_reconstruction.py
 create mode 100644 multiviewunsynch/eval.py

diff --git a/multiviewunsynch/analysis/analysis_reconstruction.py b/multiviewunsynch/analysis/analysis_reconstruction.py
new file mode 100644
index 0000000..abb0f5b
--- /dev/null
+++ b/multiviewunsynch/analysis/analysis_reconstruction.py
@@ -0,0 +1,96 @@
+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 itertools import combinations
+from matplotlib import pyplot as plt
+
+def reproject_ground_truth(cameras, gt_pts ,n_bins=10, prefix=''):
+    '''
+    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
+    '''
+    
+    # split gt_pts into two parts
+    gt_pts_parts = np.split(gt_pts, len(cameras))
+
+    combos = combinations(range(len(cameras)),2)
+
+    repo_errors = []
+    # for every pair of cameras, triangulate 3d points and reproject
+    for t1, t2 in combos:
+        print("triangulate camera pairs: (%d, %d)" %(t1, t2))
+        # undistort gt_points
+        gt_un1 = cameras[t1].undist_point(gt_pts_parts[t1].T)
+        gt_un2 = cameras[t2].undist_point(gt_pts_parts[t2].T)
+
+        # triangulate
+        X_gt = ep.triangulate_matlab(gt_un1, gt_un2, cameras[t1].P, cameras[t2].P)
+
+        # backproject the triangulated points on the images
+        gt_repo1 = cameras[t1].dist_point3d(X_gt[:-1].T)
+        gt_repo2 = cameras[t2].dist_point3d(X_gt[:-1].T)
+        # compute the error
+        repo_err1 = ep.reprojection_error(gt_un1, cameras[t1].projectPoint(X_gt))
+        repo_err2 = ep.reprojection_error(gt_un2, cameras[t2].projectPoint(X_gt))
+        repo_errors.append([repo_err1, repo_err2])
+        print("mean reprojection error of camera pair (%d, %d) is %f and %f" %(t1, t2, np.mean(repo_err1), np.mean(repo_err2)))
+
+        # histogram of reprojection error
+        fig, axs = plt.subplots(2, 1, sharex=True, tight_layout=True)
+        axs[0].hist(repo_err1, bins=n_bins)
+        axs[1].hist(repo_err2, bins=n_bins)
+        plt.savefig(prefix+'repo_cam{}_{}.png'.format(t1, t2))
+
+        # plot images
+        vis.show_2D_all(gt_pts_parts[t1].T, gt_repo1, title=prefix+'cam'+str(t1)+' ground truth reprojection', color=True, line=False, bg=cameras[t1].img)
+        vis.show_2D_all(gt_pts_parts[t2].T, gt_repo2, title=prefix+'cam'+str(t2)+' ground truth reprojection', color=True, line=False, bg=cameras[t2].img)
+
+def main():
+    # Load ground truth
+    gt_static_file = '/scratch2/wuti/Repos/3D-Object-Trajectory-Reconstruction-Webcam/multiviewunsynch/webcam-datasets/nyc-datasets/static_gt/static.txt'
+    # gt_dynamic_file = ''
+
+    gt_static = np.loadtxt(gt_static_file, delimiter=' ')
+    # gt_dynamic = np.loadtxt(gt_dynamic_file, delimiter=' ')
+
+    # Load scenes
+    data_file_dynamic = '/scratch2/wuti/Repos/mvus/experiments/dynamic/nyc_colmap_dynamic_30.pkl'
+    data_file_static = '/scratch2/wuti/Repos/mvus/experiments/static/nyc_colmap_static_superglue_30.pkl'
+    data_file_static_dynamic = '/scratch2/wuti/Repos/mvus/experiments/static_dynamic/nyc_colmap_static_dynamic_superglue_30.pkl'
+
+    with open(data_file_dynamic, 'rb') as file:
+        flight_dynamic = pickle.load(file)
+
+    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)
+
+    # Analysis
+    # 2D reprojection error
+    print("Plot reprojection error")
+    # dynamic only
+    print("Reconstructions from dynamic-only setting")
+    reproject_ground_truth(flight_dynamic.cameras, gt_static, prefix='dynamic_only_')
+    print('\n#################################################################\n')
+    print("Reconstructions from static-only setting")
+    reproject_ground_truth(flight_static.cameras, gt_static, prefix='static_only_')
+    print('\n#################################################################\n')
+    print("Reconstructions from static-dynamic setting")
+    reproject_ground_truth(flight_static_dynamic.cameras, gt_static, prefix='static_dynamic_')
+
+    print('Finish!')
+
+if __name__ == '__main__':
+    main()
\ No newline at end of file
diff --git a/multiviewunsynch/analysis/compare_gt.py b/multiviewunsynch/analysis/compare_gt.py
index 78e9c87..047b5c2 100644
--- a/multiviewunsynch/analysis/compare_gt.py
+++ b/multiviewunsynch/analysis/compare_gt.py
@@ -228,7 +228,7 @@ def align_detections(flight, visualize=False):
 
         if visualize:
             # Compare the trajectories
-            vis.show_trajectory_3D(out['reconst_tran'][1:], out['gt'], line=False, title='Reconstruction(left) vs Ground Truth(right)')
+            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'])
diff --git a/multiviewunsynch/eval.py b/multiviewunsynch/eval.py
new file mode 100644
index 0000000..f71b8fb
--- /dev/null
+++ b/multiviewunsynch/eval.py
@@ -0,0 +1,3 @@
+from analysis import analysis_reconstruction as recon
+
+recon.main()
\ No newline at end of file
diff --git a/multiviewunsynch/main.py b/multiviewunsynch/main.py
index 019baf3..f9254b2 100644
--- a/multiviewunsynch/main.py
+++ b/multiviewunsynch/main.py
@@ -30,7 +30,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'])
@@ -87,7 +87,8 @@
 vis.show_trajectory_3D(flight.traj[1:],line=False)
 
 # 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
index acd7c2b..4ee7930 100644
--- a/multiviewunsynch/main_static.py
+++ b/multiviewunsynch/main_static.py
@@ -77,7 +77,7 @@
     # 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)
+    vis.show_3D_all(static_ref, flight.static[:, flight.inlier_mask > 0], color=True, line=False)
     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])
@@ -85,7 +85,7 @@
         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)
+    vis.show_3D_all(flight.static[:, flight.inlier_mask > 0], color=False, line=False)
     # 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])
diff --git a/multiviewunsynch/main_static_dynamic.py b/multiviewunsynch/main_static_dynamic.py
index 55311ee..a608cf7 100644
--- a/multiviewunsynch/main_static_dynamic.py
+++ b/multiviewunsynch/main_static_dynamic.py
@@ -140,19 +140,21 @@
         # 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)
+        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, title='cam'+str(i), color=True, line=False, bg=cam.img)
+        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:
diff --git a/multiviewunsynch/reconstruction/common.py b/multiviewunsynch/reconstruction/common.py
index d829889..3fe72bd 100644
--- a/multiviewunsynch/reconstruction/common.py
+++ b/multiviewunsynch/reconstruction/common.py
@@ -1851,7 +1851,7 @@ def vector2P(self, vector, calib=False):
         self.compose()
         return self.P
 
-    def undist_point(self, points, method):
+    def undist_point(self, points, method='opencv'):
         '''
         Function:
             A wrapper function for the undistort point function with different models
@@ -1988,7 +1988,10 @@ def create_scene(path_input):
         # load camera information
         camera = Camera(K=np.asfarray(cam['K-matrix']), d=np.asfarray(cam['distCoeff']), fps=cam['fps'], resolution=cam['resolution'], img_path=cam['img_path'])
         # extract features
-        camera.extract_features(method=flight.settings['feature_extractor'])
+        if 'include_static' in config.keys() and config['include_static']:
+            camera.extract_features(method=flight.settings['feature_extractor'])
+        else:
+            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():
diff --git a/multiviewunsynch/thirdparty/camera_calibration_show_extrinsics.py b/multiviewunsynch/thirdparty/camera_calibration_show_extrinsics.py
index e9f1078..31f2b1a 100755
--- a/multiviewunsynch/thirdparty/camera_calibration_show_extrinsics.py
+++ b/multiviewunsynch/thirdparty/camera_calibration_show_extrinsics.py
@@ -49,9 +49,9 @@ def create_camera_model(camera_matrix, width, height, scale_focal, draw_frame_ax
 
     # 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]
+    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))
diff --git a/multiviewunsynch/tools/visualization.py b/multiviewunsynch/tools/visualization.py
index 54e863f..e9922bf 100644
--- a/multiviewunsynch/tools/visualization.py
+++ b/multiviewunsynch/tools/visualization.py
@@ -170,9 +170,9 @@ def show_2D_all(*x,title=None,color=True,line=True,text=False, bg=None):
     for i in range(num):
         # plt.subplot(1,num,i+1)
 
-        c = ['r','b']
-        m = ['o','x']
-        label = ['Raw points', 'Reconstruction points']
+        c = ['r','b','r','g']
+        m = ['o','x','o','+']
+        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:
@@ -215,6 +215,11 @@ def draw_camera_extrinsics(flight, ax, scale_focal=40):
             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)
             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,flight=None):

From 18d5c3f32981ce6f843f5bc520f5b13645e51b79 Mon Sep 17 00:00:00 2001
From: Tianyu Wu <t228wu@uwateroo.ca>
Date: Sun, 6 Jun 2021 16:41:17 +0200
Subject: [PATCH 09/25] add dynamic only for result evaluation

---
 multiviewunsynch/analysis/analysis_reconstruction.py | 10 +++++++---
 1 file changed, 7 insertions(+), 3 deletions(-)

diff --git a/multiviewunsynch/analysis/analysis_reconstruction.py b/multiviewunsynch/analysis/analysis_reconstruction.py
index abb0f5b..283fd78 100644
--- a/multiviewunsynch/analysis/analysis_reconstruction.py
+++ b/multiviewunsynch/analysis/analysis_reconstruction.py
@@ -52,8 +52,12 @@ def reproject_ground_truth(cameras, gt_pts ,n_bins=10, prefix=''):
         plt.savefig(prefix+'repo_cam{}_{}.png'.format(t1, t2))
 
         # plot images
-        vis.show_2D_all(gt_pts_parts[t1].T, gt_repo1, title=prefix+'cam'+str(t1)+' ground truth reprojection', color=True, line=False, bg=cameras[t1].img)
-        vis.show_2D_all(gt_pts_parts[t2].T, gt_repo2, title=prefix+'cam'+str(t2)+' ground truth reprojection', color=True, line=False, bg=cameras[t2].img)
+        if prefix == 'dynamic_only_':
+            vis.show_2D_all(gt_pts_parts[t1].T, gt_repo1, title=prefix+'cam'+str(t1)+' ground truth reprojection', color=True, line=False)
+            vis.show_2D_all(gt_pts_parts[t2].T, gt_repo2, title=prefix+'cam'+str(t2)+' ground truth reprojection', color=True, line=False)
+        else:
+            vis.show_2D_all(gt_pts_parts[t1].T, gt_repo1, title=prefix+'cam'+str(t1)+' ground truth reprojection', color=True, line=False, bg=cameras[t1].img)
+            vis.show_2D_all(gt_pts_parts[t2].T, gt_repo2, title=prefix+'cam'+str(t2)+' ground truth reprojection', color=True, line=False, bg=cameras[t2].img)
 
 def main():
     # Load ground truth
@@ -64,7 +68,7 @@ def main():
     # gt_dynamic = np.loadtxt(gt_dynamic_file, delimiter=' ')
 
     # Load scenes
-    data_file_dynamic = '/scratch2/wuti/Repos/mvus/experiments/dynamic/nyc_colmap_dynamic_30.pkl'
+    data_file_dynamic = '/scratch2/wuti/Repos/mvus/experiments/dynamic/nyc_colmap_dynamic_ori_30.pkl'
     data_file_static = '/scratch2/wuti/Repos/mvus/experiments/static/nyc_colmap_static_superglue_30.pkl'
     data_file_static_dynamic = '/scratch2/wuti/Repos/mvus/experiments/static_dynamic/nyc_colmap_static_dynamic_superglue_30.pkl'
 

From 878d1be180754e84d687f0f4b5cd71eb84c3a38a Mon Sep 17 00:00:00 2001
From: Tianyu Wu <t228wu@uwateroo.ca>
Date: Tue, 8 Jun 2021 14:20:05 +0200
Subject: [PATCH 10/25] add static-then-dynamic initialization

---
 .../analysis/analysis_reconstruction.py       | 141 ++++++++---
 multiviewunsynch/analysis/compare_gt.py       |   2 +-
 multiviewunsynch/main.py                      |  18 ++
 multiviewunsynch/main_static.py               |   4 +-
 multiviewunsynch/main_static_then_dynamic.py  | 220 ++++++++++++++++++
 multiviewunsynch/reconstruction/common.py     | 108 ++++++++-
 multiviewunsynch/reconstruction/epipolar.py   |  44 ++++
 .../reconstruction/synchronization.py         |  45 ++++
 .../camera_calibration_show_extrinsics.py     |   6 +-
 multiviewunsynch/tools/visualization.py       |  81 ++++---
 10 files changed, 596 insertions(+), 73 deletions(-)
 create mode 100644 multiviewunsynch/main_static_then_dynamic.py

diff --git a/multiviewunsynch/analysis/analysis_reconstruction.py b/multiviewunsynch/analysis/analysis_reconstruction.py
index 283fd78..c059a32 100644
--- a/multiviewunsynch/analysis/analysis_reconstruction.py
+++ b/multiviewunsynch/analysis/analysis_reconstruction.py
@@ -6,11 +6,12 @@
 from reconstruction import synchronization as sync
 
 from reconstruction import epipolar as ep
-
+from tools.util import match_overlap
 from itertools import combinations
 from matplotlib import pyplot as plt
+import os
 
-def reproject_ground_truth(cameras, gt_pts ,n_bins=10, prefix=''):
+def reproject_ground_truth(cameras, gt_pts, gt_dets, n_bins=10, output_dir='', prefix='', flight=None):
     '''
     Function:
         Compute and plot the reprojection errors of the ground truth matches
@@ -20,55 +21,115 @@ def reproject_ground_truth(cameras, gt_pts ,n_bins=10, prefix=''):
         n_bins = number of bins for plotting the histogram of the errors
     '''
     
-    # split gt_pts into two parts
-    gt_pts_parts = np.split(gt_pts, len(cameras))
-
     combos = combinations(range(len(cameras)),2)
 
-    repo_errors = []
+    repro_errors = []
     # for every pair of cameras, triangulate 3d points and reproject
     for t1, t2 in combos:
         print("triangulate camera pairs: (%d, %d)" %(t1, t2))
         # undistort gt_points
-        gt_un1 = cameras[t1].undist_point(gt_pts_parts[t1].T)
-        gt_un2 = cameras[t2].undist_point(gt_pts_parts[t2].T)
+        gt_un1 = cameras[t1].undist_point(gt_pts[t1].T)
+        gt_un2 = cameras[t2].undist_point(gt_pts[t2].T)
 
         # triangulate
         X_gt = ep.triangulate_matlab(gt_un1, gt_un2, cameras[t1].P, cameras[t2].P)
 
         # backproject the triangulated points on the images
-        gt_repo1 = cameras[t1].dist_point3d(X_gt[:-1].T)
-        gt_repo2 = cameras[t2].dist_point3d(X_gt[:-1].T)
+        gt_repro1 = cameras[t1].dist_point3d(X_gt[:-1].T)
+        gt_repro2 = cameras[t2].dist_point3d(X_gt[:-1].T)
         # compute the error
-        repo_err1 = ep.reprojection_error(gt_un1, cameras[t1].projectPoint(X_gt))
-        repo_err2 = ep.reprojection_error(gt_un2, cameras[t2].projectPoint(X_gt))
-        repo_errors.append([repo_err1, repo_err2])
-        print("mean reprojection error of camera pair (%d, %d) is %f and %f" %(t1, t2, np.mean(repo_err1), np.mean(repo_err2)))
+        repro_err1 = ep.reprojection_error(gt_un1, cameras[t1].projectPoint(X_gt))
+        repro_err2 = ep.reprojection_error(gt_un2, cameras[t2].projectPoint(X_gt))
+        repro_errors.append([repro_err1, repro_err2])
+        print("mean reprojection error of camera pair (%d, %d) is %f and %f" %(t1, t2, np.mean(repro_err1), np.mean(repro_err2)))
 
         # histogram of reprojection error
         fig, axs = plt.subplots(2, 1, sharex=True, tight_layout=True)
-        axs[0].hist(repo_err1, bins=n_bins)
-        axs[1].hist(repo_err2, bins=n_bins)
-        plt.savefig(prefix+'repo_cam{}_{}.png'.format(t1, t2))
-
-        # plot images
-        if prefix == 'dynamic_only_':
-            vis.show_2D_all(gt_pts_parts[t1].T, gt_repo1, title=prefix+'cam'+str(t1)+' ground truth reprojection', color=True, line=False)
-            vis.show_2D_all(gt_pts_parts[t2].T, gt_repo2, title=prefix+'cam'+str(t2)+' ground truth reprojection', color=True, line=False)
+        axs[0].hist(repro_err1, bins=n_bins)
+        axs[0].set_title('cam'+str(t1))
+        axs[1].hist(repro_err2, bins=n_bins)
+        axs[1].set_title('cam'+str(t2))
+        plt.savefig(output_dir+prefix+'repro_cam{}_{}.png'.format(t1, t2))
+
+        plt.show()
+
+        # evaluate for the dynamic part
+        # match between detections
+        if cameras[t1].fps > cameras[t2].fps:
+            det1, det2 = match_overlap(gt_dets[t1], gt_dets[t2])
         else:
-            vis.show_2D_all(gt_pts_parts[t1].T, gt_repo1, title=prefix+'cam'+str(t1)+' ground truth reprojection', color=True, line=False, bg=cameras[t1].img)
-            vis.show_2D_all(gt_pts_parts[t2].T, gt_repo2, title=prefix+'cam'+str(t2)+' ground truth reprojection', color=True, line=False, bg=cameras[t2].img)
+            det2, det1 = match_overlap(gt_dets[t2], gt_dets[t1])
+
+        vis.draw_detection_matches(cameras[t1].img, det1, cameras[t2].img, det2, title=prefix+'matched_detections.png', output_dir=output_dir)
+        
+        # undistort points
+        det_un1 = cameras[t1].undist_point(det1[1:])
+        det_un2 = cameras[t2].undist_point(det2[1:])
+        # triangulate the detections
+        Traj_gt = ep.triangulate_matlab(det_un1, det_un2, cameras[t1].P, cameras[t2].P)
+        # backproject triangulated detections on the images
+        traj_repro1 = cameras[t1].dist_point3d(Traj_gt[:-1].T)
+        traj_repro2 = cameras[t2].dist_point3d(Traj_gt[:-1].T)
+        vis.draw_detection_matches(cameras[t1].img, np.vstack([det1[0],traj_repro1]), cameras[t2].img, np.vstack([det2[0],traj_repro2]), title=prefix+'reprojected_detections.png', output_dir=output_dir)
+        
+        # compute the reprojection error
+        traj_err1 = ep.reprojection_error(det_un1, cameras[t1].projectPoint(Traj_gt))
+        traj_err2 = ep.reprojection_error(det_un2, cameras[t2].projectPoint(Traj_gt))
+        repro_errors[-1] += [traj_err1, traj_err2]
+        print("mean reprojection error of the trajectories of camera pair (%d, %d) is %f and %f" %(t1, t2, np.mean(traj_err1), np.mean(traj_err2)))
+        # plot the histogram the reprojecton error of the dynamic part
+        fig, axs = plt.subplots(2, 1, sharex=True, tight_layout=True)
+        axs[0].hist(traj_err1, bins=n_bins)
+        axs[0].set_title('cam'+str(t1))
+        axs[1].hist(traj_err2, bins=n_bins)
+        axs[1].set_title('cam'+str(t2))
+        plt.savefig(output_dir+prefix+'repro_traj_cam{}_{}.png'.format(t1, t2))
+
+        plt.show()
+        
+        # plot images
+        vis.show_2D_all(gt_pts[t1].T, gt_repro1, det1[1:], traj_repro1, title=prefix+'cam'+str(t1)+' ground truth reprojection', color=True, line=False, bg=cameras[t1].img, output_dir=output_dir)
+        vis.show_2D_all(gt_pts[t2].T, gt_repro2, det2[1:], traj_repro2, title=prefix+'cam'+str(t2)+' ground truth reprojection', color=True, line=False, bg=cameras[t2].img, output_dir=output_dir)
+
+        # plot 3D reconstructe scene
+        vis.show_3D_all(X_gt, np.empty([3,0]), Traj_gt, np.empty([3,0]), color=False, line=False, flight=flight, output_dir=output_dir+prefix)
+
+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 main():
+    # Output dir
+    output_dir = '/scratch2/wuti/Repos/mvus/experiments/eval_res/'
+
+    if not os.path.exists(output_dir):
+        os.makedirs(output_dir, exist_ok=True)
+
     # Load ground truth
-    gt_static_file = '/scratch2/wuti/Repos/3D-Object-Trajectory-Reconstruction-Webcam/multiviewunsynch/webcam-datasets/nyc-datasets/static_gt/static.txt'
-    # gt_dynamic_file = ''
+    gt_static_file = ['/scratch2/wuti/Repos/3D-Object-Trajectory-Reconstruction-Webcam/multiviewunsynch/webcam-datasets/nyc-datasets/static_gt/static_cam1.txt','/scratch2/wuti/Repos/3D-Object-Trajectory-Reconstruction-Webcam/multiviewunsynch/webcam-datasets/nyc-datasets/static_gt/static_cam2.txt']
+    gt_dynamic_file = ['/scratch2/wuti/Repos/3D-Object-Trajectory-Reconstruction-Webcam/multiviewunsynch/webcam-datasets/nyc-datasets/20210413_1000_3min/det_opencv/set12/obj1/cam0_w1574280981_12_CSRT_obj0.txt','/scratch2/wuti/Repos/3D-Object-Trajectory-Reconstruction-Webcam/multiviewunsynch/webcam-datasets/nyc-datasets/20210413_1000_3min/det_opencv/set12/obj1/cam2_w1587769795_12_CSRT_obj0.txt']
 
-    gt_static = np.loadtxt(gt_static_file, delimiter=' ')
-    # gt_dynamic = np.loadtxt(gt_dynamic_file, delimiter=' ')
+    gt_static = []
+    for gfs in gt_static_file:
+        gt_static.append(np.loadtxt(gfs, delimiter=' '))
+    
+    gt_dynamic = []
+    for gfd in gt_dynamic_file:
+        gt_dynamic.append(np.loadtxt(gfd, usecols=(2,0,1), delimiter=' ').T)
 
     # Load scenes
-    data_file_dynamic = '/scratch2/wuti/Repos/mvus/experiments/dynamic/nyc_colmap_dynamic_ori_30.pkl'
+    data_file_dynamic = '/scratch2/wuti/Repos/mvus/experiments/dynamic/nyc_colmap_dynamic_ori_inlier_30.pkl'
     data_file_static = '/scratch2/wuti/Repos/mvus/experiments/static/nyc_colmap_static_superglue_30.pkl'
     data_file_static_dynamic = '/scratch2/wuti/Repos/mvus/experiments/static_dynamic/nyc_colmap_static_dynamic_superglue_30.pkl'
 
@@ -86,13 +147,29 @@ def main():
     print("Plot reprojection error")
     # dynamic only
     print("Reconstructions from dynamic-only setting")
-    reproject_ground_truth(flight_dynamic.cameras, gt_static, prefix='dynamic_only_')
+    # convert dynamic part timestamp
+    gt_dynamic1 = [gt_dynamic[x].copy() for x in flight_dynamic.sequence]
+    gt_dynamic1 = convert_timestamps(gt_dynamic1, flight_dynamic.alpha, flight_dynamic.beta)
+    reproject_ground_truth(flight_dynamic.cameras, gt_static, gt_dynamic1, output_dir=output_dir, prefix='dynamic_only_', flight=flight_dynamic)
+    
     print('\n#################################################################\n')
     print("Reconstructions from static-only setting")
-    reproject_ground_truth(flight_static.cameras, gt_static, prefix='static_only_')
+    gt_dynamic2 = [gt_dynamic[x].copy() for x in flight_static_dynamic.sequence]
+    # no optimization for synchronization, use fps to convert timestamps
+    for i, cam in enumerate(flight_static.cameras):
+        gt_dynamic2[i][0] *= flight_static.cameras[flight_static.ref_cam].fps/cam.fps
+    reproject_ground_truth(flight_static.cameras, gt_static, gt_dynamic2, output_dir=output_dir, prefix='static_only_', flight=flight_static)
+
     print('\n#################################################################\n')
     print("Reconstructions from static-dynamic setting")
-    reproject_ground_truth(flight_static_dynamic.cameras, gt_static, prefix='static_dynamic_')
+    gt_dynamic3 = [gt_dynamic[x].copy() for x in flight_static_dynamic.sequence]
+    gt_dynamic3 = convert_timestamps(gt_dynamic3, flight_static_dynamic.alpha, flight_static_dynamic.beta)
+    reproject_ground_truth(flight_static_dynamic.cameras, gt_static, gt_dynamic3, output_dir=output_dir, prefix='static_dynamic_', flight=flight_static_dynamic)
+
+    print('\n#################################################################\n')
+    print("Reconstructions from static-only setting")
+    reproject_ground_truth(flight_static.cameras, gt_static, gt_dynamic3, output_dir=output_dir, prefix='static_only_sync_', flight=flight_static)
+
 
     print('Finish!')
 
diff --git a/multiviewunsynch/analysis/compare_gt.py b/multiviewunsynch/analysis/compare_gt.py
index 047b5c2..a12c35d 100644
--- a/multiviewunsynch/analysis/compare_gt.py
+++ b/multiviewunsynch/analysis/compare_gt.py
@@ -160,7 +160,7 @@ def align_gt_static(flight):
         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.T, flight.static[:, flight.inlier_mask > 0], shear=False, scale=True)
+    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]
 
diff --git a/multiviewunsynch/main.py b/multiviewunsynch/main.py
index f9254b2..5ce5f7f 100644
--- a/multiviewunsynch/main.py
+++ b/multiviewunsynch/main.py
@@ -85,6 +85,24 @@
 flight.spline_to_traj(sampling_rate=1)
 # Visualize the 3D trajectory
 vis.show_trajectory_3D(flight.traj[1:],line=False)
+# save the 2d trajectories
+if 'save_2d' in flight.settings.keys() and flight.settings['save_2d']:
+    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)
+
+        # # align with the raw detection
+        # _ =  align_detections(flight, visualize=True)
+
+        # save the reprojected trajectories
+        traj_res = np.vstack([x_res, flight.traj[0]]).T
+        # save the raw detection (replace the timestamp to the global timestamp)
+        det_ori_global = np.vstack([x_ori, flight.detections_global[i][0]]).T
+        np.savetxt(flight.settings['save_2d_path'].replace('.txt', '_cam'+str(i)+'.txt'), traj_res, delimiter=' ')
+        np.savetxt(flight.settings['save_2d_path'].replace('.txt', '_det_ori_global_cam'+str(i)+'.txt'), det_ori_global, delimiter=' ')
+
 
 # Align with the ground truth data if available
 if len(flight.gt) > 0:
diff --git a/multiviewunsynch/main_static.py b/multiviewunsynch/main_static.py
index 4ee7930..acd7c2b 100644
--- a/multiviewunsynch/main_static.py
+++ b/multiviewunsynch/main_static.py
@@ -77,7 +77,7 @@
     # 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)
+    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])
@@ -85,7 +85,7 @@
         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)
+    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])
diff --git a/multiviewunsynch/main_static_then_dynamic.py b/multiviewunsynch/main_static_then_dynamic.py
new file mode 100644
index 0000000..d271e0c
--- /dev/null
+++ b/multiviewunsynch/main_static_then_dynamic.py
@@ -0,0 +1,220 @@
+# 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
+
+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], 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)
+# save the 2d trajectories
+if 'save_2d' in flight.settings.keys() and flight.settings['save_2d']:
+    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)
+
+        # # align with the raw detection
+        # _ =  align_detections(flight, visualize=True)
+
+        # save the reprojected trajectories
+        traj_res = np.vstack([x_res, flight.traj[0]]).T
+        # save the raw detection (replace the timestamp to the global timestamp)
+        det_ori_global = np.vstack([x_ori, flight.detections_global[i][0]]).T
+        np.savetxt(flight.settings['save_2d_path'].replace('.txt', '_cam'+str(i)+'.txt'), traj_res, delimiter=' ')
+        np.savetxt(flight.settings['save_2d_path'].replace('.txt', '_det_ori_global_cam'+str(i)+'.txt'), det_ori_global, delimiter=' ')
+
+# 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['feature_extractor'] == 'sift':
+        for cam in flight.cameras:
+            cam.unpack_sift_kp()
+    pickle.dump(flight, f)
+
+print('Finished!')
\ No newline at end of file
diff --git a/multiviewunsynch/reconstruction/common.py b/multiviewunsynch/reconstruction/common.py
index 3fe72bd..0d48035 100644
--- a/multiviewunsynch/reconstruction/common.py
+++ b/multiviewunsynch/reconstruction/common.py
@@ -232,7 +232,7 @@ def init_traj(self,error=10,inlier_only=False, debug=False):
                 if self.settings['undist_points']:
                     # undistort the ground truth matches
                     pts1 = self.cameras[t1].undist_point(self.cameras[t1].gt_pts.T, self.settings['undist_method']).T
-                    pts2 = self.cameras[t1].undist_point(self.cameras[t2].gt_pts.T, self.settings['undist_method']).T
+                    pts2 = self.cameras[t2].undist_point(self.cameras[t2].gt_pts.T, self.settings['undist_method']).T
 
                     # plot the ground truth matches
                     # FIXME: check pts_dist == gt_pts
@@ -258,7 +258,7 @@ def init_traj(self,error=10,inlier_only=False, debug=False):
                 # undistort the matched keypoints
                 if self.settings['undist_points']:
                     pts1 = self.cameras[t1].undist_point(np.array(pts1).T, self.settings['undist_method']).T
-                    pts2 = self.cameras[t1].undist_point(np.array(pts2).T, self.settings['undist_method']).T
+                    pts2 = self.cameras[t2].undist_point(np.array(pts2).T, self.settings['undist_method']).T
             
             # stack the static features with the detections and use them together for initial pose extraction
             fp1 = np.hstack([np.int32(pts1).T, d1[1:]])
@@ -353,6 +353,70 @@ def init_traj(self,error=10,inlier_only=False, debug=False):
         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]
+        E = np.dot(self.cameras[t2].R, ep.skew(self.cameras[t2].t))
+        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()
+        # 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_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):
         t1, t2 = self.sequence[0], self.sequence[1]
@@ -363,7 +427,7 @@ def init_static(self, error=10, inlier_only=False, debug=False):
             if self.settings['undist_points']:
                 # undistort the ground truth matches
                 pts1 = self.cameras[t1].undist_point(self.cameras[t1].gt_pts.T, self.settings['undist_method']).T
-                pts2 = self.cameras[t1].undist_point(self.cameras[t2].gt_pts.T, self.settings['undist_method']).T
+                pts2 = self.cameras[t2].undist_point(self.cameras[t2].gt_pts.T, self.settings['undist_method']).T
 
                 # plot the ground truth matches
                 # FIXME: check pts_dist == gt_pts
@@ -397,7 +461,7 @@ def init_static(self, error=10, inlier_only=False, debug=False):
             # undistort the matched keypoints
             if self.settings['undist_points']:
                 pts1 = self.cameras[t1].undist_point(np.array(pts1).T, self.settings['undist_method']).T
-                pts2 = self.cameras[t1].undist_point(np.array(pts2).T, self.settings['undist_method']).T
+                pts2 = self.cameras[t2].undist_point(np.array(pts2).T, self.settings['undist_method']).T
 
             # get the valid matches
             query_ids = np.array([m[0].queryIdx for m in matches])
@@ -419,8 +483,15 @@ def init_static(self, error=10, inlier_only=False, debug=False):
         pts2 = np.int32(pts2).T
 
         # 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])
@@ -1732,7 +1803,34 @@ def time_shift(self, iter=False):
                 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()
 
 class Camera:
     """ 
@@ -1913,7 +2011,7 @@ def info(self):
     
     def read_img(self):
         self.img = cv2.imread(self.img_path)
-        self.img = cv2.cvtColor(self.img, cv2.COLOR_BGR2RGB)
+        # self.img = cv2.cvtColor(self.img, cv2.COLOR_BGR2RGB)
 
     def extract_features(self, img=None, method='sift'):
         '''
diff --git a/multiviewunsynch/reconstruction/epipolar.py b/multiviewunsynch/reconstruction/epipolar.py
index b5c7ff3..3be61d9 100644
--- a/multiviewunsynch/reconstruction/epipolar.py
+++ b/multiviewunsynch/reconstruction/epipolar.py
@@ -700,6 +700,50 @@ def epipolar_pipeline(d1, d2, K1, K2, error, inlier_only, img1, img2):
 
     return X, P, inlier, mask
 
+def epipolar_pipeline_from_F(d1, d2, K1, K2, F, 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
+    serr = Sampson_error(util.homogeneous(d1),util.homogeneous(d2),F)
+    # inlier = np.zeros(len(serr))
+    # inlier[serr < thres] = 1
+    inlier = np.ones(len(serr))
+    # print(inlier)
+    E = np.dot(np.dot(K2.T, F), 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..ae08668 100644
--- a/multiviewunsynch/reconstruction/synchronization.py
+++ b/multiviewunsynch/reconstruction/synchronization.py
@@ -173,6 +173,51 @@ 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
+
 
 if __name__ == "__main__":
 
diff --git a/multiviewunsynch/thirdparty/camera_calibration_show_extrinsics.py b/multiviewunsynch/thirdparty/camera_calibration_show_extrinsics.py
index 31f2b1a..e9f1078 100755
--- a/multiviewunsynch/thirdparty/camera_calibration_show_extrinsics.py
+++ b/multiviewunsynch/thirdparty/camera_calibration_show_extrinsics.py
@@ -49,9 +49,9 @@ def create_camera_model(camera_matrix, width, height, scale_focal, draw_frame_ax
 
     # 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]
+    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))
diff --git a/multiviewunsynch/tools/visualization.py b/multiviewunsynch/tools/visualization.py
index e9922bf..75e1572 100644
--- a/multiviewunsynch/tools/visualization.py
+++ b/multiviewunsynch/tools/visualization.py
@@ -25,7 +25,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:
@@ -55,6 +55,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)
@@ -68,24 +70,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)
 
@@ -95,7 +101,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<r1-1)
@@ -104,13 +110,13 @@ def plot_epipolar_line(img1, img2, F, x1, x2):
     
     # plot epipolar lines in img2, which are calculated using key points in img1
     plt.subplot(122),plt.imshow(img2,cmap='gray')
-    for i in range(num):
+    for i in range(x1.shape[1]):
         line = line1[:,i]
         y2_coord = np.array([(line[2]+line[0]*x)/(-line[1]) for x in x2_coord])
         idx = (y2_coord>=0) & (y2_coord<r2-1)
         plt.plot(x2_coord[idx],y2_coord[idx])
     plt.scatter(x2[0],x2[1])
-
+    plt.savefig('epipolar_lines.png')
     plt.show()
 
 
@@ -162,10 +168,15 @@ def show_trajectory_3D(*X,title=None,color=True,line=False):
     plt.show()
 
 
-def show_2D_all(*x,title=None,color=True,line=True,text=False, bg=None):
+def show_2D_all(*x,title=None,color=True,line=True,text=False, bg=None, output_dir=''):
     plt.figure(figsize=(12, 10))
     if bg is not None:
+        bg = cv2.cvtColor(bg, cv2.COLOR_BGR2RGB)
         plt.imshow(bg)
+        h,w,_ = bg.shape
+        plt.xlim([0,w])
+        plt.ylim([h,0])
+
     num = len(x)
     for i in range(num):
         # plt.subplot(1,num,i+1)
@@ -192,9 +203,9 @@ def show_2D_all(*x,title=None,color=True,line=True,text=False, bg=None):
         plt.ylabel('Y')
     if title:
         plt.suptitle(title)
-        plt.savefig(title+'.png')
+        plt.savefig(output_dir+title+'.png')
     else:
-        plt.savefig('reprojected.png')
+        plt.savefig(output_dir+'reprojected.png')
     plt.show()
 
 
@@ -222,7 +233,7 @@ def draw_camera_extrinsics(flight, ax, scale_focal=40):
         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,flight=None):
+def show_3D_all(*X,title=None,color=True,line=True,flight=None, output_dir=''):
     fig = plt.figure(figsize=(20, 15))
     num = len(X)
     ax = fig.add_subplot(111,projection='3d')
@@ -249,26 +260,34 @@ def show_3D_all(*X,title=None,color=True,line=True,flight=None):
     # 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=1)
-        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 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})
     for handle in lgnd.legendHandles:
         handle.set_sizes([100])
     # plt.axis('off')
-    plt.savefig('reconstructed_scene.png')
+    if title:
+        plt.savefig(output_dir+title+'reconstructed_scene')
+    else:
+        plt.savefig(output_dir+'reconstructed_scene.png')
     plt.show()
 
 
@@ -339,7 +358,7 @@ def error_traj(traj,error,thres=0.5,title=None,colormap='Wistia',size=100, text=
     plt.title(title, fontsize=50)
     plt.show()
 
-def draw_detection_matches(img1, d1, img2, d2):
+def draw_detection_matches(img1, d1, img2, d2, title='detection_mathches.png', output_dir=''):
     '''
     Function:
         Draw the corresponding detections in the camera views
@@ -347,6 +366,7 @@ def draw_detection_matches(img1, d1, img2, d2):
         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)
@@ -356,7 +376,7 @@ def draw_detection_matches(img1, d1, img2, d2):
     outimg = cv2.drawMatches(img1, dp1, img2, dp2, matches, None)
 
     plt.imshow(outimg), plt.show()
-    cv2.imwrite('detection_mathches.png', outimg)    
+    cv2.imwrite(output_dir+title, outimg)    
 
 def draw_matches(img1, kp1, img2, kp2, matches, matchesMask):
     '''
@@ -368,6 +388,7 @@ def draw_matches(img1, kp1, img2, kp2, matches, matchesMask):
         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,

From 69317c0b0ffaa9a8ff6d0b069a4af554d80ab167 Mon Sep 17 00:00:00 2001
From: Tianyu Wu <t228wu@uwateroo.ca>
Date: Wed, 9 Jun 2021 23:01:58 +0200
Subject: [PATCH 11/25] debug pipeline for multiple cameras

---
 .../analysis/analysis_reconstruction.py       |  66 ++++-
 multiviewunsynch/main_static.py               |   3 +
 multiviewunsynch/main_static_dynamic.py       |   3 +
 multiviewunsynch/reconstruction/common.py     | 231 ++++++++++++++----
 multiviewunsynch/reconstruction/epipolar.py   |  13 +-
 .../reconstruction/synchronization.py         |  54 ++++
 6 files changed, 310 insertions(+), 60 deletions(-)

diff --git a/multiviewunsynch/analysis/analysis_reconstruction.py b/multiviewunsynch/analysis/analysis_reconstruction.py
index c059a32..1f1f95e 100644
--- a/multiviewunsynch/analysis/analysis_reconstruction.py
+++ b/multiviewunsynch/analysis/analysis_reconstruction.py
@@ -111,7 +111,7 @@ def convert_timestamps(gt_dets, alphas, betas):
 
 def main():
     # Output dir
-    output_dir = '/scratch2/wuti/Repos/mvus/experiments/eval_res/'
+    output_dir = '/scratch2/wuti/Repos/mvus/experiments/set12_obj0/eval_res_calibrated_10/'
 
     if not os.path.exists(output_dir):
         os.makedirs(output_dir, exist_ok=True)
@@ -119,6 +119,9 @@ def main():
     # Load ground truth
     gt_static_file = ['/scratch2/wuti/Repos/3D-Object-Trajectory-Reconstruction-Webcam/multiviewunsynch/webcam-datasets/nyc-datasets/static_gt/static_cam1.txt','/scratch2/wuti/Repos/3D-Object-Trajectory-Reconstruction-Webcam/multiviewunsynch/webcam-datasets/nyc-datasets/static_gt/static_cam2.txt']
     gt_dynamic_file = ['/scratch2/wuti/Repos/3D-Object-Trajectory-Reconstruction-Webcam/multiviewunsynch/webcam-datasets/nyc-datasets/20210413_1000_3min/det_opencv/set12/obj1/cam0_w1574280981_12_CSRT_obj0.txt','/scratch2/wuti/Repos/3D-Object-Trajectory-Reconstruction-Webcam/multiviewunsynch/webcam-datasets/nyc-datasets/20210413_1000_3min/det_opencv/set12/obj1/cam2_w1587769795_12_CSRT_obj0.txt']
+    
+    # gt_static_file = ['/scratch2/wuti/Repos/3D-Object-Trajectory-Reconstruction-Webcam/multiviewunsynch/webcam-datasets/nyc-datasets/static_gt/static_cam1_undistort_div.txt', '/scratch2/wuti/Repos/3D-Object-Trajectory-Reconstruction-Webcam/multiviewunsynch/webcam-datasets/nyc-datasets/static_gt/static_cam2_undistort_div.txt']
+    # gt_dynamic_file = ['/scratch2/wuti/Repos/3D-Object-Trajectory-Reconstruction-Webcam/multiviewunsynch/webcam-datasets/nyc-datasets/20210413_1000_3min/det_opencv/set12/obj1/cam0_w1574280981_12_CSRT_obj0_undistort_div.txt', '/scratch2/wuti/Repos/3D-Object-Trajectory-Reconstruction-Webcam/multiviewunsynch/webcam-datasets/nyc-datasets/20210413_1000_3min/det_opencv/set12/obj1/cam2_w1587769795_12_CSRT_obj0_undistort_div.txt']
 
     gt_static = []
     for gfs in gt_static_file:
@@ -129,12 +132,40 @@ def main():
         gt_dynamic.append(np.loadtxt(gfd, usecols=(2,0,1), delimiter=' ').T)
 
     # Load scenes
-    data_file_dynamic = '/scratch2/wuti/Repos/mvus/experiments/dynamic/nyc_colmap_dynamic_ori_inlier_30.pkl'
-    data_file_static = '/scratch2/wuti/Repos/mvus/experiments/static/nyc_colmap_static_superglue_30.pkl'
-    data_file_static_dynamic = '/scratch2/wuti/Repos/mvus/experiments/static_dynamic/nyc_colmap_static_dynamic_superglue_30.pkl'
-
-    with open(data_file_dynamic, 'rb') as file:
-        flight_dynamic = pickle.load(file)
+    # COLMAP GUESS
+    # data_file_dynamic = '/scratch2/wuti/Repos/mvus/experiments/dynamic/nyc_colmap_dynamic_ori_inlier_30.pkl'
+    # data_file_static = '/scratch2/wuti/Repos/mvus/experiments/static/nyc_colmap_static_superglue_colmap2_30.pkl'
+    # data_file_static_dynamic = '/scratch2/wuti/Repos/mvus/experiments/static_dynamic/nyc_colmap_static_dynamic_superglue_colmap2_30.pkl'
+    # data_file_static_then_dynamic = '/scratch2/wuti/Repos/mvus/experiments/static_then_dynamic/nyc_colmap_static_then_dynamic_superglue_30.pkl'
+    
+    # COLMAP GUESS 0
+    # data_file_dynamic = '/scratch2/wuti/Repos/mvus/experiments/set12_obj0/calibrated_dynamic/nyc_colmap_dynamic_ori_inlier_calibrated_30.pkl'
+    # data_file_static = '/scratch2/wuti/Repos/mvus/experiments/set12_obj0/colmap2_static/nyc_colmap_static_superglue_colmap2_30.pkl'
+    # data_file_static_dynamic = '/scratch2/wuti/Repos/mvus/experiments/set12_obj0/colmap2_static_dynamic/nyc_colmap_static_dynamic_superglue_colmap2_30.pkl'
+
+    # COLMAP GUESS 0
+    # data_file_dynamic = '/scratch2/wuti/Repos/mvus/experiments/set12_obj0/calibrated_dynamic/nyc_colmap_dynamic_ori_inlier_calibrated_30.pkl'
+    data_file_static = '/scratch2/wuti/Repos/mvus/experiments/set12_obj0/colmap3_static/nyc_colmap_static_superglue_colmap2_30.pkl'
+    data_file_static_dynamic = '/scratch2/wuti/Repos/mvus/experiments/set12_obj0/colmap3_static_dynamic/nyc_colmap_static_dynamic_superglue_colmap2_30.pkl'
+
+    # CALIBRATED
+    # data_file_dynamic = '/scratch2/wuti/Repos/mvus/experiments/set12_obj0/calibrated_dynamic/nyc_colmap_dynamic_ori_inlier_calibrated_30.pkl'
+    # data_file_static = '/scratch2/wuti/Repos/mvus/experiments/set12_obj0/calibrated_static/nyc_colmap_static_superglue_calibrated_30.pkl'
+    # data_file_static_dynamic = '/scratch2/wuti/Repos/mvus/experiments/set12_obj0/calibrated_static_dynamic/nyc_colmap_static_dynamic_superglue_calibrated_30.pkl'
+
+    # CALIBRATED F10
+    data_file_static = '/scratch2/wuti/Repos/mvus/experiments/set12_obj0/calibrated_static_10/nyc_colmap_static_superglue_calibrated_10.pkl'
+    data_file_static_dynamic = '/scratch2/wuti/Repos/mvus/experiments/set12_obj0/calibrated_static_dynamic_10/nyc_colmap_static_dynamic_superglue_calibrated_10.pkl'
+
+
+    # UNDISTORT
+    # data_file_dynamic = '/scratch2/wuti/Repos/mvus/experiments/set12_obj0/undistort_dynamic/nyc_colmap_dynamic_ori_inlier_undistort_30.pkl'
+    # data_file_static = '/scratch2/wuti/Repos/mvus/experiments/set12_obj0/undistort_static/nyc_colmap_static_superglue_undistort_30.pkl'
+    # data_file_static_dynamic = '/scratch2/wuti/Repos/mvus/experiments/set12_obj0/undistort_static_dynamic/nyc_colmap_static_dynamic_superglue_undistort_30.pkl'
+    # data_file_static_then_dynamic = '/scratch2/wuti/Repos/mvus/experiments/static_then_dynamic/nyc_colmap_static_then_dynamic_superglue_30.pkl'
+    
+    # with open(data_file_dynamic, 'rb') as file:
+    #     flight_dynamic = pickle.load(file)
 
     with open(data_file_static, 'rb') as file:
         flight_static = pickle.load(file)
@@ -142,15 +173,19 @@ def main():
     with open(data_file_static_dynamic, 'rb') as file:
         flight_static_dynamic = pickle.load(file)
 
+    # with open(data_file_static_then_dynamic, 'rb') as file:
+    #     flight_static_then_dynamic = pickle.load(file)
+
     # Analysis
     # 2D reprojection error
     print("Plot reprojection error")
-    # dynamic only
-    print("Reconstructions from dynamic-only setting")
-    # convert dynamic part timestamp
-    gt_dynamic1 = [gt_dynamic[x].copy() for x in flight_dynamic.sequence]
-    gt_dynamic1 = convert_timestamps(gt_dynamic1, flight_dynamic.alpha, flight_dynamic.beta)
-    reproject_ground_truth(flight_dynamic.cameras, gt_static, gt_dynamic1, output_dir=output_dir, prefix='dynamic_only_', flight=flight_dynamic)
+    
+    # # dynamic only
+    # print("Reconstructions from dynamic-only setting")
+    # # convert dynamic part timestamp
+    # gt_dynamic1 = [gt_dynamic[x].copy() for x in flight_dynamic.sequence]
+    # gt_dynamic1 = convert_timestamps(gt_dynamic1, flight_dynamic.alpha, flight_dynamic.beta)
+    # reproject_ground_truth(flight_dynamic.cameras, gt_static, gt_dynamic1, output_dir=output_dir, prefix='dynamic_only_', flight=flight_dynamic)
     
     print('\n#################################################################\n')
     print("Reconstructions from static-only setting")
@@ -170,6 +205,11 @@ def main():
     print("Reconstructions from static-only setting")
     reproject_ground_truth(flight_static.cameras, gt_static, gt_dynamic3, output_dir=output_dir, prefix='static_only_sync_', flight=flight_static)
 
+    # print('\n#################################################################\n')
+    # print("Reconstructions from static-then-dynamic setting")
+    # gt_dynamic4 = [gt_dynamic[x].copy() for x in flight_static_then_dynamic.sequence]
+    # gt_dynamic4 = convert_timestamps(gt_dynamic4, flight_static_then_dynamic.alpha, flight_static_then_dynamic.beta)
+    # reproject_ground_truth(flight_static_then_dynamic.cameras, gt_static, gt_dynamic4, output_dir=output_dir, prefix='static_then_dynamic_', flight=flight_static_then_dynamic)
 
     print('Finish!')
 
diff --git a/multiviewunsynch/main_static.py b/multiviewunsynch/main_static.py
index acd7c2b..079cb4a 100644
--- a/multiviewunsynch/main_static.py
+++ b/multiviewunsynch/main_static.py
@@ -63,6 +63,9 @@
         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)
 
diff --git a/multiviewunsynch/main_static_dynamic.py b/multiviewunsynch/main_static_dynamic.py
index a608cf7..b480d6f 100644
--- a/multiviewunsynch/main_static_dynamic.py
+++ b/multiviewunsynch/main_static_dynamic.py
@@ -95,6 +95,9 @@
         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], flight.sequence[:cam_temp], debug=args.debug)
 
diff --git a/multiviewunsynch/reconstruction/common.py b/multiviewunsynch/reconstruction/common.py
index 0d48035..61d6caf 100644
--- a/multiviewunsynch/reconstruction/common.py
+++ b/multiviewunsynch/reconstruction/common.py
@@ -356,10 +356,15 @@ def init_traj(self,error=10,inlier_only=False, debug=False):
     
     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
@@ -392,6 +397,7 @@ def init_traj_from_pose(self):
         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()
@@ -414,11 +420,15 @@ def init_traj_from_pose(self):
         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
 
@@ -488,10 +498,8 @@ def init_static(self, error=10, inlier_only=False, debug=False):
         # 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])
@@ -1308,53 +1316,81 @@ def register_new_camera_static(self, cam_id, cams, debug=False):
         '''
         if 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
+            # 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
-                # 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))
-
                 # 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_points()
+
         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
+            # if have not yet initialize cam_id in feature_dict, do so
+            if cam_id not in self.feature_dict.keys():
+                # 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 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])
-
-                # 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
-
+                # 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])
-                self.cameras[cam_id].index_2d_3d = np.union1d(self.cameras[cam_id].index_2d_3d, self.cameras[i].index_2d_3d[registered_ids])
+                
+                # # 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()
@@ -1384,8 +1420,8 @@ def get_camera_pose_static(self, cam_id, cams, error=8, verbose=0, debug=False):
         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]
-        self.cameras[cam_id].index_2d_3d = self.cameras[cam_id].index_2d_3d[inliers]
+        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,)
@@ -1521,16 +1557,17 @@ def triangulate_static(self, cam_id, cams, thres=0, verbose=0):
         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])
+        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]
+            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']:
@@ -1563,22 +1600,22 @@ def triangulate_static(self, cam_id, cams, thres=0, verbose=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], ids_3d_new))
+            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(add_cand_ids, new_ids).nonzeros()[0] + int(np.sum(self.inlier_mask))
+            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((add_cand_ids, new_ids[~added_mask]))
+            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[:, ~added_mask]])
+            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])
@@ -1650,7 +1687,91 @@ 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:
+            # 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)
+                # initialize feature_dict
+                if 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()
@@ -1800,6 +1921,7 @@ 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()
@@ -1831,6 +1953,29 @@ def time_shift_from_F(self, 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:
     """ 
@@ -2029,11 +2174,13 @@ def extract_features(self, img=None, method='sift'):
             print("extract features from given img")
             self.kp, self.des = ep.extract_SIFT_feature(img)
 
-    def get_points(self):
+    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
@@ -2086,7 +2233,7 @@ def create_scene(path_input):
         # load camera information
         camera = Camera(K=np.asfarray(cam['K-matrix']), d=np.asfarray(cam['distCoeff']), fps=cam['fps'], resolution=cam['resolution'], img_path=cam['img_path'])
         # extract features
-        if 'include_static' in config.keys() and config['include_static']:
+        if 'include_static' in config['settings'].keys() and config['settings']['include_static']:
             camera.extract_features(method=flight.settings['feature_extractor'])
         else:
             camera.read_img()
diff --git a/multiviewunsynch/reconstruction/epipolar.py b/multiviewunsynch/reconstruction/epipolar.py
index 3be61d9..296108a 100644
--- a/multiviewunsynch/reconstruction/epipolar.py
+++ b/multiviewunsynch/reconstruction/epipolar.py
@@ -700,7 +700,7 @@ def epipolar_pipeline(d1, d2, K1, K2, error, inlier_only, img1, img2):
 
     return X, P, inlier, mask
 
-def epipolar_pipeline_from_F(d1, d2, K1, K2, F, thres=5):
+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
@@ -718,12 +718,15 @@ def epipolar_pipeline_from_F(d1, d2, K1, K2, F, thres=5):
     '''
 
     # Compute sampson error and filter out outliers
-    serr = Sampson_error(util.homogeneous(d1),util.homogeneous(d2),F)
-    # inlier = np.zeros(len(serr))
-    # inlier[serr < thres] = 1
-    inlier = np.ones(len(serr))
+    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)
diff --git a/multiviewunsynch/reconstruction/synchronization.py b/multiviewunsynch/reconstruction/synchronization.py
index ae08668..59e4149 100644
--- a/multiviewunsynch/reconstruction/synchronization.py
+++ b/multiviewunsynch/reconstruction/synchronization.py
@@ -218,6 +218,60 @@ def search(beta_list, thres=5):
 
     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__":
 

From 755885d77b7826053409ab56d09126e9a626a836 Mon Sep 17 00:00:00 2001
From: Tianyu Wu <t228wu@uwaterloo.ca>
Date: Thu, 10 Jun 2021 22:04:52 +0200
Subject: [PATCH 12/25] add support for superglue matches

---
 multiviewunsynch/main_static.py              |   5 +-
 multiviewunsynch/main_static_dynamic.py      |   5 +-
 multiviewunsynch/main_static_then_dynamic.py |   5 +-
 multiviewunsynch/reconstruction/common.py    | 154 ++++++++++++-------
 multiviewunsynch/tools/util.py               |  18 +++
 5 files changed, 122 insertions(+), 65 deletions(-)

diff --git a/multiviewunsynch/main_static.py b/multiviewunsynch/main_static.py
index 079cb4a..069c63b 100644
--- a/multiviewunsynch/main_static.py
+++ b/multiviewunsynch/main_static.py
@@ -9,6 +9,7 @@
 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
@@ -104,9 +105,9 @@
     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['feature_extractor'] == 'sift':
+    if flight.settings['include_static']:
         for cam in flight.cameras:
-            cam.unpack_sift_kp()
+            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
index b480d6f..c2736b4 100644
--- a/multiviewunsynch/main_static_dynamic.py
+++ b/multiviewunsynch/main_static_dynamic.py
@@ -11,6 +11,7 @@
 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
@@ -164,9 +165,9 @@
     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['feature_extractor'] == 'sift':
+    if flight.settings['include_static']:
         for cam in flight.cameras:
-            cam.unpack_sift_kp()
+            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
index d271e0c..b224172 100644
--- a/multiviewunsynch/main_static_then_dynamic.py
+++ b/multiviewunsynch/main_static_then_dynamic.py
@@ -9,6 +9,7 @@
 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
@@ -212,9 +213,9 @@
     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['feature_extractor'] == 'sift':
+    if flight.settings['include_static']:
         for cam in flight.cameras:
-            cam.unpack_sift_kp()
+            cam.kp = unpack_sift_kp(cam.kp)
     pickle.dump(flight, f)
 
 print('Finished!')
\ No newline at end of file
diff --git a/multiviewunsynch/reconstruction/common.py b/multiviewunsynch/reconstruction/common.py
index 61d6caf..1f1fb73 100644
--- a/multiviewunsynch/reconstruction/common.py
+++ b/multiviewunsynch/reconstruction/common.py
@@ -432,63 +432,70 @@ def init_static(self, error=10, inlier_only=False, debug=False):
         t1, t2 = self.sequence[0], self.sequence[1]
         K1, K2 = self.cameras[t1].K, self.cameras[t2].K
 
-        if debug:
-            # in debug, use static ground truth as 2d featues
-            if self.settings['undist_points']:
-                # undistort the ground truth matches
-                pts1 = self.cameras[t1].undist_point(self.cameras[t1].gt_pts.T, self.settings['undist_method']).T
-                pts2 = self.cameras[t2].undist_point(self.cameras[t2].gt_pts.T, self.settings['undist_method']).T
+        # if feature_dict is empty
+        if 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[0]),pts1]), self.cameras[t2].img, np.vstack([np.zeros(pts2.shape[0]),pts2]))
 
-                # plot the ground truth matches
-                # FIXME: check pts_dist == gt_pts
-                pts1_dist = self.cameras[t1].dist_point2d(pts1.T, method=self.settings['undist_method'])
-                pts2_dist = self.cameras[t2].dist_point2d(pts2.T, method=self.settings['undist_method'])
-                # vis.draw_detection_matches(self.cameras[t1].img, np.vstack([np.zeros(pts1_dist.shape[1]),pts1_dist]), self.cameras[t2].img, np.vstack([np.zeros(pts2_dist.shape[1]),pts2_dist]))
-                vis.draw_detection_matches(self.cameras[t1].img, np.vstack([np.zeros(pts1.shape[0]),self.cameras[t1].gt_pts.T]), self.cameras[t2].img, np.vstack([np.zeros(pts2.shape[0]),self.cameras[t2].gt_pts.T]))
+                # 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])
 
-            else:
-                pts1 = self.cameras[t1].gt_pts
-                pts2 = self.cameras[t2].gt_pts
+                # 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]))
             
-                vis.draw_detection_matches(self.cameras[t1].img, np.vstack([np.zeros(pts1.shape[0]),pts1.T]), self.cameras[t2].img, np.vstack([np.zeros(pts2.shape[0]),pts2.T]))
+            else:
+                # Match features with sift
+                if 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]
 
-            # 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])
+                    # 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, self.cameras[t1].gt_pts.shape[0]))
-            match_res2 = -np.ones((self.numCam, self.cameras[t2].gt_pts.shape[0]))
+                    # 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 features
-            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]
-
-            # undistort the matched keypoints
-            if self.settings['undist_points']:
-                pts1 = self.cameras[t1].undist_point(np.array(pts1).T, self.settings['undist_method']).T
-                pts2 = self.cameras[t2].undist_point(np.array(pts2).T, self.settings['undist_method']).T
-
-            # 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)))
-
-        # 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
-        
+            match_res = self.feature_dict[t1][t2]
+            query_ids = np.where(match_res > -1)[0]
+            train_ids = match_res[match_res > -1]
+
+            # 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(query_ids))]
+            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)
+
+        # undistort the matched keypoints
+        if self.settings['undist_points']:
+            pts1 = self.cameras[t1].undist_point(pts1, self.settings['undist_method']).T
+            pts2 = self.cameras[t2].undist_point(pts2, self.settings['undist_method']).T
+
         pts1 = np.int32(pts1).T
         pts2 = np.int32(pts2).T
 
@@ -1339,11 +1346,14 @@ def register_new_camera_static(self, cam_id, cams, debug=False):
             pts_2d = self.cameras[cam_id].get_gt_points()
 
         else:
-            # get the features and descriptors of the new camera
-            kp1, des1 = self.cameras[cam_id].kp, self.cameras[cam_id].des
-
             # 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
@@ -1702,8 +1712,19 @@ def select_next_camera_static(self, init=False, debug=False):
             return
         
         if init:
-            # take the first two cameras as initial pairs
-            self.sequence = [0,1]
+            # 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 = []
@@ -2192,9 +2213,6 @@ def get_gt_pts(self):
         get the registered 2d ground truth points (2, n)
         '''
         return self.gt_pts[self.index_registered_2d].T
-    
-    def unpack_sift_kp(self):
-        self.kp = np.array([kp.pt for kp in self.kp])
 
 def create_scene(path_input):
     '''
@@ -2217,6 +2235,18 @@ 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['necessary inputs']['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']
@@ -2234,7 +2264,13 @@ def create_scene(path_input):
         camera = Camera(K=np.asfarray(cam['K-matrix']), d=np.asfarray(cam['distCoeff']), fps=cam['fps'], resolution=cam['resolution'], img_path=cam['img_path'])
         # extract features
         if 'include_static' in config['settings'].keys() and config['settings']['include_static']:
-            camera.extract_features(method=flight.settings['feature_extractor'])
+            # if using sift as feature_method, also extract sift features
+            if feature_method[0] == 'sift':
+                camera.extract_features(method=flight.settings['feature_extractor'])
+            elif feature_method[0] == 'superglue':
+                camera.kp = util.convert_kpts(kpt_list[i])
+            else:
+                raise Exception("Unsupported feature extraction and matching method")
         else:
             camera.read_img()
 
diff --git a/multiviewunsynch/tools/util.py b/multiviewunsynch/tools/util.py
index 6a33881..90ecd32 100644
--- a/multiviewunsynch/tools/util.py
+++ b/multiviewunsynch/tools/util.py
@@ -206,6 +206,24 @@ 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)
     x,y,z = rotation_decompose(R)

From 8481631443f22f129492b3df088053ae2a7f7194 Mon Sep 17 00:00:00 2001
From: Tianyu Wu <t228wu@uwaterloo.ca>
Date: Fri, 11 Jun 2021 01:17:43 +0200
Subject: [PATCH 13/25] debug static-only with superglue using multiple cameras

---
 multiviewunsynch/reconstruction/common.py | 64 +++++++++++++++++------
 multiviewunsynch/tools/visualization.py   | 10 ++--
 2 files changed, 52 insertions(+), 22 deletions(-)

diff --git a/multiviewunsynch/reconstruction/common.py b/multiviewunsynch/reconstruction/common.py
index 1f1fb73..f25d0c4 100644
--- a/multiviewunsynch/reconstruction/common.py
+++ b/multiviewunsynch/reconstruction/common.py
@@ -18,6 +18,7 @@
 from mpl_toolkits.mplot3d import Axes3D
 from tools import visualization as vis
 from tools import util 
+import pickle
 
 
 class Scene:
@@ -433,13 +434,13 @@ def init_static(self, error=10, inlier_only=False, debug=False):
         K1, K2 = self.cameras[t1].K, self.cameras[t2].K
 
         # if feature_dict is empty
-        if self.feature_dict:
+        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[0]),pts1]), self.cameras[t2].img, np.vstack([np.zeros(pts2.shape[0]),pts2]))
+                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])
@@ -451,7 +452,7 @@ def init_static(self, error=10, inlier_only=False, debug=False):
             
             else:
                 # Match features with sift
-                if settings['feature_method'][0] == '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)
@@ -482,22 +483,35 @@ def init_static(self, error=10, inlier_only=False, debug=False):
         else:
             match_res = self.feature_dict[t1][t2]
             query_ids = np.where(match_res > -1)[0]
-            train_ids = match_res[match_res > -1]
+            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(query_ids))]
-            matchesMask = [[1,0] for i in range(len(query_ids))]
+            # # 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']).T
-            pts2 = self.cameras[t2].undist_point(pts2, self.settings['undist_method']).T
+            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).T
-        pts2 = np.int32(pts2).T
+        pts1 = np.int32(pts1)
+        pts2 = np.int32(pts2)
 
         # go through the epipolar pipeline for pose estimation and initial scene reconstruction
         # if debug:
@@ -516,12 +530,17 @@ def init_static(self, error=10, inlier_only=False, debug=False):
         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]
+
+            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]
@@ -1731,14 +1750,24 @@ def select_next_camera_static(self, init=False, debug=False):
             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
-                if debug:
+                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)))
@@ -2238,7 +2267,7 @@ def create_scene(path_input):
     
     # Load superglue matches or ground truth
     if 'include_static' in config['settings'].keys() and config['settings']['include_static']:
-        feature_method = config['necessary inputs']['feature_method']
+        feature_method = config['settings']['feature_method']
         # if use superglue, load the preprocessed superpoints and matching result
         if feature_method[0] == 'superglue':
             try:
@@ -2266,8 +2295,9 @@ def create_scene(path_input):
         if 'include_static' in config['settings'].keys() and config['settings']['include_static']:
             # if using sift as feature_method, also extract sift features
             if feature_method[0] == 'sift':
-                camera.extract_features(method=flight.settings['feature_extractor'])
+                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")
diff --git a/multiviewunsynch/tools/visualization.py b/multiviewunsynch/tools/visualization.py
index 75e1572..8cea3bc 100644
--- a/multiviewunsynch/tools/visualization.py
+++ b/multiviewunsynch/tools/visualization.py
@@ -120,7 +120,7 @@ def plot_epipolar_line(img1, img2, F, x1, x2):
     plt.show()
 
 
-def show_trajectory_2D(*x,title=None,color=True,line=False,text=False):
+def show_trajectory_2D(*x, title=None,color=True,line=False,text=False):
     plt.figure(figsize=(12, 10))
     num = len(x)
     for i in range(num):
@@ -143,7 +143,7 @@ def show_trajectory_2D(*x,title=None,color=True,line=False,text=False):
     plt.show()
 
 
-def show_trajectory_3D(*X,title=None,color=True,line=False):
+def show_trajectory_3D(*X, title=None,color=True,line=False):
     fig = plt.figure(figsize=(12, 10))
     num = len(X)
     for i in range(num):
@@ -168,7 +168,7 @@ def show_trajectory_3D(*X,title=None,color=True,line=False):
     plt.show()
 
 
-def show_2D_all(*x,title=None,color=True,line=True,text=False, bg=None, output_dir=''):
+def show_2D_all(*x, title=None,color=True,line=True,text=False, bg=None, output_dir=''):
     plt.figure(figsize=(12, 10))
     if bg is not None:
         bg = cv2.cvtColor(bg, cv2.COLOR_BGR2RGB)
@@ -233,7 +233,7 @@ def draw_camera_extrinsics(flight, ax, scale_focal=40):
         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,flight=None, output_dir=''):
+def show_3D_all(*X, title=None,color=True,line=True,flight=None, output_dir=''):
     fig = plt.figure(figsize=(20, 15))
     num = len(X)
     ax = fig.add_subplot(111,projection='3d')
@@ -291,7 +291,7 @@ def show_3D_all(*X,title=None,color=True,line=True,flight=None, output_dir=''):
     plt.show()
 
 
-def show_spline(*spline,title=None):
+def show_spline(*spline, title=None):
 
     num = len(spline)
     for i in range(num):

From d243a3f1571c2ad96b55673e2dc7c6df83e05710 Mon Sep 17 00:00:00 2001
From: Tianyu Wu <t228wu@uwaterloo.ca>
Date: Tue, 15 Jun 2021 14:55:45 +0200
Subject: [PATCH 14/25] debug static-dynamic with the support for superglue

---
 .../analysis/analysis_reconstruction.py       |   4 +-
 multiviewunsynch/main.py                      |   3 +
 multiviewunsynch/main_static.py               |   4 +
 multiviewunsynch/main_static_dynamic.py       |   3 +
 multiviewunsynch/reconstruction/common.py     | 168 +++++++++++-------
 5 files changed, 119 insertions(+), 63 deletions(-)

diff --git a/multiviewunsynch/analysis/analysis_reconstruction.py b/multiviewunsynch/analysis/analysis_reconstruction.py
index 1f1f95e..a424132 100644
--- a/multiviewunsynch/analysis/analysis_reconstruction.py
+++ b/multiviewunsynch/analysis/analysis_reconstruction.py
@@ -117,8 +117,8 @@ def main():
         os.makedirs(output_dir, exist_ok=True)
 
     # Load ground truth
-    gt_static_file = ['/scratch2/wuti/Repos/3D-Object-Trajectory-Reconstruction-Webcam/multiviewunsynch/webcam-datasets/nyc-datasets/static_gt/static_cam1.txt','/scratch2/wuti/Repos/3D-Object-Trajectory-Reconstruction-Webcam/multiviewunsynch/webcam-datasets/nyc-datasets/static_gt/static_cam2.txt']
-    gt_dynamic_file = ['/scratch2/wuti/Repos/3D-Object-Trajectory-Reconstruction-Webcam/multiviewunsynch/webcam-datasets/nyc-datasets/20210413_1000_3min/det_opencv/set12/obj1/cam0_w1574280981_12_CSRT_obj0.txt','/scratch2/wuti/Repos/3D-Object-Trajectory-Reconstruction-Webcam/multiviewunsynch/webcam-datasets/nyc-datasets/20210413_1000_3min/det_opencv/set12/obj1/cam2_w1587769795_12_CSRT_obj0.txt']
+    gt_static_file = ['../experiments/croatia_set3/static_gt/static_cam0.txt','../experiments/croatia_set3/static_gt/static_cam1.txt']
+    gt_dynamic_file = ['../experiments/croatia_set3/det_gt/cam0_3_det_gt.txt','../experiments/croatia_set3/det_gt/cam1_3_det_gt.txt']
     
     # gt_static_file = ['/scratch2/wuti/Repos/3D-Object-Trajectory-Reconstruction-Webcam/multiviewunsynch/webcam-datasets/nyc-datasets/static_gt/static_cam1_undistort_div.txt', '/scratch2/wuti/Repos/3D-Object-Trajectory-Reconstruction-Webcam/multiviewunsynch/webcam-datasets/nyc-datasets/static_gt/static_cam2_undistort_div.txt']
     # gt_dynamic_file = ['/scratch2/wuti/Repos/3D-Object-Trajectory-Reconstruction-Webcam/multiviewunsynch/webcam-datasets/nyc-datasets/20210413_1000_3min/det_opencv/set12/obj1/cam0_w1574280981_12_CSRT_obj0_undistort_div.txt', '/scratch2/wuti/Repos/3D-Object-Trajectory-Reconstruction-Webcam/multiviewunsynch/webcam-datasets/nyc-datasets/20210413_1000_3min/det_opencv/set12/obj1/cam2_w1587769795_12_CSRT_obj0_undistort_div.txt']
diff --git a/multiviewunsynch/main.py b/multiviewunsynch/main.py
index 5ce5f7f..237a0c3 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")
@@ -87,6 +88,8 @@
 vis.show_trajectory_3D(flight.traj[1:],line=False)
 # save the 2d trajectories
 if 'save_2d' in flight.settings.keys() and flight.settings['save_2d']:
+    if not os.path.exists(os.path.dirname(flight.settings['save_2d_path'])):
+        os.makedirs(os.path.dirname(flight.settings['save_2d_path']))
     for i, cam in enumerate(flight.cameras):
         x_res = cam.dist_point3d(flight.traj[1:])
         x_ori = flight.detections[i][1:]
diff --git a/multiviewunsynch/main_static.py b/multiviewunsynch/main_static.py
index 069c63b..a167feb 100644
--- a/multiviewunsynch/main_static.py
+++ b/multiviewunsynch/main_static.py
@@ -103,6 +103,10 @@
 # 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']:
diff --git a/multiviewunsynch/main_static_dynamic.py b/multiviewunsynch/main_static_dynamic.py
index c2736b4..c1ada5c 100644
--- a/multiviewunsynch/main_static_dynamic.py
+++ b/multiviewunsynch/main_static_dynamic.py
@@ -115,6 +115,9 @@
 flight.spline_to_traj(sampling_rate=1)
 # save the 2d trajectories
 if 'save_2d' in flight.settings.keys() and flight.settings['save_2d']:
+    if not os.path.exists(os.path.dirname(flight.settings['save_2d_path'])):
+        os.makedirs(os.path.dirname(flight.settings['save_2d_path']))
+
     for i, cam in enumerate(flight.cameras):
         x_res = cam.dist_point3d(flight.traj[1:])
         x_ori = flight.detections[i][1:]
diff --git a/multiviewunsynch/reconstruction/common.py b/multiviewunsynch/reconstruction/common.py
index f25d0c4..d624e1d 100644
--- a/multiviewunsynch/reconstruction/common.py
+++ b/multiviewunsynch/reconstruction/common.py
@@ -228,83 +228,129 @@ def init_traj(self,error=10,inlier_only=False, debug=False):
         
         # add the static part
         if 'include_static' in self.settings.keys() and self.settings['include_static']:
-            if debug:
-                # in debug, use static ground truth as 2d featues
-                if self.settings['undist_points']:
-                    # undistort the ground truth matches
-                    pts1 = self.cameras[t1].undist_point(self.cameras[t1].gt_pts.T, self.settings['undist_method']).T
-                    pts2 = self.cameras[t2].undist_point(self.cameras[t2].gt_pts.T, self.settings['undist_method']).T
-
-                    # plot the ground truth matches
-                    # FIXME: check pts_dist == gt_pts
-                    pts1_dist = self.cameras[t1].dist_point2d(pts1.T, method=self.settings['undist_method'])
-                    pts2_dist = self.cameras[t2].dist_point2d(pts2.T, method=self.settings['undist_method'])
-                    # vis.draw_detection_matches(self.cameras[t1].img, np.vstack([np.zeros(pts1_dist.shape[1]),pts1_dist]), self.cameras[t2].img, np.vstack([np.zeros(pts2_dist.shape[1]),pts2_dist]))
-                    vis.draw_detection_matches(self.cameras[t1].img, np.vstack([np.zeros(pts1.shape[0]),self.cameras[t1].gt_pts.T]), self.cameras[t2].img, np.vstack([np.zeros(pts2.shape[0]),self.cameras[t2].gt_pts.T]))
+            # 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]))
 
-                else:
-                    pts1 = self.cameras[t1].gt_pts
-                    pts2 = self.cameras[t2].gt_pts
+                    # 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]))
                 
-                    vis.draw_detection_matches(self.cameras[t1].img, np.vstack([np.zeros(pts1.shape[0]),pts1.T]), self.cameras[t2].img, np.vstack([np.zeros(pts2.shape[0]),pts2.T]))
+                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:
-                
-                # sp1, sp2 = util.match_features(self.cameras[t1].img, self.cameras[t2].img, 'sift', 'bf', 0.7)
+                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)
 
-                # Match features
-                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)
-                
-                # undistort the matched keypoints
-                if self.settings['undist_points']:
-                    pts1 = self.cameras[t1].undist_point(np.array(pts1).T, self.settings['undist_method']).T
-                    pts2 = self.cameras[t2].undist_point(np.array(pts2).T, self.settings['undist_method']).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]))
+
+
+            # 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).T, d1[1:]])
-            fp2 = np.hstack([np.int32(pts2).T, d2[1:]])
+            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 < len(pts1)]
-            inlier_traj = idx_mask[idx_mask >= len(pts1)] - len(pts1)
+            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 < len(pts1)]
+            self.static = X[:-1, idx_mask < pts1.shape[1]]
             self.inlier_mask = np.ones(self.static.shape[1])
 
-            # 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
+            # also draw the inlier matches
+            if self.settings['feature_method'][0] == 'sift':
                 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)
+            
+            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])
 
-                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)))
+                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]
@@ -312,14 +358,14 @@ def init_traj(self,error=10,inlier_only=False, debug=False):
             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
+            # 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
+            # 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 >= len(pts1)]))
+            self.traj = np.vstack((d1[0][inlier_traj], X[:-1, idx_mask >= pts1.shape[1]]))
         
         # only uses the detections for pose estimation
         else:

From 7d66da6d9f9721e8d4f8a3015fbe2f39b7efc53e Mon Sep 17 00:00:00 2001
From: Tianyu Wu <t228wu@uwateroo.ca>
Date: Tue, 15 Jun 2021 14:56:33 +0200
Subject: [PATCH 15/25] modify evaluation plots

---
 .../analysis/analysis_reconstruction.py       | 75 ++++++++++++++++++-
 multiviewunsynch/reconstruction/epipolar.py   | 26 +++++++
 multiviewunsynch/tools/visualization.py       | 43 ++++++++---
 3 files changed, 128 insertions(+), 16 deletions(-)

diff --git a/multiviewunsynch/analysis/analysis_reconstruction.py b/multiviewunsynch/analysis/analysis_reconstruction.py
index 1f1f95e..2b6809d 100644
--- a/multiviewunsynch/analysis/analysis_reconstruction.py
+++ b/multiviewunsynch/analysis/analysis_reconstruction.py
@@ -10,8 +10,9 @@
 from itertools import combinations
 from matplotlib import pyplot as plt
 import os
+import cv2
 
-def reproject_ground_truth(cameras, gt_pts, gt_dets, n_bins=10, output_dir='', prefix='', flight=None):
+def reproject_ground_truth(cameras, gt_pts, gt_dets, ref_cam=0, output_dir='', prefix='', flight=None, ax=None):
     '''
     Function:
         Compute and plot the reprojection errors of the ground truth matches
@@ -20,7 +21,63 @@ def reproject_ground_truth(cameras, gt_pts, gt_dets, n_bins=10, output_dir='', p
         gt_pts = the ground truth matches
         n_bins = number of bins for plotting the histogram of the errors
     '''
+
+    # 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)
+        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+prefix)
+
+    gt_err = []
+    traj_err = []
+    gt_labels = []
+    traj_labels = []
+    for i, (cam, gt_pt, gt_det) in enumerate(zip(cameras, gt_pts, gt_dets)):
+        # reproject to image
+        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 camera %d is %f" %(i, np.mean(err1)))
+
+        # reproject trajectories
+        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 trajectory in camera %d is %f" %(i, np.mean(err2)))
+
+        # plot reprojected points on the image
+        vis.show_2D_all(gt_pt.T, gt_repro, gt_det[1:], traj_repro, title=prefix+'cam'+str(i)+' ground truth reprojection', color=True, line=False, bg=cam.img, output_dir=output_dir)
+
+    # plot the reprojection error boxplot
+    vis.error_boxplot(gt_err, gt_labels, title=prefix+'reprojection_error_static.png', ax=ax[0])
+    vis.error_boxplot(traj_err, traj_labels, title=prefix+'reprojection_error_dynamic.png', ax=ax[1])
+
+    return gt_err, gt_labels, traj_err, traj_labels
+
+def reproject_ground_truth_2views(cameras, gt_pts, gt_dets, ref_cam=0, n_bins=10, output_dir='', prefix='', flight=None, ax=None):
     combos = combinations(range(len(cameras)),2)
 
     repro_errors = []
@@ -94,6 +151,8 @@ def reproject_ground_truth(cameras, gt_pts, gt_dets, n_bins=10, output_dir='', p
         # plot 3D reconstructe scene
         vis.show_3D_all(X_gt, np.empty([3,0]), Traj_gt, np.empty([3,0]), color=False, line=False, flight=flight, output_dir=output_dir+prefix)
 
+    
+
 def convert_timestamps(gt_dets, alphas, betas):
     '''
     Function:
@@ -187,24 +246,32 @@ def main():
     # gt_dynamic1 = convert_timestamps(gt_dynamic1, flight_dynamic.alpha, flight_dynamic.beta)
     # reproject_ground_truth(flight_dynamic.cameras, gt_static, gt_dynamic1, output_dir=output_dir, prefix='dynamic_only_', flight=flight_dynamic)
     
+    _, axs1 = plt.subplots(3, 1, sharey=True, tight_layout=True)
+    _, axs2 = plt.subplots(3, 1, sharey=True, tight_layout=True)
+
     print('\n#################################################################\n')
     print("Reconstructions from static-only setting")
     gt_dynamic2 = [gt_dynamic[x].copy() for x in flight_static_dynamic.sequence]
     # no optimization for synchronization, use fps to convert timestamps
     for i, cam in enumerate(flight_static.cameras):
         gt_dynamic2[i][0] *= flight_static.cameras[flight_static.ref_cam].fps/cam.fps
-    reproject_ground_truth(flight_static.cameras, gt_static, gt_dynamic2, output_dir=output_dir, prefix='static_only_', flight=flight_static)
+    gt_err1, gt_label1, traj_err1, traj_label1 = reproject_ground_truth(flight_static.cameras, gt_static, gt_dynamic2,ref_cam=flight_static_dynamic.ref_cam, output_dir=output_dir, prefix='static_only_', flight=flight_static, ax=[axs1[0],axs2[0]])
 
     print('\n#################################################################\n')
     print("Reconstructions from static-dynamic setting")
     gt_dynamic3 = [gt_dynamic[x].copy() for x in flight_static_dynamic.sequence]
     gt_dynamic3 = convert_timestamps(gt_dynamic3, flight_static_dynamic.alpha, flight_static_dynamic.beta)
-    reproject_ground_truth(flight_static_dynamic.cameras, gt_static, gt_dynamic3, output_dir=output_dir, prefix='static_dynamic_', flight=flight_static_dynamic)
+    gt_err2, gt_label2, traj_err2, traj_label2 = reproject_ground_truth(flight_static_dynamic.cameras, gt_static, gt_dynamic3,ref_cam=flight_static_dynamic.ref_cam, output_dir=output_dir, prefix='static_dynamic_', flight=flight_static_dynamic, ax=[axs1[1],axs2[1]])
 
     print('\n#################################################################\n')
     print("Reconstructions from static-only setting")
-    reproject_ground_truth(flight_static.cameras, gt_static, gt_dynamic3, output_dir=output_dir, prefix='static_only_sync_', flight=flight_static)
+    gt_err3, gt_label3, traj_err3, traj_label3 = reproject_ground_truth(flight_static.cameras, gt_static, gt_dynamic3,ref_cam=flight_static_dynamic.ref_cam, output_dir=output_dir, prefix='static_only_sync_', flight=flight_static, ax=[axs1[2],axs2[2]])
+
+    vis.error_boxplot(gt_err1+gt_err2+gt_err3, gt_label1+gt_label2+gt_label3, title='reprojection_error_static.png')
+    vis.error_boxplot(traj_err1+traj_err2+traj_err3, traj_label1+traj_label2+traj_label3, title='reprojection_error_dynamic.png')
+
 
+    plt.show()
     # print('\n#################################################################\n')
     # print("Reconstructions from static-then-dynamic setting")
     # gt_dynamic4 = [gt_dynamic[x].copy() for x in flight_static_then_dynamic.sequence]
diff --git a/multiviewunsynch/reconstruction/epipolar.py b/multiviewunsynch/reconstruction/epipolar.py
index 296108a..5539934 100644
--- a/multiviewunsynch/reconstruction/epipolar.py
+++ b/multiviewunsynch/reconstruction/epipolar.py
@@ -509,6 +509,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):
diff --git a/multiviewunsynch/tools/visualization.py b/multiviewunsynch/tools/visualization.py
index 8cea3bc..c723edf 100644
--- a/multiviewunsynch/tools/visualization.py
+++ b/multiviewunsynch/tools/visualization.py
@@ -168,7 +168,7 @@ def show_trajectory_3D(*X, title=None,color=True,line=False):
     plt.show()
 
 
-def show_2D_all(*x, title=None,color=True,line=True,text=False, bg=None, output_dir=''):
+def show_2D_all(*x, title=None,color=True,line=True,text=False, bg=None, output_dir='', label=[]):
     plt.figure(figsize=(12, 10))
     if bg is not None:
         bg = cv2.cvtColor(bg, cv2.COLOR_BGR2RGB)
@@ -183,7 +183,8 @@ def show_2D_all(*x, title=None,color=True,line=True,text=False, bg=None, output_
 
         c = ['r','b','r','g']
         m = ['o','x','o','+']
-        label = ['Raw points', 'Reconstruction points','Raw detections', 'Reconstructed trajectories']
+        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:
@@ -206,7 +207,8 @@ def show_2D_all(*x, title=None,color=True,line=True,text=False, bg=None, output_
         plt.savefig(output_dir+title+'.png')
     else:
         plt.savefig(output_dir+'reprojected.png')
-    plt.show()
+    plt.legend(loc=1, prop={'size': 10})
+    # plt.show()
 
 
 def draw_camera_extrinsics(flight, ax, scale_focal=40):
@@ -233,20 +235,22 @@ def draw_camera_extrinsics(flight, ax, scale_focal=40):
         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,flight=None, output_dir=''):
+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')
 
     for i in range(num):
         if color:
-            c = ['r','g']
-            m = ['o','x']
-            label = ['RTK ground truth', 'Reconstruction Spline']
+            c = ['r','b','r','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:
@@ -259,7 +263,7 @@ def show_3D_all(*X, title=None,color=True,line=True,flight=None, output_dir=''):
     
     # 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=1)
+        draw_camera_extrinsics(flight, ax, scale_focal=.5)
         # if exists trajectory, also plot it
         if len(flight.traj) > 0:
             if color:
@@ -280,7 +284,7 @@ def show_3D_all(*X, title=None,color=True,line=True,flight=None, output_dir=''):
     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=1, prop={'size': 15})
     for handle in lgnd.legendHandles:
         handle.set_sizes([100])
     # plt.axis('off')
@@ -288,7 +292,7 @@ def show_3D_all(*X, title=None,color=True,line=True,flight=None, output_dir=''):
         plt.savefig(output_dir+title+'reconstructed_scene')
     else:
         plt.savefig(output_dir+'reconstructed_scene.png')
-    plt.show()
+    # plt.show()
 
 
 def show_spline(*spline, title=None):
@@ -358,6 +362,21 @@ 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):
+    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(sharey=True)
+    
+    ax.boxplot(err, labels=labels)
+    
+    if title is not None:
+        ax.set_title(title)
+    
+    return ax
+    
+
+
 def draw_detection_matches(img1, d1, img2, d2, title='detection_mathches.png', output_dir=''):
     '''
     Function:

From 6408c76988077747ab963ed6e7ea93f0eb6816a1 Mon Sep 17 00:00:00 2001
From: Tianyu Wu <t228wu@uwaterloo.ca>
Date: Wed, 16 Jun 2021 10:20:35 +0200
Subject: [PATCH 16/25] modify visualization to include histograms

---
 .../analysis/analysis_reconstruction.py       | 159 ++++++++++++++----
 multiviewunsynch/reconstruction/common.py     |   6 +-
 .../camera_calibration_show_extrinsics.py     |   9 +-
 multiviewunsynch/tools/visualization.py       |  42 ++++-
 4 files changed, 169 insertions(+), 47 deletions(-)

diff --git a/multiviewunsynch/analysis/analysis_reconstruction.py b/multiviewunsynch/analysis/analysis_reconstruction.py
index a2ee3cc..af654b0 100644
--- a/multiviewunsynch/analysis/analysis_reconstruction.py
+++ b/multiviewunsynch/analysis/analysis_reconstruction.py
@@ -33,6 +33,7 @@ def reproject_ground_truth(cameras, gt_pts, gt_dets, ref_cam=0, output_dir='', p
         # 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])
     
@@ -53,29 +54,44 @@ def reproject_ground_truth(cameras, gt_pts, gt_dets, ref_cam=0, output_dir='', p
     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)):
-        # reproject to image
+        # 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)))
+
+        # 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 camera %d is %f" %(i, np.mean(err1)))
-
-        # reproject trajectories
+        print("mean reprojection error of the ground truth matches in camera %d is %f" %(i, 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 trajectory in camera %d is %f" %(i, np.mean(err2)))
+        print("mean reprojection error of the dynamic features in camera %d is %f" %(i, np.mean(err2)))
 
         # plot reprojected points on the image
-        vis.show_2D_all(gt_pt.T, gt_repro, gt_det[1:], traj_repro, title=prefix+'cam'+str(i)+' ground truth reprojection', color=True, line=False, bg=cam.img, output_dir=output_dir)
+        vis.show_2D_all(gt_pt.T, gt_repro, cam.dist_point2d(traj_pts2d[2*i:2*i+2]), traj_repro, title=prefix+'cam'+str(i)+' ground truth reprojection', color=True, line=False, bg=cam.img, output_dir=output_dir)
 
     # plot the reprojection error boxplot
-    vis.error_boxplot(gt_err, gt_labels, title=prefix+'reprojection_error_static.png', ax=ax[0])
+    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])
 
-    return gt_err, gt_labels, traj_err, traj_labels
+    return gt_err, gt_labels, traj_err, traj_labels, match_err, match_labels
 
 def reproject_ground_truth_2views(cameras, gt_pts, gt_dets, ref_cam=0, n_bins=10, output_dir='', prefix='', flight=None, ax=None):
     combos = combinations(range(len(cameras)),2)
@@ -162,7 +178,7 @@ def convert_timestamps(gt_dets, alphas, betas):
         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
 
@@ -170,7 +186,10 @@ def convert_timestamps(gt_dets, alphas, betas):
 
 def main():
     # Output dir
-    output_dir = '/scratch2/wuti/Repos/mvus/experiments/set12_obj0/eval_res_calibrated_10/'
+    output_dir = '../experiments/croatia_set3/eval_res_calibrated_30/'
+    # output_dir = '../experiments/croatia_set3/eval_res_undistorted_30/'
+
+    # output_dir = '../experiments/nyc_set12/eval_res_calibrated_30/'
 
     if not os.path.exists(output_dir):
         os.makedirs(output_dir, exist_ok=True)
@@ -179,9 +198,12 @@ def main():
     gt_static_file = ['../experiments/croatia_set3/static_gt/static_cam0.txt','../experiments/croatia_set3/static_gt/static_cam1.txt']
     gt_dynamic_file = ['../experiments/croatia_set3/det_gt/cam0_3_det_gt.txt','../experiments/croatia_set3/det_gt/cam1_3_det_gt.txt']
     
-    # gt_static_file = ['/scratch2/wuti/Repos/3D-Object-Trajectory-Reconstruction-Webcam/multiviewunsynch/webcam-datasets/nyc-datasets/static_gt/static_cam1_undistort_div.txt', '/scratch2/wuti/Repos/3D-Object-Trajectory-Reconstruction-Webcam/multiviewunsynch/webcam-datasets/nyc-datasets/static_gt/static_cam2_undistort_div.txt']
-    # gt_dynamic_file = ['/scratch2/wuti/Repos/3D-Object-Trajectory-Reconstruction-Webcam/multiviewunsynch/webcam-datasets/nyc-datasets/20210413_1000_3min/det_opencv/set12/obj1/cam0_w1574280981_12_CSRT_obj0_undistort_div.txt', '/scratch2/wuti/Repos/3D-Object-Trajectory-Reconstruction-Webcam/multiviewunsynch/webcam-datasets/nyc-datasets/20210413_1000_3min/det_opencv/set12/obj1/cam2_w1587769795_12_CSRT_obj0_undistort_div.txt']
-
+    # gt_static_file = ['../experiments/croatia_set3/static_gt_un/static_cam0_un.txt','../experiments/croatia_set3/static_gt_un/static_cam1_un.txt']
+    # gt_dynamic_file = ['../experiments/croatia_set3/det_gt/cam0_3_det_gt_undistort_div.txt','../experiments/croatia_set3/det_gt/cam1_3_det_gt_undistort_div.txt']
+    
+    # gt_static_file = ['../experiments/nyc_set12/static_gt/static_cam0.txt','../experiments/nyc_set12/static_gt/static_cam2.txt']
+    # gt_dynamic_file = ['../experiments/nyc_set12/det_gt/cam0_12.txt','../experiments/nyc_set12/det_gt/cam2_12.txt']
+    
     gt_static = []
     for gfs in gt_static_file:
         gt_static.append(np.loadtxt(gfs, delimiter=' '))
@@ -204,25 +226,30 @@ def main():
 
     # COLMAP GUESS 0
     # data_file_dynamic = '/scratch2/wuti/Repos/mvus/experiments/set12_obj0/calibrated_dynamic/nyc_colmap_dynamic_ori_inlier_calibrated_30.pkl'
-    data_file_static = '/scratch2/wuti/Repos/mvus/experiments/set12_obj0/colmap3_static/nyc_colmap_static_superglue_colmap2_30.pkl'
-    data_file_static_dynamic = '/scratch2/wuti/Repos/mvus/experiments/set12_obj0/colmap3_static_dynamic/nyc_colmap_static_dynamic_superglue_colmap2_30.pkl'
+    # data_file_static = '/scratch2/wuti/Repos/mvus/experiments/set12_obj0/colmap3_static/nyc_colmap_static_superglue_colmap2_30.pkl'
+    # data_file_static_dynamic = '/scratch2/wuti/Repos/mvus/experiments/set12_obj0/colmap3_static_dynamic/nyc_colmap_static_dynamic_superglue_colmap2_30.pkl'
 
     # CALIBRATED
-    # data_file_dynamic = '/scratch2/wuti/Repos/mvus/experiments/set12_obj0/calibrated_dynamic/nyc_colmap_dynamic_ori_inlier_calibrated_30.pkl'
-    # data_file_static = '/scratch2/wuti/Repos/mvus/experiments/set12_obj0/calibrated_static/nyc_colmap_static_superglue_calibrated_30.pkl'
-    # data_file_static_dynamic = '/scratch2/wuti/Repos/mvus/experiments/set12_obj0/calibrated_static_dynamic/nyc_colmap_static_dynamic_superglue_calibrated_30.pkl'
+    # data_file_dynamic = '../experiments/croatia_set3/dynamic/croatia_dynamic_superglue_30.pkl'
+    data_file_static = '../experiments/croatia_set3/static/croatia_static_superglue_30.pkl'
+    data_file_static_dynamic = '../experiments/croatia_set3/static_dynamic/croatia_static_dynamic_superglue_30.pkl'
 
     # CALIBRATED F10
-    data_file_static = '/scratch2/wuti/Repos/mvus/experiments/set12_obj0/calibrated_static_10/nyc_colmap_static_superglue_calibrated_10.pkl'
-    data_file_static_dynamic = '/scratch2/wuti/Repos/mvus/experiments/set12_obj0/calibrated_static_dynamic_10/nyc_colmap_static_dynamic_superglue_calibrated_10.pkl'
+    # data_file_static = '/scratch2/wuti/Repos/mvus/experiments/set12_obj0/calibrated_static_10/nyc_colmap_static_superglue_calibrated_10.pkl'
+    # data_file_static_dynamic = '/scratch2/wuti/Repos/mvus/experiments/set12_obj0/calibrated_static_dynamic_10/nyc_colmap_static_dynamic_superglue_calibrated_10.pkl'
 
 
     # UNDISTORT
     # data_file_dynamic = '/scratch2/wuti/Repos/mvus/experiments/set12_obj0/undistort_dynamic/nyc_colmap_dynamic_ori_inlier_undistort_30.pkl'
-    # data_file_static = '/scratch2/wuti/Repos/mvus/experiments/set12_obj0/undistort_static/nyc_colmap_static_superglue_undistort_30.pkl'
-    # data_file_static_dynamic = '/scratch2/wuti/Repos/mvus/experiments/set12_obj0/undistort_static_dynamic/nyc_colmap_static_dynamic_superglue_undistort_30.pkl'
-    # data_file_static_then_dynamic = '/scratch2/wuti/Repos/mvus/experiments/static_then_dynamic/nyc_colmap_static_then_dynamic_superglue_30.pkl'
-    
+    # data_file_dynamic = '../experiments/croatia_set3/dynamic/croatia_dynamic_superglue_30.pkl'
+    # data_file_static = '../experiments/croatia_set3/static_un/croatia_static_un_superglue_30.pkl'
+    # data_file_static_dynamic = '../experiments/croatia_set3/static_dynamic_un/croatia_static_dynamic_un_superglue_30.pkl'
+
+# data_file_dynamic = '../experiments/croatia_set3/dynamic/croatia_dynamic_superglue_30.pkl'
+    # data_file_static = '../experiments/nyc_set12/static/nyc_static_superglue_30.pkl'
+    # data_file_static_dynamic = '../experiments/nyc_set12/static_dynamic/nyc_static_dynamic_superglue_30.pkl'
+
+
     # with open(data_file_dynamic, 'rb') as file:
     #     flight_dynamic = pickle.load(file)
 
@@ -248,30 +275,90 @@ def main():
     
     _, 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")
-    gt_dynamic2 = [gt_dynamic[x].copy() for x in flight_static_dynamic.sequence]
-    # no optimization for synchronization, use fps to convert timestamps
-    for i, cam in enumerate(flight_static.cameras):
-        gt_dynamic2[i][0] *= flight_static.cameras[flight_static.ref_cam].fps/cam.fps
-    gt_err1, gt_label1, traj_err1, traj_label1 = reproject_ground_truth(flight_static.cameras, gt_static, gt_dynamic2,ref_cam=flight_static_dynamic.ref_cam, output_dir=output_dir, prefix='static_only_', flight=flight_static, ax=[axs1[0],axs2[0]])
-
+    # gt_dynamic2 = [gt_dynamic[x].copy() for x in flight_static_dynamic.sequence]
+    # gt_dynamic2 = [flight_static_dynamic.detections[x].copy() for x in flight_static_dynamic.sequence]
+    # # no optimization for synchronization, use fps and manually aligned time offset to convert timestamps
+    # offsets = [562,25]
+    # # offsets = [0,0]
+    # for i, cam in enumerate(flight_static.cameras):
+    #     gt_dynamic2[i][0] -= offsets[i]
+    #     gt_dynamic2[i][0] *= flight_static.cameras[flight_static.ref_cam].fps/cam.fps
+    flight_static.detections = flight_static_dynamic.detections
+    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,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 = [gt_dynamic[x].copy() for x in flight_static_dynamic.sequence]
+    # gt_dynamic3 = [gt_dynamic[x].copy() for x in flight_static_dynamic.sequence]
+    # gt_dynamic3 = convert_timestamps(gt_dynamic3, flight_static_dynamic.alpha, flight_static_dynamic.beta)
+    gt_dynamic3 = [flight_static_dynamic.detections[x].copy() for x in flight_static_dynamic.sequence]
     gt_dynamic3 = convert_timestamps(gt_dynamic3, flight_static_dynamic.alpha, flight_static_dynamic.beta)
-    gt_err2, gt_label2, traj_err2, traj_label2 = reproject_ground_truth(flight_static_dynamic.cameras, gt_static, gt_dynamic3,ref_cam=flight_static_dynamic.ref_cam, output_dir=output_dir, prefix='static_dynamic_', flight=flight_static_dynamic, ax=[axs1[1],axs2[1]])
-
+    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,ref_cam=flight_static_dynamic.ref_cam, output_dir=output_dir, prefix='static_dynamic_', flight=flight_static_dynamic, ax=[axs1[1],axs2[1],axs3[1]])
+    plt.show()
     print('\n#################################################################\n')
     print("Reconstructions from static-only setting")
-    gt_err3, gt_label3, traj_err3, traj_label3 = reproject_ground_truth(flight_static.cameras, gt_static, gt_dynamic3,ref_cam=flight_static_dynamic.ref_cam, output_dir=output_dir, prefix='static_only_sync_', flight=flight_static, ax=[axs1[2],axs2[2]])
+    # gt_err3, gt_label3, traj_err3, traj_label3, match_err3, match_label3 = reproject_ground_truth(flight_static.cameras, gt_static, gt_dynamic3,ref_cam=flight_static_dynamic.ref_cam, output_dir=output_dir, prefix='static_only_sync_', flight=flight_static, ax=[axs1[2],axs2[2],axs3[1]])
+
 
-    vis.error_boxplot(gt_err1+gt_err2+gt_err3, gt_label1+gt_label2+gt_label3, title='reprojection_error_static.png')
-    vis.error_boxplot(traj_err1+traj_err2+traj_err3, traj_label1+traj_label2+traj_label3, title='reprojection_error_dynamic.png')
+    # plot reprojection errors
+    # reorder error terms
+    gt_repro_errs = gt_err1+gt_err2
+    gt_repro_labels = gt_label1+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_err2
+    match_repro_labels = match_label1+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_err2
+    traj_repro_labels = traj_label1+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')
+    vis.error_boxplot(traj_repro_errs, traj_repro_labels, title='reprojection_error_dynamic.png')
+    vis.error_boxplot(match_repro_errs, match_repro_labels, title='reprojection_error_static.png')
+    plt.show()
 
+    # plot error histograms
+    fig_gt_hist, gt_hist_axs = plt.subplots(2, 1, sharex=True, sharey=True)
+    xlim = np.max(np.hstack(gt_repro_errs))
+    for i, (cam_err1, cam_label1, cam_err2, cam_label2) in enumerate(zip(gt_err1, gt_label1, gt_err2, gt_label2)):
+        vis.error_histogram(cam_err1, cam_err2, labels=[cam_label1, cam_label2],ax=gt_hist_axs[i],title='cam '+str(i),xlim=xlim)
+    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)
+    xlim = np.max(np.hstack(traj_repro_errs))
+    for i, (cam_err1, cam_label1, cam_err2, cam_label2) in enumerate(zip(traj_err1, traj_label1, traj_err2, traj_label2)):
+        vis.error_histogram(cam_err1, cam_err2, labels=[cam_label1, cam_label2],ax=traj_hist_axs[i],title='cam '+str(i),xlim=xlim)
+    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)
+    xlim = np.max(np.hstack(match_repro_errs))
+    for i, (cam_err1, cam_label1, cam_err2, cam_label2) in enumerate(zip(match_err1, match_label1, match_err2, match_label2)):
+        vis.error_histogram(cam_err1, cam_err2, labels=[cam_label1, cam_label2],ax=match_hist_axs[i],title='cam '+str(i),xlim=xlim)
+    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('\n#################################################################\n')
     # print("Reconstructions from static-then-dynamic setting")
     # gt_dynamic4 = [gt_dynamic[x].copy() for x in flight_static_then_dynamic.sequence]
diff --git a/multiviewunsynch/reconstruction/common.py b/multiviewunsynch/reconstruction/common.py
index d624e1d..f38c3ff 100644
--- a/multiviewunsynch/reconstruction/common.py
+++ b/multiviewunsynch/reconstruction/common.py
@@ -583,8 +583,10 @@ def init_static(self, error=10, inlier_only=False, debug=False):
             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 = 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]))
 
diff --git a/multiviewunsynch/thirdparty/camera_calibration_show_extrinsics.py b/multiviewunsynch/thirdparty/camera_calibration_show_extrinsics.py
index e9f1078..ab102b3 100755
--- a/multiviewunsynch/thirdparty/camera_calibration_show_extrinsics.py
+++ b/multiviewunsynch/thirdparty/camera_calibration_show_extrinsics.py
@@ -83,10 +83,15 @@ def create_camera_model(camera_matrix, width, height, scale_focal, draw_frame_ax
     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_triangle, X_center1, X_center2, X_center3, X_center4, X_frame1, X_frame2, X_frame3]
+        return [X_img_plane, 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]
+        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
diff --git a/multiviewunsynch/tools/visualization.py b/multiviewunsynch/tools/visualization.py
index c723edf..43624ee 100644
--- a/multiviewunsynch/tools/visualization.py
+++ b/multiviewunsynch/tools/visualization.py
@@ -218,16 +218,25 @@ def draw_camera_extrinsics(flight, ax, scale_focal=40):
         # 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)
+        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
 
-        for X_cam_part in X_cam_model:
+        # 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)
-            ax.plot3D(X[0,:], X[1,:], X[2,:], color=colors[i])
+            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()
 
@@ -284,7 +293,7 @@ def show_3D_all(*X, title=None,color=True,line=True,flight=None, output_dir='',l
     ax.set_zlabel('Z',fontsize=20)
 
     ax.view_init(elev=30,azim=-50)
-    lgnd = ax.legend(loc=1, prop={'size': 15})
+    lgnd = ax.legend(loc=8, prop={'size': 15})
     for handle in lgnd.legendHandles:
         handle.set_sizes([100])
     # plt.axis('off')
@@ -292,7 +301,7 @@ def show_3D_all(*X, title=None,color=True,line=True,flight=None, output_dir='',l
         plt.savefig(output_dir+title+'reconstructed_scene')
     else:
         plt.savefig(output_dir+'reconstructed_scene.png')
-    # plt.show()
+    plt.show()
 
 
 def show_spline(*spline, title=None):
@@ -362,18 +371,37 @@ 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):
+def error_boxplot(err, labels=[], title=None, ax=None, show_outliers=False):
     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(sharey=True)
     
-    ax.boxplot(err, labels=labels)
+    ax.boxplot(err, labels=labels, showfliers=show_outliers)
     
     if title is not None:
         ax.set_title(title)
     
     return ax
+
+def error_histogram(*errs, num_cams=2, labels=[], title=None, ax=None, xlim=40):
+    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)
+    
+    bins = np.arange(0,xlim,1)
+
+    for err, label in zip(errs,labels):
+        print(label)
+        ax.hist(err, bins=bins, label=label, alpha=0.5)
+    
+    if title is not None:
+        ax.set_title(title)
+    
+    ax.legend()
+    
+    return ax
     
 
 

From 6a9c1687ed8085a94c259554ca368d593741a00b Mon Sep 17 00:00:00 2001
From: Tianyu Wu <t228wu@uwaterloo.ca>
Date: Thu, 24 Jun 2021 16:42:05 +0200
Subject: [PATCH 17/25] fix a small bug for reconstruction with muliple views

---
 .../analysis/analysis_reconstruction.py       | 167 ++++++++++++++----
 multiviewunsynch/main.py                      |   2 +-
 multiviewunsynch/main_static.py               |   4 +-
 multiviewunsynch/main_static_dynamic.py       |   6 +-
 multiviewunsynch/reconstruction/common.py     |  92 +++++-----
 multiviewunsynch/tools/visualization.py       |  66 +++++--
 6 files changed, 237 insertions(+), 100 deletions(-)

diff --git a/multiviewunsynch/analysis/analysis_reconstruction.py b/multiviewunsynch/analysis/analysis_reconstruction.py
index af654b0..a8ae3a4 100644
--- a/multiviewunsynch/analysis/analysis_reconstruction.py
+++ b/multiviewunsynch/analysis/analysis_reconstruction.py
@@ -61,14 +61,14 @@ def reproject_ground_truth(cameras, gt_pts, gt_dets, ref_cam=0, output_dir='', p
         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))
+        match_labels.append('{}cam{}'.format(prefix,i))
         print("mean reprojection error of static features in camera %d is %f" %(i, 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))
+        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)))
 
         # reproject trajectories -- detection matches
@@ -80,16 +80,16 @@ def reproject_ground_truth(cameras, gt_pts, gt_dets, ref_cam=0, output_dir='', p
         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))
+        traj_labels.append('{}cam{}'.format(prefix,i))
         print("mean reprojection error of the dynamic features in camera %d is %f" %(i, np.mean(err2)))
 
         # 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, title=prefix+'cam'+str(i)+' ground truth reprojection', color=True, line=False, bg=cam.img, output_dir=output_dir)
+        vis.show_2D_all(gt_pt.T, gt_repro, cam.dist_point2d(traj_pts2d[2*i:2*i+2]), traj_repro, 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'])
 
     # 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])
+    # 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])
 
     return gt_err, gt_labels, traj_err, traj_labels, match_err, match_labels
 
@@ -184,26 +184,52 @@ def convert_timestamps(gt_dets, alphas, betas):
 
     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 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():
     # Output dir
-    output_dir = '../experiments/croatia_set3/eval_res_calibrated_30/'
+    # output_dir = '../experiments/croatia_set3/eval_res_calibrated_no_sync_30/'
     # output_dir = '../experiments/croatia_set3/eval_res_undistorted_30/'
 
     # output_dir = '../experiments/nyc_set12/eval_res_calibrated_30/'
+    # output_dir = '../experiments/nyc_set17/eval_res_calibrated_10/'
+    output_dir = '../experiments/nyc_set17/eval_res_calibrated_3cams_10/'
+
+    # output_dir = '../experiments/nyc_set17/eval_res_calibrated_10_obj0/'
+    # output_dir = '../experiments/nyc_set19/eval_res_calibrated_10/'
+
+    # output_dir = '../experiments/nyc_set17/eval_res_calibrated_4CA_10/'
+    # output_dir = '../experiments/nyc_set17/eval_res_calibrated_4CA_10_obj0/'
+    # output_dir = '../experiments/nyc_set19/eval_res_calibrated_4CA_10/'
 
     if not os.path.exists(output_dir):
         os.makedirs(output_dir, exist_ok=True)
 
     # Load ground truth
-    gt_static_file = ['../experiments/croatia_set3/static_gt/static_cam0.txt','../experiments/croatia_set3/static_gt/static_cam1.txt']
-    gt_dynamic_file = ['../experiments/croatia_set3/det_gt/cam0_3_det_gt.txt','../experiments/croatia_set3/det_gt/cam1_3_det_gt.txt']
+    # gt_static_file = ['../experiments/croatia_set3/static_gt/static_cam0.txt','../experiments/croatia_set3/static_gt/static_cam1.txt']
+    # gt_dynamic_file = ['../experiments/croatia_set3/det_gt/cam0_3_det_gt.txt','../experiments/croatia_set3/det_gt/cam1_3_det_gt.txt']
     
     # gt_static_file = ['../experiments/croatia_set3/static_gt_un/static_cam0_un.txt','../experiments/croatia_set3/static_gt_un/static_cam1_un.txt']
     # gt_dynamic_file = ['../experiments/croatia_set3/det_gt/cam0_3_det_gt_undistort_div.txt','../experiments/croatia_set3/det_gt/cam1_3_det_gt_undistort_div.txt']
     
     # gt_static_file = ['../experiments/nyc_set12/static_gt/static_cam0.txt','../experiments/nyc_set12/static_gt/static_cam2.txt']
-    # gt_dynamic_file = ['../experiments/nyc_set12/det_gt/cam0_12.txt','../experiments/nyc_set12/det_gt/cam2_12.txt']
-    
+    gt_static_file = ['../experiments/nyc_set12/static_gt_3cams/static_cam0.txt','../experiments/nyc_set12/static_gt_3cams/static_cam1.txt','../experiments/nyc_set12/static_gt_3cams/static_cam2.txt']
+
+    gt_dynamic_file = ['../experiments/nyc_set12/det_gt/cam0_12.txt','../experiments/nyc_set12/det_gt/cam2_12.txt']
+    # gt_dynamic_file = ['../experiments/nyc_set17/det_opencv/cam0_set17_obj0.txt','../experiments/nyc_set17/det_opencv/cam2_set17_obj0.txt']
+    # gt_dynamic_file = ['../experiments/nyc_set19/det_opencv/cam0_set19.txt','../experiments/nyc_set19/det_opencv/cam2_set19.txt']
+
     gt_static = []
     for gfs in gt_static_file:
         gt_static.append(np.loadtxt(gfs, delimiter=' '))
@@ -211,6 +237,9 @@ def main():
     gt_dynamic = []
     for gfd in gt_dynamic_file:
         gt_dynamic.append(np.loadtxt(gfd, usecols=(2,0,1), delimiter=' ').T)
+    
+    # gt_dynamic[0][0] += 10910
+    # gt_dynamic[1][0] += 9001
 
     # Load scenes
     # COLMAP GUESS
@@ -231,8 +260,22 @@ def main():
 
     # CALIBRATED
     # data_file_dynamic = '../experiments/croatia_set3/dynamic/croatia_dynamic_superglue_30.pkl'
-    data_file_static = '../experiments/croatia_set3/static/croatia_static_superglue_30.pkl'
-    data_file_static_dynamic = '../experiments/croatia_set3/static_dynamic/croatia_static_dynamic_superglue_30.pkl'
+    # data_file_static = '../experiments/croatia_set3/static/croatia_static_superglue_30.pkl'
+    # data_file_static_dynamic = '../experiments/croatia_set3/static_dynamic/croatia_static_dynamic_superglue_30.pkl'
+    # data_file_static_dynamic_no_sync = '../experiments/croatia_set3/static_dynamic_no_sync/croatia_static_dynamic_no_sync_superglue_30.pkl'
+
+    # data_file_static = '../experiments/nyc_set17/static/nyc_static_superglue_10.pkl'
+    # data_file_static_dynamic = '../experiments/nyc_set17/static_dynamic/nyc_static_dynamic_superglue_10.pkl'
+    # data_file_static_dynamic_no_sync = '../experiments/nyc_set17/static_dynamic_no_sync/nyc_static_dynamic_no_sync_superglue_10.pkl'
+    
+    # data_file_static = '../experiments/nyc_set17/static_4CA/nyc_static_superglue_10.pkl'
+    # data_file_static_dynamic = '../experiments/nyc_set17/static_dynamic_4CA/nyc_static_dynamic_superglue_10.pkl'
+    # data_file_static_dynamic_no_sync = '../experiments/nyc_set17/static_dynamic_no_sync_4CA/nyc_static_dynamic_no_sync_superglue_10.pkl'
+    
+    data_file_static = '../experiments/nyc_set17/static_dynamic_3cams/nyc_static_dynamic_superglue_10.pkl'
+    data_file_static_dynamic = '../experiments/nyc_set17/static_dynamic_4CA/nyc_static_dynamic_superglue_10.pkl'
+    data_file_static_dynamic_no_sync = '../experiments/nyc_set17/static_dynamic_no_sync_4CA/nyc_static_dynamic_no_sync_superglue_10.pkl'
+    
 
     # CALIBRATED F10
     # data_file_static = '/scratch2/wuti/Repos/mvus/experiments/set12_obj0/calibrated_static_10/nyc_colmap_static_superglue_calibrated_10.pkl'
@@ -259,6 +302,17 @@ def main():
     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))
+
     # with open(data_file_static_then_dynamic, 'rb') as file:
     #     flight_static_then_dynamic = pickle.load(file)
 
@@ -287,7 +341,9 @@ def main():
     # for i, cam in enumerate(flight_static.cameras):
     #     gt_dynamic2[i][0] -= offsets[i]
     #     gt_dynamic2[i][0] *= flight_static.cameras[flight_static.ref_cam].fps/cam.fps
-    flight_static.detections = flight_static_dynamic.detections
+    
+    # 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()
@@ -301,59 +357,94 @@ def main():
     print("Reconstructions from static-dynamic setting")
     # gt_dynamic3 = [gt_dynamic[x].copy() for x in flight_static_dynamic.sequence]
     # gt_dynamic3 = convert_timestamps(gt_dynamic3, flight_static_dynamic.alpha, flight_static_dynamic.beta)
+    
     gt_dynamic3 = [flight_static_dynamic.detections[x].copy() for x in flight_static_dynamic.sequence]
-    gt_dynamic3 = convert_timestamps(gt_dynamic3, flight_static_dynamic.alpha, flight_static_dynamic.beta)
+    # gt_dynamic3 = convert_timestamps(gt_dynamic3, flight_static_dynamic.alpha, flight_static_dynamic.beta)
+    
+    # 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,ref_cam=flight_static_dynamic.ref_cam, output_dir=output_dir, prefix='static_dynamic_', flight=flight_static_dynamic, ax=[axs1[1],axs2[1],axs3[1]])
+    gt_err2, gt_label2, traj_err2, traj_label2, match_err2, match_label2 = reproject_ground_truth(flight_static_dynamic.cameras, gt_static, gt_dynamic3,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-only setting")
-    # gt_err3, gt_label3, traj_err3, traj_label3, match_err3, match_label3 = reproject_ground_truth(flight_static.cameras, gt_static, gt_dynamic3,ref_cam=flight_static_dynamic.ref_cam, output_dir=output_dir, prefix='static_only_sync_', flight=flight_static, ax=[axs1[2],axs2[2],axs3[1]])
+    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_err2, gt_label2, traj_err2, traj_label2, match_err2, match_label2 = reproject_ground_truth(flight_static_dynamic.cameras, gt_static, gt_dynamic3,ref_cam=flight_static_dynamic.ref_cam, output_dir=output_dir, prefix='static_dynamic_', flight=flight_static_dynamic, ax=[axs1[2],axs2[2],axs3[2]])
+    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,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]])
 
 
     # plot reprojection errors
     # reorder error terms
-    gt_repro_errs = gt_err1+gt_err2
-    gt_repro_labels = gt_label1+gt_label2
+    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_err2
-    match_repro_labels = match_label1+match_label2
+    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_err2
-    traj_repro_labels = traj_label1+traj_label2
+    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')
-    vis.error_boxplot(traj_repro_errs, traj_repro_labels, title='reprojection_error_dynamic.png')
-    vis.error_boxplot(match_repro_errs, match_repro_labels, title='reprojection_error_static.png')
-    plt.show()
+    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)
 
     # plot error histograms
-    fig_gt_hist, gt_hist_axs = plt.subplots(2, 1, sharex=True, sharey=True)
+    width = 0.2
+    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) in enumerate(zip(gt_err1, gt_label1, gt_err2, gt_label2)):
-        vis.error_histogram(cam_err1, cam_err2, labels=[cam_label1, cam_label2],ax=gt_hist_axs[i],title='cam '+str(i),xlim=xlim)
+    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, bin_width=width)
+    # loc2 = loc[1:-1]+0.5
+    # l2 = ["{:.0f}".format(x) for x in loc[1:-1]]
+    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)
+    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) in enumerate(zip(traj_err1, traj_label1, traj_err2, traj_label2)):
-        vis.error_histogram(cam_err1, cam_err2, labels=[cam_label1, cam_label2],ax=traj_hist_axs[i],title='cam '+str(i),xlim=xlim)
+    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, 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)
+    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) in enumerate(zip(match_err1, match_label1, match_err2, match_label2)):
-        vis.error_histogram(cam_err1, cam_err2, labels=[cam_label1, cam_label2],ax=match_hist_axs[i],title='cam '+str(i),xlim=xlim)
+    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, 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()
diff --git a/multiviewunsynch/main.py b/multiviewunsynch/main.py
index 237a0c3..66975b1 100644
--- a/multiviewunsynch/main.py
+++ b/multiviewunsynch/main.py
@@ -94,7 +94,7 @@
         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)
+        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 raw detection
         # _ =  align_detections(flight, visualize=True)
diff --git a/multiviewunsynch/main_static.py b/multiviewunsynch/main_static.py
index a167feb..2db9321 100644
--- a/multiviewunsynch/main_static.py
+++ b/multiviewunsynch/main_static.py
@@ -86,7 +86,7 @@
         # 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)
+        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)
@@ -98,7 +98,7 @@
             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)
+        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:
diff --git a/multiviewunsynch/main_static_dynamic.py b/multiviewunsynch/main_static_dynamic.py
index c1ada5c..c7f145b 100644
--- a/multiviewunsynch/main_static_dynamic.py
+++ b/multiviewunsynch/main_static_dynamic.py
@@ -122,7 +122,7 @@
         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)
+        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 raw detection
         # _ =  align_detections(flight, visualize=True)
@@ -148,7 +148,7 @@
         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)
+        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)
@@ -161,7 +161,7 @@
             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)
+        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:
diff --git a/multiviewunsynch/reconstruction/common.py b/multiviewunsynch/reconstruction/common.py
index f38c3ff..ff50c39 100644
--- a/multiviewunsynch/reconstruction/common.py
+++ b/multiviewunsynch/reconstruction/common.py
@@ -13,7 +13,7 @@
 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
@@ -309,20 +309,21 @@ def init_traj(self,error=10,inlier_only=False, debug=False):
             self.inlier_mask = np.ones(self.static.shape[1])
 
             # also draw the inlier matches
-            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])
+            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]))
-            
+                    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
@@ -576,19 +577,20 @@ def init_static(self, error=10, inlier_only=False, debug=False):
         inlier_ids_masked = inlier_ids[mask]
 
         # also draw the inlier matches
-        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])
+        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]))
+                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]
@@ -746,7 +748,7 @@ def error_cam_static(self, cam_id, mode='dist', norm=False, debug=False):
         # get the 3D static points reconstructed from this camera
         point_3D = np.empty([3, 0])
         point_3D = np.hstack([point_3D, self.static[:, self.cameras[cam_id].index_2d_3d]])
-        X = util.homogeneous(point_3D)
+        X = util.homogeneous(point_3D)            
         
         # get the corresponding 2d static points
         if debug:
@@ -756,11 +758,14 @@ def error_cam_static(self, cam_id, mode='dist', norm=False, debug=False):
             # use the extracted static features
             x = self.cameras[cam_id].get_points()
         
-        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)
+        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'])
@@ -864,7 +869,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, debug=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):
         '''
         Bundle Adjustment with multiple splines
 
@@ -928,7 +933,8 @@ def error_BA(x):
 
         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)
@@ -1058,10 +1064,10 @@ def jac_BA(near=3,motion_offset=10):
                     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))
+                    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 + i * num_camParam, num_3d_points * 3 + (i+1) * num_camParam
+                    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
@@ -1076,6 +1082,8 @@ def jac_BA(near=3,motion_offset=10):
                     # 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
@@ -1150,10 +1158,10 @@ def jac_BA(near=3,motion_offset=10):
         print('Doing BA with {} cameras...\n'.format(numCam))
         fn = lambda x: error_BA(x)
         # 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:
-            res = least_squares(fn,model,jac_sparsity=A,tr_solver='lsmr',xtol=1e-12,max_nfev=max_iter,verbose=1,bounds=bounds_rs)
+        # 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:
+        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.
@@ -1410,7 +1418,7 @@ def register_new_camera_static(self, cam_id, cams, debug=False):
                 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_points()
+            pts_2d = self.cameras[cam_id].get_gt_pts()
 
         else:
             # if have not yet initialize cam_id in feature_dict, do so
@@ -1533,7 +1541,7 @@ def get_camera_pose(self, cam_id, cams, error=8, verbose=0, debug=False):
             pts_2d, pts_3d = self.register_new_camera_static(cam_id, cams, debug)
 
             # stack the static points with the detections
-            detect = np.hstack([detect, pts_2d])
+            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
diff --git a/multiviewunsynch/tools/visualization.py b/multiviewunsynch/tools/visualization.py
index 43624ee..9bde4ad 100644
--- a/multiviewunsynch/tools/visualization.py
+++ b/multiviewunsynch/tools/visualization.py
@@ -181,7 +181,7 @@ def show_2D_all(*x, title=None,color=True,line=True,text=False, bg=None, output_
     for i in range(num):
         # plt.subplot(1,num,i+1)
 
-        c = ['r','b','r','g']
+        c = ['r','b','orange','g']
         m = ['o','x','o','+']
         if len(label) == 0:
             label = ['Raw points', 'Reconstruction points','Raw detections', 'Reconstructed trajectories']
@@ -202,12 +202,14 @@ def show_2D_all(*x, title=None,color=True,line=True,text=False, bg=None, output_
 
         plt.xlabel('X')
         plt.ylabel('Y')
+    
+    plt.legend(loc=1)
+
     if title:
         plt.suptitle(title)
         plt.savefig(output_dir+title+'.png')
     else:
         plt.savefig(output_dir+'reprojected.png')
-    plt.legend(loc=1, prop={'size': 10})
     # plt.show()
 
 
@@ -249,9 +251,13 @@ def show_3D_all(*X, title=None,color=True,line=True,flight=None, output_dir='',l
     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','b','r','g']
+            c = ['r','b','orange','g']
             m = ['o','x','o','+']
             if len(label) == 0:
                 label = ['RTK ground truth', 'Reconstruction Spline']
@@ -371,37 +377,69 @@ 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):
+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(sharey=True)
+        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(output_dir+title)
+    else:
+        plt.savefig(output_dir+'error_boxplot.png')
     return ax
 
-def error_histogram(*errs, num_cams=2, labels=[], title=None, ax=None, xlim=40):
+def error_histogram(*errs, num_cams=2, labels=[], title=None, ax=None, xlim=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)
+        fig, ax = plt.subplots(sharex=True, sharey=True, dpi=300)
     
-    bins = np.arange(0,xlim,1)
-
-    for err, label in zip(errs,labels):
-        print(label)
-        ax.hist(err, bins=bins, label=label, alpha=0.5)
+    # 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, 1)
+    
+    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
+    return ax, bins[:-1]
     
 
 

From 32eada7f9adc44cbb836c1b18dd960a398b82e21 Mon Sep 17 00:00:00 2001
From: Tianyu Wu <t228wu@uwaterloo.ca>
Date: Sun, 4 Jul 2021 22:09:34 +0200
Subject: [PATCH 18/25] cleanup

---
 multiviewunsynch/main.py                     | 26 +++++---------------
 multiviewunsynch/main_static_dynamic.py      | 23 -----------------
 multiviewunsynch/main_static_then_dynamic.py | 17 -------------
 multiviewunsynch/reconstruction/common.py    |  6 +++--
 4 files changed, 10 insertions(+), 62 deletions(-)

diff --git a/multiviewunsynch/main.py b/multiviewunsynch/main.py
index 66975b1..561ea75 100644
--- a/multiviewunsynch/main.py
+++ b/multiviewunsynch/main.py
@@ -86,26 +86,12 @@
 flight.spline_to_traj(sampling_rate=1)
 # Visualize the 3D trajectory
 vis.show_trajectory_3D(flight.traj[1:],line=False)
-# save the 2d trajectories
-if 'save_2d' in flight.settings.keys() and flight.settings['save_2d']:
-    if not os.path.exists(os.path.dirname(flight.settings['save_2d_path'])):
-        os.makedirs(os.path.dirname(flight.settings['save_2d_path']))
-    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 raw detection
-        # _ =  align_detections(flight, visualize=True)
-
-        # save the reprojected trajectories
-        traj_res = np.vstack([x_res, flight.traj[0]]).T
-        # save the raw detection (replace the timestamp to the global timestamp)
-        det_ori_global = np.vstack([x_ori, flight.detections_global[i][0]]).T
-        np.savetxt(flight.settings['save_2d_path'].replace('.txt', '_cam'+str(i)+'.txt'), traj_res, delimiter=' ')
-        np.savetxt(flight.settings['save_2d_path'].replace('.txt', '_det_ori_global_cam'+str(i)+'.txt'), det_ori_global, delimiter=' ')
-
+# 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
 if len(flight.gt) > 0:
diff --git a/multiviewunsynch/main_static_dynamic.py b/multiviewunsynch/main_static_dynamic.py
index c7f145b..ebc1ab4 100644
--- a/multiviewunsynch/main_static_dynamic.py
+++ b/multiviewunsynch/main_static_dynamic.py
@@ -45,9 +45,6 @@
 # Convert raw detections into the global timeline
 flight.detection_to_global()
 
-# # Initialize with the static part
-# flight.init_static(inlier_only=True, debug=args.debug)
-
 # Initialize the first 3D trajectory
 flight.init_traj(error=flight.settings['thres_Fmatix'], inlier_only=True, debug=args.debug)
 
@@ -113,26 +110,6 @@
 
 # Discretize trajectory
 flight.spline_to_traj(sampling_rate=1)
-# save the 2d trajectories
-if 'save_2d' in flight.settings.keys() and flight.settings['save_2d']:
-    if not os.path.exists(os.path.dirname(flight.settings['save_2d_path'])):
-        os.makedirs(os.path.dirname(flight.settings['save_2d_path']))
-
-    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 raw detection
-        # _ =  align_detections(flight, visualize=True)
-
-        # save the reprojected trajectories
-        traj_res = np.vstack([x_res, flight.traj[0]]).T
-        # save the raw detection (replace the timestamp to the global timestamp)
-        det_ori_global = np.vstack([x_ori, flight.detections_global[i][0]]).T
-        np.savetxt(flight.settings['save_2d_path'].replace('.txt', '_cam'+str(i)+'.txt'), traj_res, delimiter=' ')
-        np.savetxt(flight.settings['save_2d_path'].replace('.txt', '_det_ori_global_cam'+str(i)+'.txt'), det_ori_global, delimiter=' ')
 
 # Visualize the 3D trajectory
 vis.show_trajectory_3D(flight.traj[1:],line=False)
diff --git a/multiviewunsynch/main_static_then_dynamic.py b/multiviewunsynch/main_static_then_dynamic.py
index b224172..786b155 100644
--- a/multiviewunsynch/main_static_then_dynamic.py
+++ b/multiviewunsynch/main_static_then_dynamic.py
@@ -161,23 +161,6 @@
 
 # Discretize trajectory
 flight.spline_to_traj(sampling_rate=1)
-# save the 2d trajectories
-if 'save_2d' in flight.settings.keys() and flight.settings['save_2d']:
-    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)
-
-        # # align with the raw detection
-        # _ =  align_detections(flight, visualize=True)
-
-        # save the reprojected trajectories
-        traj_res = np.vstack([x_res, flight.traj[0]]).T
-        # save the raw detection (replace the timestamp to the global timestamp)
-        det_ori_global = np.vstack([x_ori, flight.detections_global[i][0]]).T
-        np.savetxt(flight.settings['save_2d_path'].replace('.txt', '_cam'+str(i)+'.txt'), traj_res, delimiter=' ')
-        np.savetxt(flight.settings['save_2d_path'].replace('.txt', '_det_ori_global_cam'+str(i)+'.txt'), det_ori_global, delimiter=' ')
 
 # Visualize the 3D trajectory
 vis.show_trajectory_3D(flight.traj[1:],line=False)
diff --git a/multiviewunsynch/reconstruction/common.py b/multiviewunsynch/reconstruction/common.py
index ff50c39..4b4f603 100644
--- a/multiviewunsynch/reconstruction/common.py
+++ b/multiviewunsynch/reconstruction/common.py
@@ -2346,10 +2346,12 @@ def create_scene(path_input):
             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'], img_path=cam['img_path'])
+        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':
@@ -2357,7 +2359,7 @@ def create_scene(path_input):
                 camera.kp = util.convert_kpts(kpt_list[i])
             else:
                 raise Exception("Unsupported feature extraction and matching method")
-        else:
+        elif 'img_path' in cam.keys():
             camera.read_img()
 
         # load the ground truth static matches if given

From 0afdc7364dae989e337dc033160cfc15b11d8649 Mon Sep 17 00:00:00 2001
From: Tianyu Wu <t228wu@uwaterloo.ca>
Date: Sun, 4 Jul 2021 22:16:49 +0200
Subject: [PATCH 19/25] add visualization of camera center in 2D visualization

---
 multiviewunsynch/tools/visualization.py | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/multiviewunsynch/tools/visualization.py b/multiviewunsynch/tools/visualization.py
index 9bde4ad..4aae566 100644
--- a/multiviewunsynch/tools/visualization.py
+++ b/multiviewunsynch/tools/visualization.py
@@ -168,7 +168,7 @@ def show_trajectory_3D(*X, title=None,color=True,line=False):
     plt.show()
 
 
-def show_2D_all(*x, title=None,color=True,line=True,text=False, bg=None, output_dir='', label=[]):
+def show_2D_all(*x, title=None,color=True,line=True,text=False, bg=None, output_dir='', label=[], cam_center=[]):
     plt.figure(figsize=(12, 10))
     if bg is not None:
         bg = cv2.cvtColor(bg, cv2.COLOR_BGR2RGB)
@@ -195,6 +195,9 @@ def show_2D_all(*x, title=None,color=True,line=True,text=False, bg=None, output_
         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)
+        if len(cam_center) == 2:
+            # plot camera position
+            plt.scatter(cam_center[0], cam_center[1], c='c',marker='*')
 
         # plt.gca().set_xlim([0,1920])
         # plt.gca().set_ylim([0,1080])

From 1756cba03689dfdaa67422c39fb2a9b95cf08bac Mon Sep 17 00:00:00 2001
From: Tianyu Wu <t228wu@uwaterloo.ca>
Date: Mon, 5 Jul 2021 13:40:17 +0200
Subject: [PATCH 20/25] debug for backward support

---
 multiviewunsynch/main_static_dynamic.py      |  2 +-
 multiviewunsynch/main_static_then_dynamic.py |  2 +-
 multiviewunsynch/reconstruction/common.py    | 23 ++++++++++----------
 3 files changed, 14 insertions(+), 13 deletions(-)

diff --git a/multiviewunsynch/main_static_dynamic.py b/multiviewunsynch/main_static_dynamic.py
index ebc1ab4..8441a32 100644
--- a/multiviewunsynch/main_static_dynamic.py
+++ b/multiviewunsynch/main_static_dynamic.py
@@ -97,7 +97,7 @@
     flight.select_most_overlap()
     
     # Add the next camera and get its pose
-    flight.get_camera_pose(flight.sequence[cam_temp], flight.sequence[:cam_temp], debug=args.debug)
+    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'])
diff --git a/multiviewunsynch/main_static_then_dynamic.py b/multiviewunsynch/main_static_then_dynamic.py
index 786b155..8b37515 100644
--- a/multiviewunsynch/main_static_then_dynamic.py
+++ b/multiviewunsynch/main_static_then_dynamic.py
@@ -148,7 +148,7 @@
         break
 
     # Add the next camera and get its pose
-    flight.get_camera_pose(flight.sequence[cam_temp], flight.sequence[:cam_temp], debug=args.debug)
+    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'])
diff --git a/multiviewunsynch/reconstruction/common.py b/multiviewunsynch/reconstruction/common.py
index 4b4f603..154427f 100644
--- a/multiviewunsynch/reconstruction/common.py
+++ b/multiviewunsynch/reconstruction/common.py
@@ -144,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:], self.settings['undist_method']) 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:
@@ -216,15 +217,15 @@ def init_traj(self,error=10,inlier_only=False, debug=False):
         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'])
+        # # 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)
+        #     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']:
@@ -1515,7 +1516,7 @@ def get_camera_pose_static(self, cam_id, cams, error=8, verbose=0, debug=False):
         if verbose:
             print('{} out of {} points are inliers for PnP'.format(inliers.shape[0], N))
 
-    def get_camera_pose(self, cam_id, cams, error=8, verbose=0, debug=False):
+    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.
 
@@ -1538,7 +1539,7 @@ def get_camera_pose(self, cam_id, cams, error=8, verbose=0, debug=False):
         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, cams, debug)
+            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])])

From de5c7e915eb1430ed04fe198893d9323ff11c9b6 Mon Sep 17 00:00:00 2001
From: Tianyu Wu <t228wu@uwaterloo.ca>
Date: Mon, 5 Jul 2021 15:27:50 +0200
Subject: [PATCH 21/25] debug camera visualization in 2D

---
 .../analysis/analysis_reconstruction.py       | 125 +++++-------------
 multiviewunsynch/tools/visualization.py       |   9 +-
 2 files changed, 36 insertions(+), 98 deletions(-)

diff --git a/multiviewunsynch/analysis/analysis_reconstruction.py b/multiviewunsynch/analysis/analysis_reconstruction.py
index a8ae3a4..64efebd 100644
--- a/multiviewunsynch/analysis/analysis_reconstruction.py
+++ b/multiviewunsynch/analysis/analysis_reconstruction.py
@@ -6,7 +6,7 @@
 from reconstruction import synchronization as sync
 
 from reconstruction import epipolar as ep
-from tools.util import match_overlap
+from tools.util import match_overlap, homogeneous
 from itertools import combinations
 from matplotlib import pyplot as plt
 import os
@@ -83,8 +83,21 @@ def reproject_ground_truth(cameras, gt_pts, gt_dets, ref_cam=0, output_dir='', p
         traj_labels.append('{}cam{}'.format(prefix,i))
         print("mean reprojection error of the dynamic features in camera %d is %f" %(i, 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, 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'])
+        vis.show_2D_all(gt_pt.T, gt_repro, cam.dist_point2d(traj_pts2d[2*i:2*i+2]), traj_repro, 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)
 
     # plot the reprojection error boxplot
     # vis.error_boxplot(gt_err, gt_labels, title=prefix+'reprojection_error_static_ground_truth.png', ax=ax[0])
@@ -93,82 +106,6 @@ def reproject_ground_truth(cameras, gt_pts, gt_dets, ref_cam=0, output_dir='', p
 
     return gt_err, gt_labels, traj_err, traj_labels, match_err, match_labels
 
-def reproject_ground_truth_2views(cameras, gt_pts, gt_dets, ref_cam=0, n_bins=10, output_dir='', prefix='', flight=None, ax=None):
-    combos = combinations(range(len(cameras)),2)
-
-    repro_errors = []
-    # for every pair of cameras, triangulate 3d points and reproject
-    for t1, t2 in combos:
-        print("triangulate camera pairs: (%d, %d)" %(t1, t2))
-        # undistort gt_points
-        gt_un1 = cameras[t1].undist_point(gt_pts[t1].T)
-        gt_un2 = cameras[t2].undist_point(gt_pts[t2].T)
-
-        # triangulate
-        X_gt = ep.triangulate_matlab(gt_un1, gt_un2, cameras[t1].P, cameras[t2].P)
-
-        # backproject the triangulated points on the images
-        gt_repro1 = cameras[t1].dist_point3d(X_gt[:-1].T)
-        gt_repro2 = cameras[t2].dist_point3d(X_gt[:-1].T)
-        # compute the error
-        repro_err1 = ep.reprojection_error(gt_un1, cameras[t1].projectPoint(X_gt))
-        repro_err2 = ep.reprojection_error(gt_un2, cameras[t2].projectPoint(X_gt))
-        repro_errors.append([repro_err1, repro_err2])
-        print("mean reprojection error of camera pair (%d, %d) is %f and %f" %(t1, t2, np.mean(repro_err1), np.mean(repro_err2)))
-
-        # histogram of reprojection error
-        fig, axs = plt.subplots(2, 1, sharex=True, tight_layout=True)
-        axs[0].hist(repro_err1, bins=n_bins)
-        axs[0].set_title('cam'+str(t1))
-        axs[1].hist(repro_err2, bins=n_bins)
-        axs[1].set_title('cam'+str(t2))
-        plt.savefig(output_dir+prefix+'repro_cam{}_{}.png'.format(t1, t2))
-
-        plt.show()
-
-        # evaluate for the dynamic part
-        # match between detections
-        if cameras[t1].fps > cameras[t2].fps:
-            det1, det2 = match_overlap(gt_dets[t1], gt_dets[t2])
-        else:
-            det2, det1 = match_overlap(gt_dets[t2], gt_dets[t1])
-
-        vis.draw_detection_matches(cameras[t1].img, det1, cameras[t2].img, det2, title=prefix+'matched_detections.png', output_dir=output_dir)
-        
-        # undistort points
-        det_un1 = cameras[t1].undist_point(det1[1:])
-        det_un2 = cameras[t2].undist_point(det2[1:])
-        # triangulate the detections
-        Traj_gt = ep.triangulate_matlab(det_un1, det_un2, cameras[t1].P, cameras[t2].P)
-        # backproject triangulated detections on the images
-        traj_repro1 = cameras[t1].dist_point3d(Traj_gt[:-1].T)
-        traj_repro2 = cameras[t2].dist_point3d(Traj_gt[:-1].T)
-        vis.draw_detection_matches(cameras[t1].img, np.vstack([det1[0],traj_repro1]), cameras[t2].img, np.vstack([det2[0],traj_repro2]), title=prefix+'reprojected_detections.png', output_dir=output_dir)
-        
-        # compute the reprojection error
-        traj_err1 = ep.reprojection_error(det_un1, cameras[t1].projectPoint(Traj_gt))
-        traj_err2 = ep.reprojection_error(det_un2, cameras[t2].projectPoint(Traj_gt))
-        repro_errors[-1] += [traj_err1, traj_err2]
-        print("mean reprojection error of the trajectories of camera pair (%d, %d) is %f and %f" %(t1, t2, np.mean(traj_err1), np.mean(traj_err2)))
-        # plot the histogram the reprojecton error of the dynamic part
-        fig, axs = plt.subplots(2, 1, sharex=True, tight_layout=True)
-        axs[0].hist(traj_err1, bins=n_bins)
-        axs[0].set_title('cam'+str(t1))
-        axs[1].hist(traj_err2, bins=n_bins)
-        axs[1].set_title('cam'+str(t2))
-        plt.savefig(output_dir+prefix+'repro_traj_cam{}_{}.png'.format(t1, t2))
-
-        plt.show()
-        
-        # plot images
-        vis.show_2D_all(gt_pts[t1].T, gt_repro1, det1[1:], traj_repro1, title=prefix+'cam'+str(t1)+' ground truth reprojection', color=True, line=False, bg=cameras[t1].img, output_dir=output_dir)
-        vis.show_2D_all(gt_pts[t2].T, gt_repro2, det2[1:], traj_repro2, title=prefix+'cam'+str(t2)+' ground truth reprojection', color=True, line=False, bg=cameras[t2].img, output_dir=output_dir)
-
-        # plot 3D reconstructe scene
-        vis.show_3D_all(X_gt, np.empty([3,0]), Traj_gt, np.empty([3,0]), color=False, line=False, flight=flight, output_dir=output_dir+prefix)
-
-    
-
 def convert_timestamps(gt_dets, alphas, betas):
     '''
     Function:
@@ -199,12 +136,12 @@ def undistort_image(cam, output_dir = '', title=None):
 
 def main():
     # Output dir
-    # output_dir = '../experiments/croatia_set3/eval_res_calibrated_no_sync_30/'
+    output_dir = '../experiments/croatia_set3/eval_res_calibrated_30_new/'
     # output_dir = '../experiments/croatia_set3/eval_res_undistorted_30/'
 
     # output_dir = '../experiments/nyc_set12/eval_res_calibrated_30/'
     # output_dir = '../experiments/nyc_set17/eval_res_calibrated_10/'
-    output_dir = '../experiments/nyc_set17/eval_res_calibrated_3cams_10/'
+    # output_dir = '../experiments/nyc_set17/eval_res_calibrated_3cams_10/'
 
     # output_dir = '../experiments/nyc_set17/eval_res_calibrated_10_obj0/'
     # output_dir = '../experiments/nyc_set19/eval_res_calibrated_10/'
@@ -217,16 +154,16 @@ def main():
         os.makedirs(output_dir, exist_ok=True)
 
     # Load ground truth
-    # gt_static_file = ['../experiments/croatia_set3/static_gt/static_cam0.txt','../experiments/croatia_set3/static_gt/static_cam1.txt']
-    # gt_dynamic_file = ['../experiments/croatia_set3/det_gt/cam0_3_det_gt.txt','../experiments/croatia_set3/det_gt/cam1_3_det_gt.txt']
+    gt_static_file = ['../experiments/croatia_set3/static_gt/static_cam0.txt','../experiments/croatia_set3/static_gt/static_cam1.txt']
+    gt_dynamic_file = ['../experiments/croatia_set3/det_gt/cam0_3_det_gt.txt','../experiments/croatia_set3/det_gt/cam1_3_det_gt.txt']
     
     # gt_static_file = ['../experiments/croatia_set3/static_gt_un/static_cam0_un.txt','../experiments/croatia_set3/static_gt_un/static_cam1_un.txt']
     # gt_dynamic_file = ['../experiments/croatia_set3/det_gt/cam0_3_det_gt_undistort_div.txt','../experiments/croatia_set3/det_gt/cam1_3_det_gt_undistort_div.txt']
     
     # gt_static_file = ['../experiments/nyc_set12/static_gt/static_cam0.txt','../experiments/nyc_set12/static_gt/static_cam2.txt']
-    gt_static_file = ['../experiments/nyc_set12/static_gt_3cams/static_cam0.txt','../experiments/nyc_set12/static_gt_3cams/static_cam1.txt','../experiments/nyc_set12/static_gt_3cams/static_cam2.txt']
+    # gt_static_file = ['../experiments/nyc_set12/static_gt_3cams/static_cam0.txt','../experiments/nyc_set12/static_gt_3cams/static_cam1.txt','../experiments/nyc_set12/static_gt_3cams/static_cam2.txt']
 
-    gt_dynamic_file = ['../experiments/nyc_set12/det_gt/cam0_12.txt','../experiments/nyc_set12/det_gt/cam2_12.txt']
+    # gt_dynamic_file = ['../experiments/nyc_set12/det_gt/cam0_12.txt','../experiments/nyc_set12/det_gt/cam2_12.txt']
     # gt_dynamic_file = ['../experiments/nyc_set17/det_opencv/cam0_set17_obj0.txt','../experiments/nyc_set17/det_opencv/cam2_set17_obj0.txt']
     # gt_dynamic_file = ['../experiments/nyc_set19/det_opencv/cam0_set19.txt','../experiments/nyc_set19/det_opencv/cam2_set19.txt']
 
@@ -260,9 +197,9 @@ def main():
 
     # CALIBRATED
     # data_file_dynamic = '../experiments/croatia_set3/dynamic/croatia_dynamic_superglue_30.pkl'
-    # data_file_static = '../experiments/croatia_set3/static/croatia_static_superglue_30.pkl'
-    # data_file_static_dynamic = '../experiments/croatia_set3/static_dynamic/croatia_static_dynamic_superglue_30.pkl'
-    # data_file_static_dynamic_no_sync = '../experiments/croatia_set3/static_dynamic_no_sync/croatia_static_dynamic_no_sync_superglue_30.pkl'
+    data_file_static = '../experiments/croatia_set3/static/croatia_static_superglue_30.pkl'
+    data_file_static_dynamic = '../experiments/croatia_set3/static_dynamic/croatia_static_dynamic_superglue_30.pkl'
+    data_file_static_dynamic_no_sync = '../experiments/croatia_set3/static_dynamic_no_sync/croatia_static_dynamic_no_sync_superglue_30.pkl'
 
     # data_file_static = '../experiments/nyc_set17/static/nyc_static_superglue_10.pkl'
     # data_file_static_dynamic = '../experiments/nyc_set17/static_dynamic/nyc_static_dynamic_superglue_10.pkl'
@@ -272,9 +209,9 @@ def main():
     # data_file_static_dynamic = '../experiments/nyc_set17/static_dynamic_4CA/nyc_static_dynamic_superglue_10.pkl'
     # data_file_static_dynamic_no_sync = '../experiments/nyc_set17/static_dynamic_no_sync_4CA/nyc_static_dynamic_no_sync_superglue_10.pkl'
     
-    data_file_static = '../experiments/nyc_set17/static_dynamic_3cams/nyc_static_dynamic_superglue_10.pkl'
-    data_file_static_dynamic = '../experiments/nyc_set17/static_dynamic_4CA/nyc_static_dynamic_superglue_10.pkl'
-    data_file_static_dynamic_no_sync = '../experiments/nyc_set17/static_dynamic_no_sync_4CA/nyc_static_dynamic_no_sync_superglue_10.pkl'
+    # data_file_static = '../experiments/nyc_set17/static_dynamic_3cams/nyc_static_dynamic_superglue_10.pkl'
+    # data_file_static_dynamic = '../experiments/nyc_set17/static_dynamic_4CA/nyc_static_dynamic_superglue_10.pkl'
+    # data_file_static_dynamic_no_sync = '../experiments/nyc_set17/static_dynamic_no_sync_4CA/nyc_static_dynamic_no_sync_superglue_10.pkl'
     
 
     # CALIBRATED F10
@@ -342,7 +279,7 @@ def main():
     #     gt_dynamic2[i][0] -= offsets[i]
     #     gt_dynamic2[i][0] *= flight_static.cameras[flight_static.ref_cam].fps/cam.fps
     
-    # flight_static.detections = flight_static_dynamic_no_sync.detections
+    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'])
@@ -361,8 +298,8 @@ def main():
     gt_dynamic3 = [flight_static_dynamic.detections[x].copy() for x in flight_static_dynamic.sequence]
     # gt_dynamic3 = convert_timestamps(gt_dynamic3, flight_static_dynamic.alpha, flight_static_dynamic.beta)
     
-    # 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.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,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]])
@@ -371,7 +308,7 @@ def main():
     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.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_err2, gt_label2, traj_err2, traj_label2, match_err2, match_label2 = reproject_ground_truth(flight_static_dynamic.cameras, gt_static, gt_dynamic3,ref_cam=flight_static_dynamic.ref_cam, output_dir=output_dir, prefix='static_dynamic_', flight=flight_static_dynamic, ax=[axs1[2],axs2[2],axs3[2]])
diff --git a/multiviewunsynch/tools/visualization.py b/multiviewunsynch/tools/visualization.py
index 4aae566..fda8b2d 100644
--- a/multiviewunsynch/tools/visualization.py
+++ b/multiviewunsynch/tools/visualization.py
@@ -168,7 +168,7 @@ def show_trajectory_3D(*X, title=None,color=True,line=False):
     plt.show()
 
 
-def show_2D_all(*x, title=None,color=True,line=True,text=False, bg=None, output_dir='', label=[], cam_center=[]):
+def show_2D_all(*x, title=None,color=True,line=True,text=False, bg=None, output_dir='', label=[], cam_center=None):
     plt.figure(figsize=(12, 10))
     if bg is not None:
         bg = cv2.cvtColor(bg, cv2.COLOR_BGR2RGB)
@@ -195,9 +195,6 @@ def show_2D_all(*x, title=None,color=True,line=True,text=False, bg=None, output_
         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)
-        if len(cam_center) == 2:
-            # plot camera position
-            plt.scatter(cam_center[0], cam_center[1], c='c',marker='*')
 
         # plt.gca().set_xlim([0,1920])
         # plt.gca().set_ylim([0,1080])
@@ -206,6 +203,10 @@ def show_2D_all(*x, title=None,color=True,line=True,text=False, bg=None, output_
         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)
 
     if title:

From 1e31b8abda75ed9d509ddbaa926d1fde8dc8f290 Mon Sep 17 00:00:00 2001
From: Tianyu Wu <t228wu@uwaterloo.ca>
Date: Mon, 26 Jul 2021 10:12:50 +0200
Subject: [PATCH 22/25] add normalization

---
 .../analysis/analysis_reconstruction.py       | 47 ++++++-------------
 multiviewunsynch/main_static.py               |  6 +--
 multiviewunsynch/main_static_dynamic.py       |  5 +-
 multiviewunsynch/reconstruction/common.py     | 29 ++++++++----
 multiviewunsynch/tools/visualization.py       |  4 +-
 5 files changed, 43 insertions(+), 48 deletions(-)

diff --git a/multiviewunsynch/analysis/analysis_reconstruction.py b/multiviewunsynch/analysis/analysis_reconstruction.py
index 64efebd..b5f969a 100644
--- a/multiviewunsynch/analysis/analysis_reconstruction.py
+++ b/multiviewunsynch/analysis/analysis_reconstruction.py
@@ -137,6 +137,7 @@ def undistort_image(cam, output_dir = '', title=None):
 def main():
     # Output dir
     output_dir = '../experiments/croatia_set3/eval_res_calibrated_30_new/'
+    output_dir = '../experiments/croatia_set3/eval_res_calibrated_30_scale/'
     # output_dir = '../experiments/croatia_set3/eval_res_undistorted_30/'
 
     # output_dir = '../experiments/nyc_set12/eval_res_calibrated_30/'
@@ -174,33 +175,19 @@ def main():
     gt_dynamic = []
     for gfd in gt_dynamic_file:
         gt_dynamic.append(np.loadtxt(gfd, usecols=(2,0,1), delimiter=' ').T)
-    
-    # gt_dynamic[0][0] += 10910
-    # gt_dynamic[1][0] += 9001
 
     # Load scenes
-    # COLMAP GUESS
-    # data_file_dynamic = '/scratch2/wuti/Repos/mvus/experiments/dynamic/nyc_colmap_dynamic_ori_inlier_30.pkl'
-    # data_file_static = '/scratch2/wuti/Repos/mvus/experiments/static/nyc_colmap_static_superglue_colmap2_30.pkl'
-    # data_file_static_dynamic = '/scratch2/wuti/Repos/mvus/experiments/static_dynamic/nyc_colmap_static_dynamic_superglue_colmap2_30.pkl'
-    # data_file_static_then_dynamic = '/scratch2/wuti/Repos/mvus/experiments/static_then_dynamic/nyc_colmap_static_then_dynamic_superglue_30.pkl'
-    
-    # COLMAP GUESS 0
-    # data_file_dynamic = '/scratch2/wuti/Repos/mvus/experiments/set12_obj0/calibrated_dynamic/nyc_colmap_dynamic_ori_inlier_calibrated_30.pkl'
-    # data_file_static = '/scratch2/wuti/Repos/mvus/experiments/set12_obj0/colmap2_static/nyc_colmap_static_superglue_colmap2_30.pkl'
-    # data_file_static_dynamic = '/scratch2/wuti/Repos/mvus/experiments/set12_obj0/colmap2_static_dynamic/nyc_colmap_static_dynamic_superglue_colmap2_30.pkl'
-
-    # COLMAP GUESS 0
-    # data_file_dynamic = '/scratch2/wuti/Repos/mvus/experiments/set12_obj0/calibrated_dynamic/nyc_colmap_dynamic_ori_inlier_calibrated_30.pkl'
-    # data_file_static = '/scratch2/wuti/Repos/mvus/experiments/set12_obj0/colmap3_static/nyc_colmap_static_superglue_colmap2_30.pkl'
-    # data_file_static_dynamic = '/scratch2/wuti/Repos/mvus/experiments/set12_obj0/colmap3_static_dynamic/nyc_colmap_static_dynamic_superglue_colmap2_30.pkl'
 
     # CALIBRATED
     # data_file_dynamic = '../experiments/croatia_set3/dynamic/croatia_dynamic_superglue_30.pkl'
-    data_file_static = '../experiments/croatia_set3/static/croatia_static_superglue_30.pkl'
-    data_file_static_dynamic = '../experiments/croatia_set3/static_dynamic/croatia_static_dynamic_superglue_30.pkl'
-    data_file_static_dynamic_no_sync = '../experiments/croatia_set3/static_dynamic_no_sync/croatia_static_dynamic_no_sync_superglue_30.pkl'
-
+    # data_file_static = '../experiments/croatia_set3/static/croatia_static_superglue_30.pkl'
+    # data_file_static_dynamic = '../experiments/croatia_set3/static_dynamic/croatia_static_dynamic_superglue_30.pkl'
+    # data_file_static_dynamic_no_sync = '../experiments/croatia_set3/static_dynamic_no_sync/croatia_static_dynamic_no_sync_superglue_30.pkl'
+    
+    data_file_static = '../experiments/croatia_set3/static/croatia_static_superglue_scale_30.pkl'
+    data_file_static_dynamic = '../experiments/croatia_set3/static_dynamic/croatia_static_dynamic_superglue_scale_30.pkl'
+    data_file_static_dynamic_no_sync = '../experiments/croatia_set3/static_dynamic_no_sync/croatia_static_dynamic_no_sync_superglue_scale_30.pkl'
+    
     # data_file_static = '../experiments/nyc_set17/static/nyc_static_superglue_10.pkl'
     # data_file_static_dynamic = '../experiments/nyc_set17/static_dynamic/nyc_static_dynamic_superglue_10.pkl'
     # data_file_static_dynamic_no_sync = '../experiments/nyc_set17/static_dynamic_no_sync/nyc_static_dynamic_no_sync_superglue_10.pkl'
@@ -213,19 +200,13 @@ def main():
     # data_file_static_dynamic = '../experiments/nyc_set17/static_dynamic_4CA/nyc_static_dynamic_superglue_10.pkl'
     # data_file_static_dynamic_no_sync = '../experiments/nyc_set17/static_dynamic_no_sync_4CA/nyc_static_dynamic_no_sync_superglue_10.pkl'
     
-
-    # CALIBRATED F10
-    # data_file_static = '/scratch2/wuti/Repos/mvus/experiments/set12_obj0/calibrated_static_10/nyc_colmap_static_superglue_calibrated_10.pkl'
-    # data_file_static_dynamic = '/scratch2/wuti/Repos/mvus/experiments/set12_obj0/calibrated_static_dynamic_10/nyc_colmap_static_dynamic_superglue_calibrated_10.pkl'
-
-
     # UNDISTORT
     # data_file_dynamic = '/scratch2/wuti/Repos/mvus/experiments/set12_obj0/undistort_dynamic/nyc_colmap_dynamic_ori_inlier_undistort_30.pkl'
     # data_file_dynamic = '../experiments/croatia_set3/dynamic/croatia_dynamic_superglue_30.pkl'
     # data_file_static = '../experiments/croatia_set3/static_un/croatia_static_un_superglue_30.pkl'
     # data_file_static_dynamic = '../experiments/croatia_set3/static_dynamic_un/croatia_static_dynamic_un_superglue_30.pkl'
 
-# data_file_dynamic = '../experiments/croatia_set3/dynamic/croatia_dynamic_superglue_30.pkl'
+    # data_file_dynamic = '../experiments/croatia_set3/dynamic/croatia_dynamic_superglue_30.pkl'
     # data_file_static = '../experiments/nyc_set12/static/nyc_static_superglue_30.pkl'
     # data_file_static_dynamic = '../experiments/nyc_set12/static_dynamic/nyc_static_dynamic_superglue_30.pkl'
 
@@ -346,11 +327,11 @@ def main():
     vis.error_boxplot(traj_repro_errs2, traj_repro_labels2, title='reprojection_error_dynamic2.png', output_dir=output_dir)
 
     # plot error histograms
-    width = 0.2
+    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, bin_width=width)
+        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)
     # loc2 = loc[1:-1]+0.5
     # l2 = ["{:.0f}".format(x) for x in loc[1:-1]]
     xticks = np.arange(0,90,10)
@@ -367,7 +348,7 @@ def main():
     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, bin_width=width)
+        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)
@@ -378,7 +359,7 @@ def main():
     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, bin_width=width)
+        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)
diff --git a/multiviewunsynch/main_static.py b/multiviewunsynch/main_static.py
index 2db9321..ae2a6f9 100644
--- a/multiviewunsynch/main_static.py
+++ b/multiviewunsynch/main_static.py
@@ -20,7 +20,7 @@
 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')
@@ -45,7 +45,7 @@
 
     print('\nDoing the first BA')
     # Bundle adjustment
-    res = flight.BA_static(cam_temp, debug=args.debug)
+    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]]))
 
@@ -55,7 +55,7 @@
 
     print('\nDoing the second BA')
     # Bundle adjustment after outlier removal
-    res = flight.BA_static(cam_temp, debug=args.debug)
+    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]]))
 
diff --git a/multiviewunsynch/main_static_dynamic.py b/multiviewunsynch/main_static_dynamic.py
index 8441a32..2ff418f 100644
--- a/multiviewunsynch/main_static_dynamic.py
+++ b/multiviewunsynch/main_static_dynamic.py
@@ -22,6 +22,7 @@
 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()
 
@@ -67,7 +68,7 @@
     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)
+        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]]))
@@ -83,7 +84,7 @@
     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)
+        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]]))
diff --git a/multiviewunsynch/reconstruction/common.py b/multiviewunsynch/reconstruction/common.py
index 154427f..40e7d0f 100644
--- a/multiviewunsynch/reconstruction/common.py
+++ b/multiviewunsynch/reconstruction/common.py
@@ -748,7 +748,8 @@ def error_cam_static(self, cam_id, mode='dist', norm=False, debug=False):
         '''
         # get the 3D static points reconstructed from this camera
         point_3D = np.empty([3, 0])
-        point_3D = np.hstack([point_3D, self.static[:, self.cameras[cam_id].index_2d_3d]])
+        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
@@ -870,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, debug=False, sync=True):
+    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
 
@@ -1162,7 +1163,11 @@ def jac_BA(near=3,motion_offset=10):
         # 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:
-        res = least_squares(fn,model,jac_sparsity=A,tr_solver='lsmr',xtol=1e-12,max_nfev=max_iter,verbose=1,bounds=bounds_rs)
+        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.
@@ -1198,7 +1203,7 @@ def jac_BA(near=3,motion_offset=10):
 
         return res
     
-    def BA_static(self, numCam, max_iter=10, debug=False):
+    def BA_static(self, numCam, max_iter=10, debug=False, scaling=True):
         '''
         Function:
             standard BA with static points and camera models
@@ -1295,7 +1300,11 @@ def jac_BA():
 
         # least-sqaure optimization for BA
         # res = least_squares(fn, model, tr_solver='lsmr', xtol=1e-12, max_nfev=max_iter, verbose=1)
-        res = least_squares(fn, model, jac_sparsity=A, 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
@@ -1309,6 +1318,7 @@ def jac_BA():
         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):
         '''
@@ -1482,7 +1492,10 @@ def register_new_camera_static(self, cam_id, cams, debug=False):
             pts_2d = self.cameras[cam_id].get_points()
         
         # get the registered 3d static point from the new camera
-        pts_3d = self.static[:, self.cameras[cam_id].index_2d_3d]
+        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
     
@@ -2121,9 +2134,9 @@ def __init__(self,**kwargs):
         self.kp = []
         self.des = []
         # the indices of the features used for 3D static point reconstruction
-        self.index_registered_2d = np.empty(0)
+        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)
+        self.index_2d_3d = np.empty(0, dtype=int)
         # ground truth static matches for debugging
         self.gt_pts = None
 
diff --git a/multiviewunsynch/tools/visualization.py b/multiviewunsynch/tools/visualization.py
index fda8b2d..8815452 100644
--- a/multiviewunsynch/tools/visualization.py
+++ b/multiviewunsynch/tools/visualization.py
@@ -396,7 +396,7 @@ def error_boxplot(err, labels=[], title=None, ax=None, show_outliers=False, outp
         plt.savefig(output_dir+'error_boxplot.png')
     return ax
 
-def error_histogram(*errs, num_cams=2, labels=[], title=None, ax=None, xlim=40, ylim=350, bin_width=0.1):
+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:
@@ -414,7 +414,7 @@ def error_histogram(*errs, num_cams=2, labels=[], title=None, ax=None, xlim=40,
     #     # if ax.get_ylim()[-1] < np.max(n):
     #     #     ax.set_ylim([0,np.max(n)+0.75])
 
-    bins = np.arange(0, xlim+1, 1)
+    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)

From f724bb1fdad5f8dd22ccad951eac51d09bd06bd5 Mon Sep 17 00:00:00 2001
From: Tianyu Wu <t228wu@uwateroo.ca>
Date: Wed, 28 Jul 2021 11:01:51 +0200
Subject: [PATCH 23/25] debug scaling

---
 .gitignore                                |  2 +
 multiviewunsynch/main_static.py           |  9 +++
 multiviewunsynch/main_static_dynamic.py   | 11 +++
 multiviewunsynch/reconstruction/common.py | 94 +++++++++++++++--------
 4 files changed, 85 insertions(+), 31 deletions(-)

diff --git a/.gitignore b/.gitignore
index c936980..5350931 100644
--- a/.gitignore
+++ b/.gitignore
@@ -126,4 +126,6 @@ dmypy.json
 # Datas
 data/
 experiments/
+multiviewunsynch/drone-tracking-datasets
 multiviewunsynch/*.png
+multiviewunsynch/*-datasets.png
\ No newline at end of file
diff --git a/multiviewunsynch/main_static.py b/multiviewunsynch/main_static.py
index ae2a6f9..77f0176 100644
--- a/multiviewunsynch/main_static.py
+++ b/multiviewunsynch/main_static.py
@@ -40,6 +40,9 @@
 
 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]]))
 
@@ -53,12 +56,18 @@
     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))
diff --git a/multiviewunsynch/main_static_dynamic.py b/multiviewunsynch/main_static_dynamic.py
index 2ff418f..9176151 100644
--- a/multiviewunsynch/main_static_dynamic.py
+++ b/multiviewunsynch/main_static_dynamic.py
@@ -58,6 +58,9 @@
 
 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]]))
@@ -78,6 +81,9 @@
     # 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)
@@ -89,6 +95,9 @@
     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], 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))
@@ -144,6 +153,8 @@
 # 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']:
diff --git a/multiviewunsynch/reconstruction/common.py b/multiviewunsynch/reconstruction/common.py
index 40e7d0f..40d25bd 100644
--- a/multiviewunsynch/reconstruction/common.py
+++ b/multiviewunsynch/reconstruction/common.py
@@ -217,15 +217,15 @@ def init_traj(self,error=10,inlier_only=False, debug=False):
         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'])
+        # 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)
+            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']:
@@ -1341,29 +1341,30 @@ def remove_outliers(self, cams, thres=30, verbose=False, debug=False):
                 
                 # 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']:
-                    # 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]
+                    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))
+                    # 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):
         '''
@@ -1378,7 +1379,7 @@ def remove_outliers_static(self, cams, thres=30, verbose=False, debug=False):
                 # 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
+                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']]
@@ -1396,6 +1397,37 @@ def remove_outliers_static(self, cams, thres=30, verbose=False, debug=False):
                 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:

From 39d3f3c95d22e9d2676f376ccfd2a0ebb3e981b7 Mon Sep 17 00:00:00 2001
From: Tianyu Wu <t228wu@uwateroo.ca>
Date: Wed, 4 Aug 2021 15:38:12 +0200
Subject: [PATCH 24/25] reform evaluation

---
 .../analysis/analysis_reconstruction.py       | 207 +++++++-----------
 multiviewunsynch/eval.py                      |  26 ++-
 multiviewunsynch/main_static_dynamic.py       |   2 +-
 multiviewunsynch/tools/visualization.py       |  89 +++++---
 4 files changed, 156 insertions(+), 168 deletions(-)

diff --git a/multiviewunsynch/analysis/analysis_reconstruction.py b/multiviewunsynch/analysis/analysis_reconstruction.py
index b5f969a..e98a45c 100644
--- a/multiviewunsynch/analysis/analysis_reconstruction.py
+++ b/multiviewunsynch/analysis/analysis_reconstruction.py
@@ -7,12 +7,14 @@
 
 from reconstruction import epipolar as ep
 from tools.util import match_overlap, homogeneous
-from itertools import combinations
 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, ref_cam=0, output_dir='', prefix='', flight=None, ax=None):
+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
@@ -21,6 +23,10 @@ def reproject_ground_truth(cameras, gt_pts, gt_dets, ref_cam=0, output_dir='', p
         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 = []
@@ -48,7 +54,7 @@ def reproject_ground_truth(cameras, gt_pts, gt_dets, ref_cam=0, output_dir='', p
     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+prefix)
+    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 = []
@@ -57,12 +63,21 @@ def reproject_ground_truth(cameras, gt_pts, gt_dets, ref_cam=0, output_dir='', p
     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)
@@ -70,6 +85,8 @@ def reproject_ground_truth(cameras, gt_pts, gt_dets, ref_cam=0, output_dir='', p
         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
@@ -82,6 +99,8 @@ def reproject_ground_truth(cameras, gt_pts, gt_dets, ref_cam=0, output_dir='', p
         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 = []
@@ -97,13 +116,19 @@ def reproject_ground_truth(cameras, gt_pts, gt_dets, ref_cam=0, output_dir='', p
         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, 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)
+        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):
@@ -134,86 +159,14 @@ def undistort_image(cam, output_dir = '', title=None):
         cv2.imwrite(os.path.join(output_dir, 'undistorted.png'), dst)
 
 
-def main():
-    # Output dir
-    output_dir = '../experiments/croatia_set3/eval_res_calibrated_30_new/'
-    output_dir = '../experiments/croatia_set3/eval_res_calibrated_30_scale/'
-    # output_dir = '../experiments/croatia_set3/eval_res_undistorted_30/'
-
-    # output_dir = '../experiments/nyc_set12/eval_res_calibrated_30/'
-    # output_dir = '../experiments/nyc_set17/eval_res_calibrated_10/'
-    # output_dir = '../experiments/nyc_set17/eval_res_calibrated_3cams_10/'
-
-    # output_dir = '../experiments/nyc_set17/eval_res_calibrated_10_obj0/'
-    # output_dir = '../experiments/nyc_set19/eval_res_calibrated_10/'
-
-    # output_dir = '../experiments/nyc_set17/eval_res_calibrated_4CA_10/'
-    # output_dir = '../experiments/nyc_set17/eval_res_calibrated_4CA_10_obj0/'
-    # output_dir = '../experiments/nyc_set19/eval_res_calibrated_4CA_10/'
-
-    if not os.path.exists(output_dir):
-        os.makedirs(output_dir, exist_ok=True)
-
-    # Load ground truth
-    gt_static_file = ['../experiments/croatia_set3/static_gt/static_cam0.txt','../experiments/croatia_set3/static_gt/static_cam1.txt']
-    gt_dynamic_file = ['../experiments/croatia_set3/det_gt/cam0_3_det_gt.txt','../experiments/croatia_set3/det_gt/cam1_3_det_gt.txt']
-    
-    # gt_static_file = ['../experiments/croatia_set3/static_gt_un/static_cam0_un.txt','../experiments/croatia_set3/static_gt_un/static_cam1_un.txt']
-    # gt_dynamic_file = ['../experiments/croatia_set3/det_gt/cam0_3_det_gt_undistort_div.txt','../experiments/croatia_set3/det_gt/cam1_3_det_gt_undistort_div.txt']
-    
-    # gt_static_file = ['../experiments/nyc_set12/static_gt/static_cam0.txt','../experiments/nyc_set12/static_gt/static_cam2.txt']
-    # gt_static_file = ['../experiments/nyc_set12/static_gt_3cams/static_cam0.txt','../experiments/nyc_set12/static_gt_3cams/static_cam1.txt','../experiments/nyc_set12/static_gt_3cams/static_cam2.txt']
-
-    # gt_dynamic_file = ['../experiments/nyc_set12/det_gt/cam0_12.txt','../experiments/nyc_set12/det_gt/cam2_12.txt']
-    # gt_dynamic_file = ['../experiments/nyc_set17/det_opencv/cam0_set17_obj0.txt','../experiments/nyc_set17/det_opencv/cam2_set17_obj0.txt']
-    # gt_dynamic_file = ['../experiments/nyc_set19/det_opencv/cam0_set19.txt','../experiments/nyc_set19/det_opencv/cam2_set19.txt']
+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=' '))
     
-    gt_dynamic = []
-    for gfd in gt_dynamic_file:
-        gt_dynamic.append(np.loadtxt(gfd, usecols=(2,0,1), delimiter=' ').T)
-
-    # Load scenes
-
-    # CALIBRATED
-    # data_file_dynamic = '../experiments/croatia_set3/dynamic/croatia_dynamic_superglue_30.pkl'
-    # data_file_static = '../experiments/croatia_set3/static/croatia_static_superglue_30.pkl'
-    # data_file_static_dynamic = '../experiments/croatia_set3/static_dynamic/croatia_static_dynamic_superglue_30.pkl'
-    # data_file_static_dynamic_no_sync = '../experiments/croatia_set3/static_dynamic_no_sync/croatia_static_dynamic_no_sync_superglue_30.pkl'
-    
-    data_file_static = '../experiments/croatia_set3/static/croatia_static_superglue_scale_30.pkl'
-    data_file_static_dynamic = '../experiments/croatia_set3/static_dynamic/croatia_static_dynamic_superglue_scale_30.pkl'
-    data_file_static_dynamic_no_sync = '../experiments/croatia_set3/static_dynamic_no_sync/croatia_static_dynamic_no_sync_superglue_scale_30.pkl'
-    
-    # data_file_static = '../experiments/nyc_set17/static/nyc_static_superglue_10.pkl'
-    # data_file_static_dynamic = '../experiments/nyc_set17/static_dynamic/nyc_static_dynamic_superglue_10.pkl'
-    # data_file_static_dynamic_no_sync = '../experiments/nyc_set17/static_dynamic_no_sync/nyc_static_dynamic_no_sync_superglue_10.pkl'
-    
-    # data_file_static = '../experiments/nyc_set17/static_4CA/nyc_static_superglue_10.pkl'
-    # data_file_static_dynamic = '../experiments/nyc_set17/static_dynamic_4CA/nyc_static_dynamic_superglue_10.pkl'
-    # data_file_static_dynamic_no_sync = '../experiments/nyc_set17/static_dynamic_no_sync_4CA/nyc_static_dynamic_no_sync_superglue_10.pkl'
-    
-    # data_file_static = '../experiments/nyc_set17/static_dynamic_3cams/nyc_static_dynamic_superglue_10.pkl'
-    # data_file_static_dynamic = '../experiments/nyc_set17/static_dynamic_4CA/nyc_static_dynamic_superglue_10.pkl'
-    # data_file_static_dynamic_no_sync = '../experiments/nyc_set17/static_dynamic_no_sync_4CA/nyc_static_dynamic_no_sync_superglue_10.pkl'
-    
-    # UNDISTORT
-    # data_file_dynamic = '/scratch2/wuti/Repos/mvus/experiments/set12_obj0/undistort_dynamic/nyc_colmap_dynamic_ori_inlier_undistort_30.pkl'
-    # data_file_dynamic = '../experiments/croatia_set3/dynamic/croatia_dynamic_superglue_30.pkl'
-    # data_file_static = '../experiments/croatia_set3/static_un/croatia_static_un_superglue_30.pkl'
-    # data_file_static_dynamic = '../experiments/croatia_set3/static_dynamic_un/croatia_static_dynamic_un_superglue_30.pkl'
-
-    # data_file_dynamic = '../experiments/croatia_set3/dynamic/croatia_dynamic_superglue_30.pkl'
-    # data_file_static = '../experiments/nyc_set12/static/nyc_static_superglue_30.pkl'
-    # data_file_static_dynamic = '../experiments/nyc_set12/static_dynamic/nyc_static_dynamic_superglue_30.pkl'
-
-
-    # with open(data_file_dynamic, 'rb') as file:
-    #     flight_dynamic = pickle.load(file)
-
+    # read result files
     with open(data_file_static, 'rb') as file:
         flight_static = pickle.load(file)
 
@@ -223,43 +176,26 @@ def main():
     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))
-
-    # with open(data_file_static_then_dynamic, 'rb') as file:
-    #     flight_static_then_dynamic = 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("Plot reprojection error")
     
-    # # dynamic only
-    # print("Reconstructions from dynamic-only setting")
-    # # convert dynamic part timestamp
-    # gt_dynamic1 = [gt_dynamic[x].copy() for x in flight_dynamic.sequence]
-    # gt_dynamic1 = convert_timestamps(gt_dynamic1, flight_dynamic.alpha, flight_dynamic.beta)
-    # reproject_ground_truth(flight_dynamic.cameras, gt_static, gt_dynamic1, output_dir=output_dir, prefix='dynamic_only_', flight=flight_dynamic)
-    
     _, 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")
-    # gt_dynamic2 = [gt_dynamic[x].copy() for x in flight_static_dynamic.sequence]
-    # gt_dynamic2 = [flight_static_dynamic.detections[x].copy() for x in flight_static_dynamic.sequence]
-    # # no optimization for synchronization, use fps and manually aligned time offset to convert timestamps
-    # offsets = [562,25]
-    # # offsets = [0,0]
-    # for i, cam in enumerate(flight_static.cameras):
-    #     gt_dynamic2[i][0] -= offsets[i]
-    #     gt_dynamic2[i][0] *= flight_static.cameras[flight_static.ref_cam].fps/cam.fps
-    
+
+    # 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
@@ -269,33 +205,31 @@ def main():
     # 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,ref_cam=flight_static_dynamic.ref_cam, output_dir=output_dir, prefix='static_only_', flight=flight_static, ax=[axs1[0],axs2[0],axs3[0]])
+    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 = [gt_dynamic[x].copy() for x in flight_static_dynamic.sequence]
-    # gt_dynamic3 = convert_timestamps(gt_dynamic3, flight_static_dynamic.alpha, flight_static_dynamic.beta)
-    
-    gt_dynamic3 = [flight_static_dynamic.detections[x].copy() for x in flight_static_dynamic.sequence]
-    # gt_dynamic3 = convert_timestamps(gt_dynamic3, flight_static_dynamic.alpha, flight_static_dynamic.beta)
-    
+    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,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]])
+    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_err2, gt_label2, traj_err2, traj_label2, match_err2, match_label2 = reproject_ground_truth(flight_static_dynamic.cameras, gt_static, gt_dynamic3,ref_cam=flight_static_dynamic.ref_cam, output_dir=output_dir, prefix='static_dynamic_', flight=flight_static_dynamic, ax=[axs1[2],axs2[2],axs3[2]])
-    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,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]])
+    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
@@ -325,15 +259,15 @@ def main():
     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)
-    # loc2 = loc[1:-1]+0.5
-    # l2 = ["{:.0f}".format(x) for x in loc[1:-1]]
     xticks = np.arange(0,90,10)
     l2 = ["{:.0f}".format(x) for x in xticks]
     loc2 = xticks
@@ -343,7 +277,7 @@ def main():
     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()
+    # 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))
@@ -354,7 +288,7 @@ def main():
     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()
+    # 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))
@@ -365,16 +299,29 @@ def main():
     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('\n#################################################################\n')
-    # print("Reconstructions from static-then-dynamic setting")
-    # gt_dynamic4 = [gt_dynamic[x].copy() for x in flight_static_then_dynamic.sequence]
-    # gt_dynamic4 = convert_timestamps(gt_dynamic4, flight_static_then_dynamic.alpha, flight_static_then_dynamic.beta)
-    # reproject_ground_truth(flight_static_then_dynamic.cameras, gt_static, gt_dynamic4, output_dir=output_dir, prefix='static_then_dynamic_', flight=flight_static_then_dynamic)
+    # plt.show()
 
     print('Finish!')
 
 if __name__ == '__main__':
-    main()
\ No newline at end of file
+    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/eval.py b/multiviewunsynch/eval.py
index f71b8fb..a0a7571 100644
--- a/multiviewunsynch/eval.py
+++ b/multiviewunsynch/eval.py
@@ -1,3 +1,27 @@
 from analysis import analysis_reconstruction as recon
+import argparse
+from glob import glob
+import os
 
-recon.main()
\ No newline at end of file
+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_static_dynamic.py b/multiviewunsynch/main_static_dynamic.py
index 9176151..1b3c0c4 100644
--- a/multiviewunsynch/main_static_dynamic.py
+++ b/multiviewunsynch/main_static_dynamic.py
@@ -96,7 +96,7 @@
     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], verbose=True, debug=args.debug)
+    flight.remove_outliers_3d(flight.sequence[:cam_temp], thres=10, mode='camera', verbose=True, debug=args.debug)
 
     num_end = flight.numCam if flight.find_order else len(flight.sequence)
     if cam_temp == num_end:
diff --git a/multiviewunsynch/tools/visualization.py b/multiviewunsynch/tools/visualization.py
index 8815452..e6e91dc 100644
--- a/multiviewunsynch/tools/visualization.py
+++ b/multiviewunsynch/tools/visualization.py
@@ -11,6 +11,7 @@
 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):
     ''' 
@@ -168,52 +169,66 @@ def show_trajectory_3D(*X, title=None,color=True,line=False):
     plt.show()
 
 
-def show_2D_all(*x, title=None,color=True,line=True,text=False, bg=None, output_dir='', label=[], cam_center=None):
-    plt.figure(figsize=(12, 10))
+def show_2D_all(*x, title=None,color=True,line=True,text=False, bg=None, output_dir='', label=[], cam_center=None, vec=0):
+    # plt.figure(figsize=(12, 10))
+    plt.figure()
     if bg is not None:
         bg = cv2.cvtColor(bg, cv2.COLOR_BGR2RGB)
         plt.imshow(bg)
         h,w,_ = bg.shape
         plt.xlim([0,w])
         plt.ylim([h,0])
+    
+    # c = ['r','b','orange','g']
+    c = ['lime', 'r', 'lime', 'r']
+    m = ['o','x','o','+']
 
     num = len(x)
-    for i in range(num):
-        # plt.subplot(1,num,i+1)
-
-        c = ['r','b','orange','g']
-        m = ['o','x','o','+']
-        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)
+    
+    if vec > 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.gca().set_xlim([0,1920])
+            # plt.gca().set_ylim([0,1080])
+            # plt.gca().invert_yaxis()
 
-        plt.xlabel('X')
-        plt.ylabel('Y')
+            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')
+    # 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.legend(loc=1)
+    plt.axis('off')
 
     if title:
-        plt.suptitle(title)
-        plt.savefig(output_dir+title+'.png')
+        # plt.suptitle(title)
+        plt.savefig(os.path.join(output_dir,title+'.png'),bbox_inches='tight',pad_inches = 0)
     else:
-        plt.savefig(output_dir+'reprojected.png')
+        plt.savefig(os.path.join(output_dir,'reprojected.png'),bbox_inches='tight',pad_inches = 0)
     # plt.show()
 
 
@@ -308,9 +323,9 @@ def show_3D_all(*X, title=None,color=True,line=True,flight=None, output_dir='',l
         handle.set_sizes([100])
     # plt.axis('off')
     if title:
-        plt.savefig(output_dir+title+'reconstructed_scene')
+        plt.savefig(os.path.join(output_dir,title+'.png'))
     else:
-        plt.savefig(output_dir+'reconstructed_scene.png')
+        plt.savefig(os.path.join(output_dir,'reconstructed_scene.png'))
     plt.show()
 
 
@@ -391,9 +406,9 @@ def error_boxplot(err, labels=[], title=None, ax=None, show_outliers=False, outp
     
     if title is not None:
         ax.set_title(title)
-        plt.savefig(output_dir+title)
+        plt.savefig(os.path.join(output_dir,title))
     else:
-        plt.savefig(output_dir+'error_boxplot.png')
+        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):
@@ -462,10 +477,12 @@ def draw_detection_matches(img1, d1, img2, d2, title='detection_mathches.png', o
     # 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(output_dir+title, outimg)    
+    cv2.imwrite(os.path.join(output_dir,title), outimg)    
 
 def draw_matches(img1, kp1, img2, kp2, matches, matchesMask):
     '''

From c1387b2ae4085c2b3e7dfd128fd7fa44d95118c5 Mon Sep 17 00:00:00 2001
From: Tianyu Wu <t228wu@uwateroo.ca>
Date: Tue, 23 Nov 2021 13:13:51 +0100
Subject: [PATCH 25/25] replace RANSAC with MAGSAC

---
 .../analysis/analysis_reconstruction.py       | 19 +++--
 multiviewunsynch/main_static_dynamic.py       |  2 +-
 multiviewunsynch/plot_detections.py           | 70 +++++++++++++++++++
 multiviewunsynch/reconstruction/epipolar.py   |  3 +-
 multiviewunsynch/tools/util.py                |  2 +-
 5 files changed, 86 insertions(+), 10 deletions(-)
 create mode 100644 multiviewunsynch/plot_detections.py

diff --git a/multiviewunsynch/analysis/analysis_reconstruction.py b/multiviewunsynch/analysis/analysis_reconstruction.py
index e98a45c..c7880e5 100644
--- a/multiviewunsynch/analysis/analysis_reconstruction.py
+++ b/multiviewunsynch/analysis/analysis_reconstruction.py
@@ -153,6 +153,10 @@ def undistort_image(cam, output_dir = '', title=None):
     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:
@@ -176,16 +180,17 @@ def main(data_file_static, data_file_static_dynamic, data_file_static_dynamic_no
     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))
+    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)
diff --git a/multiviewunsynch/main_static_dynamic.py b/multiviewunsynch/main_static_dynamic.py
index 1b3c0c4..cc45ae6 100644
--- a/multiviewunsynch/main_static_dynamic.py
+++ b/multiviewunsynch/main_static_dynamic.py
@@ -96,7 +96,7 @@
     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=10, mode='camera', verbose=True, debug=args.debug)
+    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:
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/epipolar.py b/multiviewunsynch/reconstruction/epipolar.py
index 5539934..f68613e 100644
--- a/multiviewunsynch/reconstruction/epipolar.py
+++ b/multiviewunsynch/reconstruction/epipolar.py
@@ -90,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)
@@ -99,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
diff --git a/multiviewunsynch/tools/util.py b/multiviewunsynch/tools/util.py
index 90ecd32..e4990cd 100644
--- a/multiviewunsynch/tools/util.py
+++ b/multiviewunsynch/tools/util.py
@@ -56,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.