Free Motion Capture for Everyone πβ¨
TypeScript
that's calibration, baybee! ;D
Jon272fc3a5d044
diff --git a/freemocap/fmc_anipose.py b/freemocap/fmc_anipose.pyindex 47c02088..1b956c05 100644--- a/freemocap/fmc_anipose.py+++ b/freemocap/fmc_anipose.py@@ -17,6 +17,7 @@ from aniposelib.boards import merge_rows, extract_points, \from aniposelib.utils import get_initial_extrinsics, make_M, get_rtvec, \get_connections+@jit(nopython=True, parallel=True)def triangulate_simple(points, camera_mats):num_cams = len(camera_mats)@@ -41,7 +42,7 @@ def get_error_dict(errors_full, min_points=10):error_dict = dict()for i in range(n_cams):- for j in range(i+1, n_cams):+ for j in range(i + 1, n_cams):subset = good[i] & good[j]err_subset = errors_norm[:, subset][[i, j]]err_subset_mean = np.mean(err_subset, axis=0)@@ -51,11 +52,13 @@ def get_error_dict(errors_full, min_points=10):error_dict[(i, j)] = (err_subset.shape[1], percents)return error_dict+def check_errors(cgroup, imgp):p3ds = cgroup.triangulate(imgp)errors_full = cgroup.reprojection_error(p3ds, imgp, mean=False)return get_error_dict(errors_full)+def subset_extra(extra, ixs):if extra is None:return None@@ -68,10 +71,11 @@ def subset_extra(extra, ixs):}return new_extra+def resample_points_extra(imgp, extra, n_samp=25):n_cams, n_points, _ = imgp.shapeids = remap_ids(extra['ids'])- n_ids = np.max(ids)+1+ n_ids = np.max(ids) + 1good = ~np.isnan(imgp[:, :, 0])ixs = np.arange(n_points)@@ -89,7 +93,7 @@ def resample_points_extra(imgp, extra, n_samp=25):include.update(ixs[ids == board_id])cam_totals += cam_counts[board_id]if cam_totals[cam_num] >= n_samp or \- cam_counts_random[board_id, cam_num] < 1:+ cam_counts_random[board_id, cam_num] < 1:breakfinal_ixs = sorted(include)@@ -97,6 +101,7 @@ def resample_points_extra(imgp, extra, n_samp=25):extra = subset_extra(extra, final_ixs)return newp, extra+def resample_points(imgp, extra=None, n_samp=25):# if extra is not None:# return resample_points_extra(imgp, extra, n_samp)@@ -110,7 +115,7 @@ def resample_points(imgp, extra=None, n_samp=25):include = set()for i in range(n_cams):- for j in range(i+1, n_cams):+ for j in range(i + 1, n_cams):subset = good[i] & good[j]n_good = np.sum(subset)if n_good > 0:@@ -126,15 +131,18 @@ def resample_points(imgp, extra=None, n_samp=25):extra = subset_extra(extra, final_ixs)return newp, extra+def medfilt_data(values, size=15):- padsize = size+5+ padsize = size + 5vpad = np.pad(values, (padsize, padsize), mode='reflect')vpadf = signal.medfilt(vpad, kernel_size=size)return vpadf[padsize:-padsize]+def nan_helper(y):return np.isnan(y), lambda z: z.nonzero()[0]+def interpolate_data(vals):nans, ix = nan_helper(vals)out = np.copy(vals)@@ -144,6 +152,7 @@ def interpolate_data(vals):out[:] = 0return out+def remap_ids(ids):unique_ids = np.unique(ids)ids_out = np.copy(ids)@@ -151,6 +160,7 @@ def remap_ids(ids):ids_out[ids == num] = ireturn ids_out+def transform_points(points, rvecs, tvecs):"""Rotate points by given rotation vectors and translate.Rodrigues' rotation formula is used.@@ -164,8 +174,8 @@ def transform_points(points, rvecs, tvecs):sin_theta = np.sin(theta)rotated = cos_theta * points + \- sin_theta * np.cross(v, points) + \- dot * (1 - cos_theta) * v+ sin_theta * np.cross(v, points) + \+ dot * (1 - cos_theta) * vreturn rotated + tvecs@@ -336,6 +346,7 @@ class Camera:name=self.get_name(),extra_dist=self.extra_dist)+class FisheyeCamera(Camera):def __init__(self,matrix=np.eye(3),@@ -403,7 +414,7 @@ class FisheyeCamera(Camera):self.set_distortions(dist)def get_params(self):- params = np.zeros(8+self.extra_dist, dtype='float64')+ params = np.zeros(8 + self.extra_dist, dtype='float64')params[0:3] = self.get_rotation()params[3:6] = self.get_translation()params[6] = self.get_focal_length()@@ -425,6 +436,7 @@ class FisheyeCamera(Camera):name=self.get_name(),extra_dist=self.extra_dist)+class CameraGroup:def __init__(self, cameras, metadata={}):self.cameras = cameras@@ -612,7 +624,6 @@ class CameraGroup:min_cams=min_cams,progress=progress)-@jit(parallel=True, forceobj=True)def reprojection_error(self, p3ds, p2ds, mean=False):"""Given an Nx3 array of 3D points and an CxNx2 array of 2D points,@@ -652,7 +663,6 @@ class CameraGroup:return errors-def bundle_adjust_iter(self, p2ds, extra=None,n_iters=10, start_mu=15, end_mu=1,max_nfev=200, ftol=1e-4,@@ -667,7 +677,7 @@ class CameraGroup:This is inspired by the algorithm for Fast Global Registration by Zhou, Park, and Koltun"""error_list = []-+assert p2ds.shape[0] == len(self.cameras), \"Invalid points shape, first dim should be equal to" \" number of cameras ({}), but shape is {}".format(@@ -726,7 +736,6 @@ class CameraGroup:max_nfev=max_nfev,verbose=verbose)-p2ds, extra = resample_points(p2ds_full, extra_full,n_samp=n_samp_full)p3ds = self.triangulate(p2ds)@@ -833,7 +842,7 @@ class CameraGroup:n_cams = len(self.cameras)sub = n_cam_params * n_camsn3d = p2ds.shape[1] * 3- p3ds_test = params[sub:sub+n3d].reshape(-1, 3)+ p3ds_test = params[sub:sub + n3d].reshape(-1, 3)errors = self.reprojection_error(p3ds_test, p2ds)errors_reproj = errors[good]@@ -842,9 +851,9 @@ class CameraGroup:objp = extra['objp']min_scale = np.min(objp[objp > 0])n_boards = int(np.max(ids)) + 1- a = sub+n3d- rvecs = params[a:a+n_boards*3].reshape(-1, 3)- tvecs = params[a+n_boards*3:a+n_boards*6].reshape(-1, 3)+ a = sub + n3d+ rvecs = params[a:a + n_boards * 3].reshape(-1, 3)+ tvecs = params[a + n_boards * 3:a + n_boards * 6].reshape(-1, 3)expected = transform_points(objp, rvecs[ids], tvecs[ids])errors_obj = 2 * (p3ds_test - expected).ravel() / min_scaleelse:@@ -852,7 +861,6 @@ class CameraGroup:return np.hstack([errors_reproj, errors_obj])-def _jac_sparsity_bundle(self, p2ds, n_cam_params, extra):"""Given an CxNx2 array of 2D points,where N is the number of points and C is the number of cameras,@@ -872,7 +880,7 @@ class CameraGroup:if extra is not None:ids = extra['ids_map']n_boards = int(np.max(ids)) + 1- total_board_params = n_boards * (3 + 3) # rvecs + tvecs+ total_board_params = n_boards * (3 + 3) # rvecs + tvecselse:n_boards = 0total_board_params = 0@@ -915,17 +923,15 @@ class CameraGroup:## update board rotation and translation based on error from expectedfor i in range(3):for j in range(3):- A_sparse[n_good_values + point_ix*3 + i,- total_params_reproj + ids*3 + j] = 1- A_sparse[n_good_values + point_ix*3 + i,- total_params_reproj + n_boards*3 + ids*3 + j] = 1-+ A_sparse[n_good_values + point_ix * 3 + i,+ total_params_reproj + ids * 3 + j] = 1+ A_sparse[n_good_values + point_ix * 3 + i,+ total_params_reproj + n_boards * 3 + ids * 3 + j] = 1## update point position based on error from expectedfor i in range(3):- A_sparse[n_good_values + point_ix*3 + i,- n_cams*n_cam_params + point_ix*3 + i] = 1-+ A_sparse[n_good_values + point_ix * 3 + i,+ n_cams * n_cam_params + point_ix * 3 + i] = 1return A_sparse@@ -949,7 +955,7 @@ class CameraGroup:if extra is not None:ids = extra['ids_map']n_boards = int(np.max(ids[~np.isnan(ids)])) + 1- total_board_params = n_boards * (3 + 3) # rvecs + tvecs+ total_board_params = n_boards * (3 + 3) # rvecs + tvecs# initialize to 0rvecs = np.zeros((n_boards, 3), dtype='float64')@@ -976,12 +982,12 @@ class CameraGroup:x0 = np.zeros(total_cam_params + p3ds.size + total_board_params)x0[:total_cam_params] = cam_params- x0[total_cam_params:total_cam_params+p3ds.size] = p3ds.ravel()+ x0[total_cam_params:total_cam_params + p3ds.size] = p3ds.ravel()if extra is not None:- start_board = total_cam_params+p3ds.size- x0[start_board:start_board + n_boards*3] = rvecs.ravel()- x0[start_board + n_boards*3:start_board + n_boards*6] = \+ start_board = total_cam_params + p3ds.size+ x0[start_board:start_board + n_boards * 3] = rvecs.ravel()+ x0[start_board + n_boards * 3:start_board + n_boards * 6] = \tvecs.ravel()return x0, n_cam_params@@ -1023,7 +1029,7 @@ class CameraGroup:p3ds_med = np.apply_along_axis(medfilt_data, 0, p3ds_intp, size=7)- default_smooth = 1.0/np.mean(np.abs(np.diff(p3ds_med, axis=0)))+ default_smooth = 1.0 / np.mean(np.abs(np.diff(p3ds_med, axis=0)))scale_smooth_full = scale_smooth * default_smootht1 = time.time()@@ -1040,7 +1046,7 @@ class CameraGroup:x0=x0, jac_sparsity=jac,loss='linear',ftol=1e-3,- verbose=2*verbose,+ verbose=2 * verbose,args=(points,constraints,constraints_weak,@@ -1061,7 +1067,6 @@ class CameraGroup:return p3ds_new2-def optim_points_possible(self, points, p3ds,constraints=[],constraints_weak=[],@@ -1100,7 +1105,7 @@ class CameraGroup:p3ds_med = np.apply_along_axis(medfilt_data, 0, p3ds_intp, size=7)- default_smooth = 1.0/np.mean(np.abs(np.diff(p3ds_med, axis=0)))+ default_smooth = 1.0 / np.mean(np.abs(np.diff(p3ds_med, axis=0)))scale_smooth_full = scale_smooth * default_smootht1 = time.time()@@ -1122,7 +1127,7 @@ class CameraGroup:x0=x0, jac_sparsity=jac,loss='linear',ftol=1e-3,- verbose=2*verbose,+ verbose=2 * verbose,args=(points,beta,constraints,@@ -1160,7 +1165,6 @@ class CameraGroup:return p3ds_new2, alphas_norm-def triangulate_optim(self, points, init_ransac=False, init_progress=False,**kwargs):"""@@ -1188,7 +1192,7 @@ class CameraGroup:# constraints = np.array(constraints)# constraints_weak = np.array(constraints_weak)- points_shaped = points.reshape(n_cams, n_frames*n_joints, 2)+ points_shaped = points.reshape(n_cams, n_frames * n_joints, 2)if init_ransac:p3ds, picked, p2ds, errors = self.triangulate_ransac(points_shaped, progress=init_progress)points = p2ds.reshape(points.shape)@@ -1203,8 +1207,6 @@ class CameraGroup:return self.optim_points(points, p3ds, **kwargs)--@jit(forceobj=True, parallel=True)def _error_fun_triangulation(self, params, p2ds,constraints=[],@@ -1218,14 +1220,14 @@ class CameraGroup:n_deriv_smooth=1):n_cams, n_frames, n_joints, _ = p2ds.shape- n_3d = n_frames*n_joints*3+ n_3d = n_frames * n_joints * 3n_constraints = len(constraints)n_constraints_weak = len(constraints_weak)# load paramsp3ds = params[:n_3d].reshape((n_frames, n_joints, 3))- joint_lengths = np.array(params[n_3d:n_3d+n_constraints])- joint_lengths_weak = np.array(params[n_3d+n_constraints:])+ joint_lengths = np.array(params[n_3d:n_3d + n_constraints])+ joint_lengths_weak = np.array(params[n_3d + n_constraints:])# reprojection errorsp3ds_flat = p3ds.reshape(-1, 3)@@ -1240,11 +1242,11 @@ class CameraGroup:errors_reproj = np.abs(errors_reproj)if reproj_loss == 'huber':bad = errors_reproj > rp- errors_reproj[bad] = rp*(2*np.sqrt(errors_reproj[bad]/rp) - 1)+ errors_reproj[bad] = rp * (2 * np.sqrt(errors_reproj[bad] / rp) - 1)elif reproj_loss == 'linear':passelif reproj_loss == 'soft_l1':- errors_reproj = rp*2*(np.sqrt(1+errors_reproj/rp)-1)+ errors_reproj = rp * 2 * (np.sqrt(1 + errors_reproj / rp) - 1)# temporal constrainterrors_smooth = np.diff(p3ds, n=n_deriv_smooth, axis=0).ravel() * scale_smooth@@ -1254,14 +1256,14 @@ class CameraGroup:for cix, (a, b) in enumerate(constraints):lengths = np.linalg.norm(p3ds[:, a] - p3ds[:, b], axis=1)expected = joint_lengths[cix]- errors_lengths[cix] = 100*(lengths - expected)/expected+ errors_lengths[cix] = 100 * (lengths - expected) / expectederrors_lengths = errors_lengths.ravel() * scale_l0engtherrors_lengths_weak = np.empty((n_constraints_weak, n_frames), dtype='float64')for cix, (a, b) in enumerate(constraints_weak):lengths = np.linalg.norm(p3ds[:, a] - p3ds[:, b], axis=1)expected = joint_lengths_weak[cix]- errors_lengths_weak[cix] = 100*(lengths - expected)/expected+ errors_lengths_weak[cix] = 100 * (lengths - expected) / expectederrors_lengths_weak = errors_lengths_weak.ravel() * scale_length_weakreturn np.hstack([errors_reproj, errors_smooth,@@ -1280,10 +1282,10 @@ class CameraGroup:n_cams, n_frames, n_joints, n_possible, _ = p2ds.shape- n_3d = n_frames*n_joints*3+ n_3d = n_frames * n_joints * 3n_constraints = len(constraints)n_constraints_weak = len(constraints_weak)- n_params_norm = n_3d+n_constraints+n_constraints_weak+ n_params_norm = n_3d + n_constraints + n_constraints_weak# load paramsbad = np.isnan(p2ds[:, :, :, :, 0])@@ -1314,8 +1316,7 @@ class CameraGroup:return np.hstack([errors, errors_alphas])-- def _initialize_params_triangulation(self, p3ds,+ def _initialize_params_triangulation(self, p3ds,constraints=[],constraints_weak=[]):joint_lengths = np.empty(len(constraints), dtype='float64')@@ -1325,7 +1326,6 @@ class CameraGroup:lengths = np.linalg.norm(p3ds[:, a] - p3ds[:, b], axis=1)joint_lengths[cix] = np.median(lengths)-for cix, (a, b) in enumerate(constraints_weak):lengths = np.linalg.norm(p3ds[:, a] - p3ds[:, b], axis=1)joint_lengths_weak[cix] = np.median(lengths)@@ -1339,8 +1339,8 @@ class CameraGroup:joint_lengths[joint_lengths == 0] = medjoint_lengths_weak[joint_lengths_weak == 0] = med- joint_lengths[joint_lengths > med+mad*5] = med- joint_lengths_weak[joint_lengths_weak > med+mad*5] = med+ joint_lengths[joint_lengths > med + mad * 5] = med+ joint_lengths_weak[joint_lengths_weak > med + mad * 5] = medreturn np.hstack([p3ds.ravel(), joint_lengths, joint_lengths_weak])@@ -1373,19 +1373,19 @@ class CameraGroup:for i in range(p2ds_flat.shape[1]):point_indices[:, i] = i- point_indices_3d = np.arange(n_frames*n_joints)\- .reshape((n_frames, n_joints))+ point_indices_3d = np.arange(n_frames * n_joints) \+ .reshape((n_frames, n_joints))good = ~np.isnan(p2ds_flat)n_errors_reproj = np.sum(good)- n_errors_smooth = (n_frames-n_deriv_smooth) * n_joints * 3+ n_errors_smooth = (n_frames - n_deriv_smooth) * n_joints * 3n_errors_lengths = n_constraints * n_framesn_errors_lengths_weak = n_constraints_weak * n_framesn_errors = n_errors_reproj + n_errors_smooth + \- n_errors_lengths + n_errors_lengths_weak+ n_errors_lengths + n_errors_lengths_weak- n_3d = n_frames*n_joints*3+ n_3d = n_frames * n_joints * 3n_params = n_3d + n_constraints + n_constraints_weakpoint_indices_good = point_indices[good]@@ -1398,20 +1398,20 @@ class CameraGroup:A_sparse[ix_reproj, point_indices_good * 3 + k] = 1# sparse constraints for smoothness in time- frames = np.arange(n_frames-n_deriv_smooth)+ frames = np.arange(n_frames - n_deriv_smooth)for j in range(n_joints):- for n in range(n_deriv_smooth+1):+ for n in range(n_deriv_smooth + 1):pa = point_indices_3d[frames, j]- pb = point_indices_3d[frames+n, j]+ pb = point_indices_3d[frames + n, j]for k in range(3):- A_sparse[n_errors_reproj + pa*3 + k, pb*3 + k] = 1+ A_sparse[n_errors_reproj + pa * 3 + k, pb * 3 + k] = 1## -- strong constraints --# joint lengths should change with joint lengths errorsstart = n_errors_reproj + n_errors_smoothframes = np.arange(n_frames)for cix, (a, b) in enumerate(constraints):- A_sparse[start + cix*n_frames + frames, n_3d+cix] = 1+ A_sparse[start + cix * n_frames + frames, n_3d + cix] = 1# points should change accordingly to match joint lengths tooframes = np.arange(n_frames)@@ -1419,15 +1419,15 @@ class CameraGroup:pa = point_indices_3d[frames, a]pb = point_indices_3d[frames, b]for k in range(3):- A_sparse[start + cix*n_frames + frames, pa*3 + k] = 1- A_sparse[start + cix*n_frames + frames, pb*3 + k] = 1+ A_sparse[start + cix * n_frames + frames, pa * 3 + k] = 1+ A_sparse[start + cix * n_frames + frames, pb * 3 + k] = 1## -- weak constraints --# joint lengths should change with joint lengths errorsstart = n_errors_reproj + n_errors_smooth + n_errors_lengthsframes = np.arange(n_frames)for cix, (a, b) in enumerate(constraints_weak):- A_sparse[start + cix*n_frames + frames, n_3d + n_constraints + cix] = 1+ A_sparse[start + cix * n_frames + frames, n_3d + n_constraints + cix] = 1# points should change accordingly to match joint lengths tooframes = np.arange(n_frames)@@ -1435,8 +1435,8 @@ class CameraGroup:pa = point_indices_3d[frames, a]pb = point_indices_3d[frames, b]for k in range(3):- A_sparse[start + cix*n_frames + frames, pa*3 + k] = 1- A_sparse[start + cix*n_frames + frames, pb*3 + k] = 1+ A_sparse[start + cix * n_frames + frames, pa * 3 + k] = 1+ A_sparse[start + cix * n_frames + frames, pb * 3 + k] = 1return A_sparse@@ -1461,8 +1461,8 @@ class CameraGroup:for r, c in zip(*A_sparse.nonzero()):B_sparse[r, c] = A_sparse[r, c]- point_indices_2d = np.arange(n_cams*n_frames*n_joints)\- .reshape(n_cams, n_frames, n_joints)+ point_indices_2d = np.arange(n_cams * n_frames * n_joints) \+ .reshape(n_cams, n_frames, n_joints)point_indices_2d_rep = np.repeat(point_indices_2d[:, :, :, None], 2, axis=3)point_indices_2d_good = point_indices_2d_rep[~np.isnan(p2ds)]point_indices_good = point_indices_2d[any_good]@@ -1573,7 +1573,7 @@ class CameraGroup:error = self.bundle_adjust_iter(imgp, extra, verbose=verbose, error_threshold=1)- return error,merged,charuco_frames+ return error, merged, charuco_framesdef get_rows_videos(self, videos, board, verbose=True):all_rows = []@@ -1607,10 +1607,10 @@ class CameraGroup:if init_extrinsics:self.set_camera_sizes_videos(videos)- error,merged,charuco_frames = self.calibrate_rows(all_rows, board,- init_intrinsics=init_intrinsics,- init_extrinsics=init_extrinsics,- verbose=verbose, **kwargs)+ error, merged, charuco_frames = self.calibrate_rows(all_rows, board,+ init_intrinsics=init_intrinsics,+ init_extrinsics=init_extrinsics,+ verbose=verbose, **kwargs)return error, merged, charuco_framesdef get_dicts(self):@@ -1629,7 +1629,7 @@ class CameraGroup:cameras.append(cam)return CameraGroup(cameras)- def from_names(names, fisheye=False):+ def from_names(self, names, fisheye=False):cameras = []for name in names:if fisheye:diff --git a/requirements.txt b/requirements.txtindex b0c81c8a..e5286eca 100644--- a/requirements.txt+++ b/requirements.txt@@ -33,4 +33,5 @@ pyqtgraph==0.12.4pyqt5==5.15.6pyopengl==3.1.6mediapipe~=0.8.9.1-setuptools~=58.0.4\ No newline at end of file+setuptools~=58.0.4+aniposelib~=0.4.3\ No newline at end of filediff --git a/src/api/routes/session/session_router.py b/src/api/routes/session/session_router.pyindex 70aaabe6..2da2ce49 100644--- a/src/api/routes/session/session_router.py+++ b/src/api/routes/session/session_router.py@@ -13,7 +13,7 @@ session_router = APIRouter()class SessionCreateModel(BaseModel):- user_session_str: Optional[str]+ user_session_tag_str: Optional[str]class SessionResponse(BaseModel):@@ -23,9 +23,14 @@ class SessionResponse(BaseModel):@session_router.post("/session/create")async def create_session(session_create_model: SessionCreateModel):- session_id = create_session_id(session_create_model.user_session_str)+ session_id = create_session_id(session_create_model.user_session_tag_str)session_path = Path(get_session_path(session_id))logger.info(f'Creating session folder at: {str(session_path)}')session_path.mkdir(parents=True, exist_ok=False)return SessionResponse(session_id=session_id,session_path=str(session_path))++@session_router.post("session/calibrate")+async def calibrate_session():+ """launch camera calibration (or load previous calibration)"""+ passdiff --git a/src/cameras/capture/dataclasses/frame_payload.py b/src/cameras/capture/dataclasses/frame_payload.pyindex aff71311..e060f480 100644--- a/src/cameras/capture/dataclasses/frame_payload.py+++ b/src/cameras/capture/dataclasses/frame_payload.py@@ -7,6 +7,7 @@ class FramePayload(NamedTuple):success: bool = Falseimage: np.ndarray = Nonetimestamp: Union[int, float] = None+ frame_number: int = Nonewebcam_id: str = Nonediff --git a/src/cameras/capture/opencv_camera/opencv_camera.py b/src/cameras/capture/opencv_camera/opencv_camera.pyindex 802df284..61b89e0f 100644--- a/src/cameras/capture/opencv_camera/opencv_camera.py+++ b/src/cameras/capture/opencv_camera/opencv_camera.py@@ -17,6 +17,7 @@ class OpenCVCamera:"""Performant implementation of video capture against webcams"""+def __init__(self, config: WebcamConfig, session_id: str = None):self._config = configself._name = f"Camera_{self._config.webcam_id}"@@ -24,6 +25,14 @@ class OpenCVCamera:self._video_recorder: VideoRecorder = Noneself._running_thread: VideoCaptureThread = Noneself._new_frame_ready = False+ self._number_of_frames_recorded = 0+ self._session_id = session_id++ @property+ def session_id(self):+ if self._session_id is None:+ return False+ return self._session_id@propertydef new_frame_ready(self):@@ -56,6 +65,24 @@ class OpenCVCamera:self._new_frame_ready = Falsereturn self._running_thread.latest_frame+ @property+ def latest_frame_number(self):+ return self._number_of_frames_recorded++ @property+ def image_width(self):+ try:+ return int(self._opencv_video_capture_object.get(3))+ except Exception as e:+ raise e++ @property+ def image_height(self):+ try:+ return int(self._opencv_video_capture_object.get(4))+ except Exception as e:+ raise e+def connect(self):if platform.system() == "Windows":cap_backend = cv2.CAP_DSHOW@@ -76,20 +103,27 @@ class OpenCVCamera:)return False- try:- image_height = image.shape[0]- image_width = image.shape[1]- self._video_recorder = VideoRecorder(self._name,- image_width=image_width,- image_height=image_height)- except:- logger.error(f"could not create video recorder for image with shape (image_width={image_width}, image_height={image_height}) ")- return False-logger.debug(f"Camera found at port number {self._config.webcam_id}")fps_input_stream = int(self._opencv_video_capture_object.get(5))logger.debug("FPS of webcam hardware/input stream: {}".format(fps_input_stream))+ if self.session_id is False:+ logger.info(+ f"No `session_id` specified for {self._name}, video_recorder will not be created (because we won't know where to save the videos)")+ self._video_recorder = None+ else:+ image_height = image.shape[0]+ image_width = image.shape[1]+ try:+ self._video_recorder = VideoRecorder(self._name,+ image_width=image_width,+ image_height=image_height,+ session_id=self.session_id)+ except:+ logger.error(+ f"could not create video recorder for image with shape (image_width={image_width}, image_height={image_height}) ")+ return False # NOTE- this is a failure because we *tried* to make a video_recorder and it didn't work+return successdef start_frame_capture_thread(self):@@ -109,21 +143,6 @@ class OpenCVCamera:get_next_frame=self.get_next_frame,)- @property- def image_width(self):- try:- return int(self._opencv_video_capture_object.get(3))- except Exception as e:- raise e--- @property- def image_height(self):- try:- return int(self._opencv_video_capture_object.get(4))- except Exception as e:- raise e-def _apply_configuration(self):# set camera stream parametersself._opencv_video_capture_object.set(@@ -153,10 +172,18 @@ class OpenCVCamera:success, image = self._opencv_video_capture_object.retrieve()timestamp_ns_post = time.perf_counter_ns()- timestamp_ns = (timestamp_ns_pre + timestamp_ns_post)/2+ timestamp_ns = (timestamp_ns_pre + timestamp_ns_post) / 2self._new_frame_ready = success- return FramePayload(success, image, timestamp_ns, self.webcam_id_as_str)++ if success:+ self._number_of_frames_recorded += 1++ return FramePayload(success=success,+ image=image,+ timestamp=timestamp_ns,+ frame_number=self.latest_frame_number,+ webcam_id=self.webcam_id_as_str)def stop_frame_capture(self):self.close()diff --git a/src/cameras/multicam_manager/cv_camera_manager.py b/src/cameras/multicam_manager/cv_camera_manager.pyindex be2f3f05..ac223984 100644--- a/src/cameras/multicam_manager/cv_camera_manager.py+++ b/src/cameras/multicam_manager/cv_camera_manager.py@@ -46,7 +46,8 @@ class OpenCVCameraManager:exposure=webcam_config_model.exposure,resolution_width=webcam_config_model.resolution_width,resolution_height=webcam_config_model.resolution_height, )- return OpenCVCamera(config=single_camera_config)+ return OpenCVCamera(config=single_camera_config,+ session_id=self._session_id)@contextmanagerdef start_capture_session_single_cam(diff --git a/src/cameras/persistence/video_writer/video_recorder.py b/src/cameras/persistence/video_writer/video_recorder.pyindex 998ac8f7..92724263 100644--- a/src/cameras/persistence/video_writer/video_recorder.py+++ b/src/cameras/persistence/video_writer/video_recorder.py@@ -11,6 +11,7 @@ import pandas as pdfrom src.cameras.capture.dataclasses.frame_payload import FramePayloadfrom src.cameras.persistence.video_writer.save_options_dataclass import SaveOptionsfrom src.config.data_paths import freemocap_data_path+from src.config.home_dir import get_session_pathlogger = logging.getLogger(__name__)@@ -20,71 +21,108 @@ class VideoRecorder:camera_name: str,image_width: int,image_height: int,- fourcc: str = "MP4V"+ session_id: str,+ fourcc: str = "MP4V",):self._camera_name = camera_nameself._image_width = image_widthself._image_height = image_heightself._fourcc = fourcc- self._frames: List[FramePayload] = []- self._path_to_save_video = None+ self._frame_list: List[FramePayload] = []self._video_type_str = ''self._timestamps_npy = np.empty(0)+ self._cv2_video_writer = None++ # get yr paths straight+ session_path = Path(get_session_path(session_id))+ self._synchronized_videos_folder_path = session_path / 'synchronized_videos'+ self._synchronized_videos_folder_path.mkdir(parents=True, exist_ok=True)+ video_file_name = self._camera_name + '_synchronized.mp4'+ self._path_to_save_video_file = self._synchronized_videos_folder_path / video_file_name+++ @property+ def synchronized_videos_folder_path(self):+ return self._synchronized_videos_folder_path++ @property+ def path_to_save_video_file(self):+ return self._path_to_save_video_file@propertydef frame_count(self):- return len(self._frames)+ return len(self._frame_list)- def record(self, frame_payload: FramePayload):- self._frames.append(frame_payload)+ @property+ def median_framerate(self):+ if self.frame_count < 10:+ logger.warning(f"camera framerate not known, setting video playback rate to 30 frames per second")+ self._median_framerate = 30+ else:+ self._gather_timestamps()+ self._median_framerate = (np.nanmedian(np.diff(self._timestamps_npy / 1e9))) ** -1- def save_to_disk(self, path_to_save_video: Path, video_type_str: str = 'raw'):- if len(self._frames) == 0:+ return self._median_framerate++ def save_frame_to_video_file(self,frame_payload: FramePayload):+ if self._cv2_video_writer is None:+ self._initialize_video_writer()++ self._cv2_video_writer.write(frame_payload.image)++ def close(self):+ self._cv2_video_writer.release()++ def append_frame_to_list(self, frame_payload: FramePayload):+ self._frame_list.append(frame_payload)++ def save_frame_list_to_disk(self):+ if len(self._frame_list) == 0:logging.error(f"No frames to save for camera: {self._camera_name}")return- self._video_type_str = video_type_str- self._path_to_save_video = path_to_save_video- self._path_to_save_video.mkdir(parents=True, exist_ok=True)self._gather_timestamps()- self._write_video()+ self._initialize_video_writer()+ self._write_frame_list_to_video_file()self._save_timestamps()- def _write_video(self):- video_file_name = self._camera_name + "_synchronized_" + self._video_type_str + ".mp4"- video_full_path = self._path_to_save_video / video_file_name+ def _initialize_video_writer(self):- cv2_writer = cv2.VideoWriter(- str(video_full_path),+ self._cv2_video_writer = cv2.VideoWriter(+ str(self.path_to_save_video_file),cv2.VideoWriter_fourcc(*self._fourcc),- self._median_framerate,+ self.median_framerate,(self._image_width, self._image_height))+ def _write_frame_list_to_video_file(self):+try:- for frame in self._frames:- cv2_writer.write(frame.image)+ for frame in self._frame_list:+ self._cv2_video_writer.write(frame.image)except Exception as e:logger.debug("Failed during save in video writer")traceback.print_exc()raise efinally:- logger.info(f"Saved video to path: {video_full_path}")- cv2_writer.release()+ logger.info(f"Saved video to path: {self.path_to_save_video_file}")+ self._cv2_video_writer.release()def _gather_timestamps(self):try:- for frame in self._frames:+ for frame in self._frame_list:self._timestamps_npy = np.append(self._timestamps_npy, frame.timestamp)- self._median_framerate = (np.nanmedian(np.diff(self._timestamps_npy / 1e9))) ** -1except:logger.error("Error gathering timestamps")def _save_timestamps(self):- timestamp_file_name_npy = self._camera_name + "_timestamps.npy"- np.save(str(self._path_to_save_video / timestamp_file_name_npy), self._timestamps_npy)+ timestamp_file_name_npy = self._camera_name + "_timestamps_binary.npy"+ timestamp_npy_full_save_path = self._synchronized_videos_folder_path / timestamp_file_name_npy+ np.save(str(timestamp_npy_full_save_path), self._timestamps_npy)logger.info(f"Saved timestamps to path: {timestamp_file_name_npy}")- timestamp_file_name_csv = self._camera_name + "_timestamps.csv"- self._timestamps_npy.tofile(str(self._path_to_save_video / timestamp_file_name_csv), sep=',\n')+ timestamp_file_name_csv = self._camera_name + "_timestamps_human_readable.csv"+ timestamp_csv_full_save_path = self._synchronized_videos_folder_path / timestamp_file_name_csv+ timestamp_dataframe = pd.DataFrame(self._timestamps_npy)+ timestamp_dataframe.to_csv(str(timestamp_csv_full_save_path))logger.info(f"Saved timestamps to path: {timestamp_file_name_csv}")diff --git a/src/core_processor/camera_calibration/camera_calibrator.py b/src/core_processor/camera_calibration/camera_calibrator.pyindex fe0146b7..72c123b6 100644--- a/src/core_processor/camera_calibration/camera_calibrator.py+++ b/src/core_processor/camera_calibration/camera_calibrator.py@@ -1,16 +1,14 @@import logging-from dataclasses import dataclassimport cv2-import numpy as npfrom src.cameras.capture.dataclasses.frame_payload import FramePayloadfrom src.cameras.capture.opencv_camera.opencv_camera import OpenCVCamerafrom src.core_processor.camera_calibration.calibration_dataclasses import CameraCalibrationData-from src.core_processor.camera_calibration.charuco_board_detection.charuco_board_detector import CharucoBoardDetector-from src.core_processor.camera_calibration.charuco_board_detection.charuco_image_annotator import \+from src.pipelines.calibration_pipeline.charuco_board_detection import CharucoBoardDetector+from src.pipelines.calibration_pipeline.charuco_board_detection import \annotate_image_with_charuco_data-from src.core_processor.camera_calibration.charuco_board_detection.dataclasses.charuco_view_data import CharucoViewData+from src.pipelines.calibration_pipeline.charuco_board_detection.dataclasses import CharucoViewDatalogger = logging.getLogger(__name__)diff --git a/src/core_processor/camera_calibration/charuco_board_detection/dataclasses/charuco_board_definition.py b/src/core_processor/camera_calibration/charuco_board_detection/dataclasses/charuco_board_definition.pydeleted file mode 100644index 08faa8c4..00000000--- a/src/core_processor/camera_calibration/charuco_board_detection/dataclasses/charuco_board_definition.py+++ /dev/null@@ -1,37 +0,0 @@-from dataclasses import dataclass-from typing import Dict--import cv2-import numpy as np---@dataclass-class CharucoBoard:- _aruco_marker_dict: Dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_4X4_250)- _number_of_squares_width: int = 7- _number_of_squares_height: int = 5- _black_square_side_length: int = 1- _aruco_marker_length_proportional: float = .8-- def __post_init__(self):- self._charuco_board = cv2.aruco.CharucoBoard_create(self._number_of_squares_width,- self._number_of_squares_height,- self._black_square_side_length,- self._aruco_marker_length_proportional,- self._aruco_marker_dict)-- self._number_of_charuco_corners = (self._number_of_squares_width - 1) * (self._number_of_squares_height - 1)--- @property- def charuco_board(self):- return self._charuco_board-- @property- def number_of_charuco_corners(self):- return self._number_of_charuco_corners-- @property- def aruco_marker_dict(self):- return self._aruco_marker_dict-diff --git a/src/core_processor/camera_calibration/lens_distortion_calibrator.py b/src/core_processor/camera_calibration/lens_distortion_calibrator.pyindex db582a86..5e21df22 100644--- a/src/core_processor/camera_calibration/lens_distortion_calibrator.py+++ b/src/core_processor/camera_calibration/lens_distortion_calibrator.py@@ -4,10 +4,10 @@ from rich import printfrom src.cameras.capture.dataclasses.frame_payload import FramePayloadfrom src.core_processor.camera_calibration.calibration_dataclasses import LensDistortionCalibrationData-from src.core_processor.camera_calibration.charuco_board_detection.board_detector import BoardDetector+from src.pipelines.calibration_pipeline.charuco_board_detection import BoardDetectorfrom src.core_processor.camera_calibration.calibration_diagnostics_visualizer import CalibrationDiagnosticsVisualizer-from src.core_processor.camera_calibration.charuco_board_detection.dataclasses.charuco_view_data import CharucoViewData-from src.core_processor.camera_calibration.charuco_board_detection.charuco_image_annotator import \+from src.pipelines.calibration_pipeline.charuco_board_detection.dataclasses import CharucoViewData+from src.pipelines.calibration_pipeline.charuco_board_detection import \annotate_image_with_charuco_datadiff --git a/src/core_processor/fps/timestamp_manager.py b/src/core_processor/fps/timestamp_manager.pyindex ef6dd1cc..04c391a0 100644--- a/src/core_processor/fps/timestamp_manager.py+++ b/src/core_processor/fps/timestamp_manager.py@@ -1,4 +1,4 @@-from typing import Dict, List+from typing import Dict, List, Unionimport numpy as np@@ -71,8 +71,11 @@ class TimestampManager:def median_frames_per_second_for_main_loop(self):return self._main_loop_timestamp_logger.median_frames_per_second- def increment_frame_processed_for_webcam(self, webcam_id, timestamp):+ def increment_frame_processed_for_webcam(self, webcam_id: str, timestamp: Union[float, int],+ frame_number: int = None):self._webcam_timestamp_loggers[webcam_id].log_new_timestamp(timestamp)+ if frame_number is not None:+ assert (self._webcam_timestamp_loggers[webcam_id].number_of_frames == frame_number, f"Numer of timestamps ({self._webcam_timestamp_loggers[webcam_id].number_of_frames}) logged does not match frame_number ({frame_number}) for: camera_{webcam_id}")def median_frames_per_second_for_webcam(self, webcam_id):return self._webcam_timestamp_loggers[webcam_id].median_frames_per_seconddiff --git a/src/core_processor/camera_calibration/charuco_board_detection/__init__.py b/src/pipelines/__init__.pysimilarity index 100%rename from src/core_processor/camera_calibration/charuco_board_detection/__init__.pyrename to src/pipelines/__init__.pydiff --git a/src/core_processor/camera_calibration/charuco_board_detection/dataclasses/__init__.py b/src/pipelines/calibration_pipeline/__init__.pysimilarity index 100%rename from src/core_processor/camera_calibration/charuco_board_detection/dataclasses/__init__.pyrename to src/pipelines/calibration_pipeline/__init__.pydiff --git a/src/session_pipeline/__init__.py b/src/pipelines/calibration_pipeline/anipose_camera_calibration/__init__.pysimilarity index 100%rename from src/session_pipeline/__init__.pyrename to src/pipelines/calibration_pipeline/anipose_camera_calibration/__init__.pydiff --git a/src/pipelines/calibration_pipeline/anipose_camera_calibration/anipose_camera_calibration.py b/src/pipelines/calibration_pipeline/anipose_camera_calibration/anipose_camera_calibration.pynew file mode 100644index 00000000..ed52dc1c--- /dev/null+++ b/src/pipelines/calibration_pipeline/anipose_camera_calibration/anipose_camera_calibration.py@@ -0,0 +1,79 @@+import logging+from pathlib import Path+from typing import Union++from cv2.cv2 import aruco_CharucoBoard++from src.config.home_dir import get_session_path+from src.pipelines.calibration_pipeline.anipose_camera_calibration import freemocap_anipose+from aniposelib.boards import CharucoBoard as AniposeCharucoBoard++from src.pipelines.calibration_pipeline.charuco_board_detection.dataclasses.charuco_board_definition import CharucoBoard++logger = logging.getLogger(__name__)+++class AniposeCameraCalibrator:+ def __init__(self,+ session_id: str,+ charuco_board_object: CharucoBoard,+ charuco_square_size:Union[int,float] = 1,+ ):++ self._session_id = session_id+ self._charuco_board_object = charuco_board_object++ if charuco_square_size == 1:+ logger.warning('Charuco square size is not set, so units of 3d reconstructed data will be in units of `however_long_the_black_edge_of_the_charuco_square_was`. Please input `charuco_square_size` in millimeters (or your preferred unity of length)')+ self._charuco_square_size = charuco_square_size+ self.get_paths_and_whatnot()+ self.initialize_anipose_objects()++ def get_paths_and_whatnot(self):+ self.session_folder_path = Path(get_session_path(self._session_id))+ synchronized_videos_folder = self.session_folder_path / 'synchronized_videos'+ self._list_of_video_paths = [this_video_path for this_video_path in+ synchronized_videos_folder.glob('*.mp4'.lower())]++ def initialize_anipose_objects(self):+ list_of_camera_names = [this_video_path.stem for this_video_path in self._list_of_video_paths]+ self._anipose_camera_group_object = freemocap_anipose.CameraGroup.from_names(list_of_camera_names)+ self._anipose_charuco_board = AniposeCharucoBoard(self._charuco_board_object.number_of_squares_width,+ self._charuco_board_object.number_of_squares_height,+ square_length=self._charuco_square_size, # mm+ marker_length=self._charuco_square_size * .8,+ marker_bits=4,+ dict_size=250)++ def calibrate_camera_capture_volume(self):+ # anipose needs this to be a list of lists (which is annoying but whatevs)+ video_paths_list_of_list_of_strings = [[str(this_path)] for this_path in self._list_of_video_paths]++ error, charuco_frame_data, charuco_frame_numbers = self._anipose_camera_group_object.calibrate_videos(+ video_paths_list_of_list_of_strings,+ self._anipose_charuco_board)++ logger.info("Anipose Calibration Successful!")++ # save calibration info to files+ calibration_toml_filename = f"{self._session_id}_camera_calibration.toml"+ camera_calibration_toml_path = self.session_folder_path / calibration_toml_filename+ self._anipose_camera_group_object.dump(camera_calibration_toml_path)+ logger.info(f"anipose camera calibration data saved to {str(camera_calibration_toml_path)}")+ # # convert charuco data into a format that can be 3d reconstructed (effectively providing dummy data for the rest of the 3d reconstruction pipeline)+ # self.charuco_nCams_nFrames_nImgPts_XY = np.empty(+ # [self.multi_cam.num_cams, self.multi_cam.num_frames, num_charuco_tracked_points, 2])+ # self.charuco_nCams_nFrames_nImgPts_XY[:] = np.nan+ #+ # for this_charuco_frame_data, this_charuco_frame_num in zip(charuco_frame_data, charuco_frame_numbers):+ # for this_cam_num in range(self.multi_cam.num_cams):+ # try:+ # self.charuco_nCams_nFrames_nImgPts_XY[this_cam_num, this_charuco_frame_num, :, :] = np.squeeze(+ # this_charuco_frame_data[this_cam_num]["filled"])+ # except:+ # # print("failed frame:", frame)+ # continue+++if __name__ == "__main__":+ passdiff --git a/src/pipelines/calibration_pipeline/anipose_camera_calibration/freemocap_anipose.py b/src/pipelines/calibration_pipeline/anipose_camera_calibration/freemocap_anipose.pynew file mode 100644index 00000000..1e3e652f--- /dev/null+++ b/src/pipelines/calibration_pipeline/anipose_camera_calibration/freemocap_anipose.py@@ -0,0 +1,1666 @@+import cv2+import numpy as np+from copy import copy+from scipy.sparse import lil_matrix, dok_matrix+from scipy import optimize+from scipy import signal+from numba import jit+from collections import defaultdict, Counter+import toml+import itertools+from tqdm import trange+from pprint import pprint+import time++from aniposelib.boards import merge_rows, extract_points, \+ extract_rtvecs, get_video_params+from aniposelib.utils import get_initial_extrinsics, make_M, get_rtvec, \+ get_connections+++@jit(nopython=True, parallel=True)+def triangulate_simple(points, camera_mats):+ num_cams = len(camera_mats)+ A = np.zeros((num_cams * 2, 4))+ for i in range(num_cams):+ x, y = points[i]+ mat = camera_mats[i]+ A[(i * 2):(i * 2 + 1)] = x * mat[2] - mat[0]+ A[(i * 2 + 1):(i * 2 + 2)] = y * mat[2] - mat[1]+ u, s, vh = np.linalg.svd(A, full_matrices=True)+ p3d = vh[-1]+ p3d = p3d[:3] / p3d[3]+ return p3d+++def get_error_dict(errors_full, min_points=10):+ n_cams = errors_full.shape[0]+ errors_norm = np.linalg.norm(errors_full, axis=2)++ good = ~np.isnan(errors_full[:, :, 0])++ error_dict = dict()++ for i in range(n_cams):+ for j in range(i + 1, n_cams):+ subset = good[i] & good[j]+ err_subset = errors_norm[:, subset][[i, j]]+ err_subset_mean = np.mean(err_subset, axis=0)+ if np.sum(subset) > min_points:+ percents = np.percentile(err_subset_mean, [15, 75])+ # percents = np.percentile(err_subset, [25, 75])+ error_dict[(i, j)] = (err_subset.shape[1], percents)+ return error_dict+++def check_errors(cgroup, imgp):+ p3ds = cgroup.triangulate(imgp)+ errors_full = cgroup.reprojection_error(p3ds, imgp, mean=False)+ return get_error_dict(errors_full)+++def subset_extra(extra, ixs):+ if extra is None:+ return None++ new_extra = {+ 'objp': extra['objp'][ixs],+ 'ids': extra['ids'][ixs],+ 'rvecs': extra['rvecs'][:, ixs],+ 'tvecs': extra['tvecs'][:, ixs]+ }+ return new_extra+++def resample_points_extra(imgp, extra, n_samp=25):+ n_cams, n_points, _ = imgp.shape+ ids = remap_ids(extra['ids'])+ n_ids = np.max(ids) + 1+ good = ~np.isnan(imgp[:, :, 0])+ ixs = np.arange(n_points)++ cam_counts = np.zeros((n_ids, n_cams), dtype='int64')+ for idnum in range(n_ids):+ cam_counts[idnum] = np.sum(good[:, ids == idnum], axis=1)+ cam_counts_random = cam_counts + np.random.random(size=cam_counts.shape)+ best_boards = np.argsort(-cam_counts_random, axis=0)++ cam_totals = np.zeros(n_cams, dtype='int64')++ include = set()+ for cam_num in range(n_cams):+ for board_id in best_boards[:, cam_num]:+ include.update(ixs[ids == board_id])+ cam_totals += cam_counts[board_id]+ if cam_totals[cam_num] >= n_samp or \+ cam_counts_random[board_id, cam_num] < 1:+ break++ final_ixs = sorted(include)+ newp = imgp[:, final_ixs]+ extra = subset_extra(extra, final_ixs)+ return newp, extra+++def resample_points(imgp, extra=None, n_samp=25):+ # if extra is not None:+ # return resample_points_extra(imgp, extra, n_samp)++ n_cams = imgp.shape[0]+ good = ~np.isnan(imgp[:, :, 0])+ ixs = np.arange(imgp.shape[1])++ num_cams = np.sum(~np.isnan(imgp[:, :, 0]), axis=0)++ include = set()++ for i in range(n_cams):+ for j in range(i + 1, n_cams):+ subset = good[i] & good[j]+ n_good = np.sum(subset)+ if n_good > 0:+ ## pick points, prioritizing points seen by more cameras+ arr = np.copy(num_cams[subset]).astype('float64')+ arr += np.random.random(size=arr.shape)+ picked_ix = np.argsort(-arr)[:n_samp]+ picked = ixs[subset][picked_ix]+ include.update(picked)++ final_ixs = sorted(include)+ newp = imgp[:, final_ixs]+ extra = subset_extra(extra, final_ixs)+ return newp, extra+++def medfilt_data(values, size=15):+ padsize = size + 5+ vpad = np.pad(values, (padsize, padsize), mode='reflect')+ vpadf = signal.medfilt(vpad, kernel_size=size)+ return vpadf[padsize:-padsize]+++def nan_helper(y):+ return np.isnan(y), lambda z: z.nonzero()[0]+++def interpolate_data(vals):+ nans, ix = nan_helper(vals)+ out = np.copy(vals)+ try:+ out[nans] = np.interp(ix(nans), ix(~nans), vals[~nans])+ except ValueError:+ out[:] = 0+ return out+++def remap_ids(ids):+ unique_ids = np.unique(ids)+ ids_out = np.copy(ids)+ for i, num in enumerate(unique_ids):+ ids_out[ids == num] = i+ return ids_out+++def transform_points(points, rvecs, tvecs):+ """Rotate points by given rotation vectors and translate.+ Rodrigues' rotation formula is used.+ """+ theta = np.linalg.norm(rvecs, axis=1)[:, np.newaxis]+ with np.errstate(invalid='ignore'):+ v = rvecs / theta+ v = np.nan_to_num(v)+ dot = np.sum(points * v, axis=1)[:, np.newaxis]+ cos_theta = np.cos(theta)+ sin_theta = np.sin(theta)++ rotated = cos_theta * points + \+ sin_theta * np.cross(v, points) + \+ dot * (1 - cos_theta) * v++ return rotated + tvecs+++class Camera:+ def __init__(self,+ matrix=np.eye(3),+ dist=np.zeros(5),+ size=None,+ rvec=np.zeros(3),+ tvec=np.zeros(3),+ name=None,+ extra_dist=False):++ self.set_camera_matrix(matrix)+ self.set_distortions(dist)+ self.set_size(size)+ self.set_rotation(rvec)+ self.set_translation(tvec)+ self.set_name(name)+ self.extra_dist = extra_dist++ def get_dict(self):+ return {+ 'name': self.get_name(),+ 'size': list(self.get_size()),+ 'matrix': self.get_camera_matrix().tolist(),+ 'distortions': self.get_distortions().tolist(),+ 'rotation': self.get_rotation().tolist(),+ 'translation': self.get_translation().tolist(),+ }++ def load_dict(self, d):+ self.set_camera_matrix(d['matrix'])+ self.set_rotation(d['rotation'])+ self.set_translation(d['translation'])+ self.set_distortions(d['distortions'])+ self.set_name(d['name'])+ self.set_size(d['size'])++ def from_dict(d):+ cam = Camera()+ cam.load_dict(d)+ return cam++ def get_camera_matrix(self):+ return self.matrix++ def get_distortions(self):+ return self.dist++ def set_camera_matrix(self, matrix):+ self.matrix = np.array(matrix, dtype='float64')++ def set_focal_length(self, fx, fy=None):+ if fy is None:+ fy = fx+ self.matrix[0, 0] = fx+ self.matrix[1, 1] = fy++ def get_focal_length(self, both=False):+ fx = self.matrix[0, 0]+ fy = self.matrix[1, 1]+ if both:+ return (fx, fy)+ else:+ return (fx + fy) / 2.0++ def set_distortions(self, dist):+ self.dist = np.array(dist, dtype='float64').ravel()++ def set_rotation(self, rvec):+ self.rvec = np.array(rvec, dtype='float64').ravel()++ def get_rotation(self):+ return self.rvec++ def set_translation(self, tvec):+ self.tvec = np.array(tvec, dtype='float64').ravel()++ def get_translation(self):+ return self.tvec++ def get_extrinsics_mat(self):+ return make_M(self.rvec, self.tvec)++ def get_name(self):+ return self.name++ def set_name(self, name):+ self.name = str(name)++ def set_size(self, size):+ """set size as (width, height)"""+ self.size = size++ def get_size(self):+ """get size as (width, height)"""+ return self.size++ def resize_camera(self, scale):+ """resize the camera by scale factor, updating intrinsics to match"""+ size = self.get_size()+ new_size = size[0] * scale, size[1] * scale+ matrix = self.get_camera_matrix()+ new_matrix = matrix * scale+ new_matrix[2, 2] = 1+ self.set_size(new_size)+ self.set_camera_matrix(new_matrix)++ def get_params(self):+ params = np.zeros(8 + self.extra_dist, dtype='float64')+ params[0:3] = self.get_rotation()+ params[3:6] = self.get_translation()+ params[6] = self.get_focal_length()+ dist = self.get_distortions()+ params[7] = dist[0]+ if self.extra_dist:+ params[8] = dist[1]+ return params++ def set_params(self, params):+ self.set_rotation(params[0:3])+ self.set_translation(params[3:6])+ self.set_focal_length(params[6])++ dist = np.zeros(5, dtype='float64')+ dist[0] = params[7]+ if self.extra_dist:+ dist[1] = params[8]+ self.set_distortions(dist)++ def distort_points(self, points):+ shape = points.shape+ points = points.reshape(-1, 1, 2)+ new_points = np.dstack([points, np.ones((points.shape[0], 1, 1))])+ out, _ = cv2.projectPoints(new_points, np.zeros(3), np.zeros(3),+ self.matrix.astype('float64'),+ self.dist.astype('float64'))+ return out.reshape(shape)++ def undistort_points(self, points):+ shape = points.shape+ points = points.reshape(-1, 1, 2)+ out = cv2.undistortPoints(points,+ self.matrix.astype('float64'),+ self.dist.astype('float64'))+ return out.reshape(shape)++ def project(self, points):+ points = points.reshape(-1, 1, 3)+ out, _ = cv2.projectPoints(points, self.rvec, self.tvec,+ self.matrix.astype('float64'),+ self.dist.astype('float64'))+ return out++ def reprojection_error(self, p3d, p2d):+ proj = self.project(p3d).reshape(p2d.shape)+ return p2d - proj++ def copy(self):+ return \+ Camera(matrix=self.get_camera_matrix().copy(),+ dist=self.get_distortions().copy(),+ size=self.get_size(),+ rvec=self.get_rotation().copy(),+ tvec=self.get_translation().copy(),+ name=self.get_name(),+ extra_dist=self.extra_dist)+++class FisheyeCamera(Camera):+ def __init__(self,+ matrix=np.eye(3),+ dist=np.zeros(4),+ size=None,+ rvec=np.zeros(3),+ tvec=np.zeros(3),+ name=None,+ extra_dist=False):+ self.set_camera_matrix(matrix)+ self.set_distortions(dist)+ self.set_size(size)+ self.set_rotation(rvec)+ self.set_translation(tvec)+ self.set_name(name)+ self.extra_dist = extra_dist++ def from_dict(d):+ cam = FisheyeCamera()+ cam.load_dict(d)+ return cam++ def get_dict(self):+ d = super().get_dict()+ d['fisheye'] = True+ return d++ def distort_points(self, points):+ shape = points.shape+ points = points.reshape(-1, 1, 2)+ new_points = np.dstack([points, np.ones((points.shape[0], 1, 1))])+ out, _ = cv2.fisheye.projectPoints(new_points,+ np.zeros(3), np.zeros(3),+ self.matrix.astype('float64'),+ self.dist.astype('float64'))+ return out.reshape(shape)++ def undistort_points(self, points):+ shape = points.shape+ points = points.reshape(-1, 1, 2)+ out = cv2.fisheye.undistortPoints(points.astype('float64'),+ self.matrix.astype('float64'),+ self.dist.astype('float64'))+ return out.reshape(shape)++ def project(self, points):+ points = points.reshape(-1, 1, 3)+ out, _ = cv2.fisheye.projectPoints(points,+ self.rvec, self.tvec,+ self.matrix.astype('float64'),+ self.dist.astype('float64'))+ return out++ def set_params(self, params):+ self.set_rotation(params[0:3])+ self.set_translation(params[3:6])+ self.set_focal_length(params[6])++ dist = np.zeros(4, dtype='float64')+ dist[0] = params[7]+ if self.extra_dist:+ dist[1] = params[8]+ # dist[2] = params[9]+ # dist[3] = params[10]+ self.set_distortions(dist)++ def get_params(self):+ params = np.zeros(8 + self.extra_dist, dtype='float64')+ params[0:3] = self.get_rotation()+ params[3:6] = self.get_translation()+ params[6] = self.get_focal_length()+ dist = self.get_distortions()+ params[7] = dist[0]+ if self.extra_dist:+ params[8] = dist[1]+ # params[9] = dist[2]+ # params[10] = dist[3]+ return params++ def copy(self):+ return FisheyeCamera(+ matrix=self.get_camera_matrix().copy(),+ dist=self.get_distortions().copy(),+ size=self.get_size(),+ rvec=self.get_rotation().copy(),+ tvec=self.get_translation().copy(),+ name=self.get_name(),+ extra_dist=self.extra_dist)+++class CameraGroup:+ def __init__(self, cameras, metadata={}):+ self.cameras = cameras+ self.metadata = metadata++ def subset_cameras(self, indices):+ cams = [self.cameras[ix].copy() for ix in indices]+ return CameraGroup(cams, self.metadata)++ def subset_cameras_names(self, names):+ cur_names = self.get_names()+ cur_names_dict = dict(zip(cur_names, range(len(cur_names))))+ indices = []+ for name in names:+ if name not in cur_names_dict:+ raise IndexError(+ "name {} not part of camera names: {}".format(+ name, cur_names+ ))+ indices.append(cur_names_dict[name])+ return self.subset_cameras(indices)++ def project(self, points):+ """Given an Nx3 array of points, this returns an CxNx2 array of 2D points,+ where C is the number of cameras"""+ points = points.reshape(-1, 1, 3)+ n_points = points.shape[0]+ n_cams = len(self.cameras)++ out = np.empty((n_cams, n_points, 2), dtype='float64')+ for cnum, cam in enumerate(self.cameras):+ out[cnum] = cam.project(points).reshape(n_points, 2)++ return out++ def triangulate(self, points, undistort=True, progress=False):+ """Given an CxNx2 array, this returns an Nx3 array of points,+ where N is the number of points and C is the number of cameras"""++ assert points.shape[0] == len(self.cameras), \+ "Invalid points shape, first dim should be equal to" \+ " number of cameras ({}), but shape is {}".format(+ len(self.cameras), points.shape+ )++ one_point = False+ if len(points.shape) == 2:+ points = points.reshape(-1, 1, 2)+ one_point = True++ if undistort:+ new_points = np.empty(points.shape)+ for cnum, cam in enumerate(self.cameras):+ # must copy in order to satisfy opencv underneath+ sub = np.copy(points[cnum])+ new_points[cnum] = cam.undistort_points(sub)+ points = new_points++ n_cams, n_points, _ = points.shape++ out = np.empty((n_points, 3))+ out[:] = np.nan++ cam_mats = np.array([cam.get_extrinsics_mat() for cam in self.cameras])++ if progress:+ iterator = trange(n_points, ncols=70)+ else:+ iterator = range(n_points)++ for ip in iterator:+ subp = points[:, ip, :]+ good = ~np.isnan(subp[:, 0])+ if np.sum(good) >= 2:+ out[ip] = triangulate_simple(subp[good], cam_mats[good])++ if one_point:+ out = out[0]++ return out++ def triangulate_possible(self, points, undistort=True,+ min_cams=2, progress=False, threshold=0.5):+ """Given an CxNxPx2 array, this returns an Nx3 array of points+ by triangulating all possible points and picking the ones with+ best reprojection error+ where:+ C: number of cameras+ N: number of points+ P: number of possible options per point+ """++ assert points.shape[0] == len(self.cameras), \+ "Invalid points shape, first dim should be equal to" \+ " number of cameras ({}), but shape is {}".format(+ len(self.cameras), points.shape+ )++ n_cams, n_points, n_possible, _ = points.shape++ cam_nums, point_nums, possible_nums = np.where(+ ~np.isnan(points[:, :, :, 0]))++ all_iters = defaultdict(dict)++ for cam_num, point_num, possible_num in zip(cam_nums, point_nums,+ possible_nums):+ if cam_num not in all_iters[point_num]:+ all_iters[point_num][cam_num] = []+ all_iters[point_num][cam_num].append((cam_num, possible_num))++ for point_num in all_iters.keys():+ for cam_num in all_iters[point_num].keys():+ all_iters[point_num][cam_num].append(None)++ out = np.full((n_points, 3), np.nan, dtype='float64')+ picked_vals = np.zeros((n_cams, n_points, n_possible), dtype='bool')+ errors = np.zeros(n_points, dtype='float64')+ points_2d = np.full((n_cams, n_points, 2), np.nan, dtype='float64')++ if progress:+ iterator = trange(n_points, ncols=70)+ else:+ iterator = range(n_points)++ for point_ix in iterator:+ best_point = None+ best_error = 200++ n_cams_max = len(all_iters[point_ix])++ for picked in itertools.product(*all_iters[point_ix].values()):+ picked = [p for p in picked if p is not None]+ if len(picked) < min_cams and len(picked) != n_cams_max:+ continue++ cnums = [p[0] for p in picked]+ xnums = [p[1] for p in picked]++ pts = points[cnums, point_ix, xnums]+ cc = self.subset_cameras(cnums)++ p3d = cc.triangulate(pts, undistort=undistort)+ err = cc.reprojection_error(p3d, pts, mean=True)++ if err < best_error:+ best_point = {+ 'error': err,+ 'point': p3d[:3],+ 'points': pts,+ 'picked': picked,+ 'joint_ix': point_ix+ }+ best_error = err+ if best_error < threshold:+ break++ if best_point is not None:+ out[point_ix] = best_point['point']+ picked = best_point['picked']+ cnums = [p[0] for p in picked]+ xnums = [p[1] for p in picked]+ picked_vals[cnums, point_ix, xnums] = True+ errors[point_ix] = best_point['error']+ points_2d[cnums, point_ix] = best_point['points']++ return out, picked_vals, points_2d, errors++ def triangulate_ransac(self, points, undistort=True, min_cams=2, progress=False):+ """Given an CxNx2 array, this returns an Nx3 array of points,+ where N is the number of points and C is the number of cameras"""++ assert points.shape[0] == len(self.cameras), \+ "Invalid points shape, first dim should be equal to" \+ " number of cameras ({}), but shape is {}".format(+ len(self.cameras), points.shape+ )++ n_cams, n_points, _ = points.shape++ points_ransac = points.reshape(n_cams, n_points, 1, 2)++ return self.triangulate_possible(points_ransac,+ undistort=undistort,+ min_cams=min_cams,+ progress=progress)++ @jit(parallel=True, forceobj=True)+ def reprojection_error(self, p3ds, p2ds, mean=False):+ """Given an Nx3 array of 3D points and an CxNx2 array of 2D points,+ where N is the number of points and C is the number of cameras,+ this returns an CxNx2 array of errors.+ Optionally mean=True, this averages the errors and returns array of length N of errors"""++ one_point = False+ if len(p3ds.shape) == 1 and len(p2ds.shape) == 2:+ p3ds = p3ds.reshape(1, 3)+ p2ds = p2ds.reshape(-1, 1, 2)+ one_point = True++ n_cams, n_points, _ = p2ds.shape+ assert p3ds.shape == (n_points, 3), \+ "shapes of 2D and 3D points are not consistent: " \+ "2D={}, 3D={}".format(p2ds.shape, p3ds.shape)++ errors = np.empty((n_cams, n_points, 2))++ for cnum, cam in enumerate(self.cameras):+ errors[cnum] = cam.reprojection_error(p3ds, p2ds[cnum])++ if mean:+ errors_norm = np.linalg.norm(errors, axis=2)+ good = ~np.isnan(errors_norm)+ errors_norm[~good] = 0+ denom = np.sum(good, axis=0).astype('float64')+ denom[denom < 1.5] = np.nan+ errors = np.sum(errors_norm, axis=0) / denom++ if one_point:+ if mean:+ errors = float(errors[0])+ else:+ errors = errors.reshape(-1, 2)++ return errors++ def bundle_adjust_iter(self, p2ds, extra=None,+ n_iters=10, start_mu=15, end_mu=1,+ max_nfev=200, ftol=1e-4,+ n_samp_iter=100, n_samp_full=1000,+ error_threshold=0.3,+ verbose=False):+ """Given an CxNx2 array of 2D points,+ where N is the number of points and C is the number of cameras,+ this performs iterative bundle adjustsment to fine-tune the parameters of the cameras.+ That is, it performs bundle adjustment multiple times, adjusting the weights given to points+ to reduce the influence of outliers.+ This is inspired by the algorithm for Fast Global Registration by Zhou, Park, and Koltun+ """+ error_list = []++ assert p2ds.shape[0] == len(self.cameras), \+ "Invalid points shape, first dim should be equal to" \+ " number of cameras ({}), but shape is {}".format(+ len(self.cameras), p2ds.shape+ )++ p2ds_full = p2ds+ extra_full = extra++ p2ds, extra = resample_points(p2ds_full, extra_full,+ n_samp=n_samp_full)+ error = self.average_error(p2ds, median=True)++ if verbose:+ print('error: ', error)++ mus = np.exp(np.linspace(np.log(start_mu), np.log(end_mu), num=n_iters))++ if verbose:+ print('n_samples: {}'.format(n_samp_iter))++ for i in range(n_iters):+ p2ds, extra = resample_points(p2ds_full, extra_full,+ n_samp=n_samp_full)+ p3ds = self.triangulate(p2ds)+ errors_full = self.reprojection_error(p3ds, p2ds, mean=False)+ errors_norm = self.reprojection_error(p3ds, p2ds, mean=True)++ error_dict = get_error_dict(errors_full)+ max_error = 0+ min_error = 0+ for k, v in error_dict.items():+ num, percents = v+ max_error = max(percents[-1], max_error)+ min_error = max(percents[0], min_error)+ mu = max(min(max_error, mus[i]), min_error)++ good = errors_norm < mu+ extra_good = subset_extra(extra, good)+ p2ds_samp, extra_samp = resample_points(+ p2ds[:, good], extra_good, n_samp=n_samp_iter)++ error = np.median(errors_norm)+ error_list.append(error)++ if error < error_threshold:+ break++ if verbose:+ pprint(error_dict)+ print('error: {:.2f}, mu: {:.1f}, ratio: {:.3f}'.format(error, mu, np.mean(good)))+ print(f'previous error scores: {error_list}')++ self.bundle_adjust(p2ds_samp, extra_samp,+ loss='linear', ftol=ftol,+ max_nfev=max_nfev,+ verbose=verbose)++ p2ds, extra = resample_points(p2ds_full, extra_full,+ n_samp=n_samp_full)+ p3ds = self.triangulate(p2ds)+ errors_full = self.reprojection_error(p3ds, p2ds, mean=False)+ errors_norm = self.reprojection_error(p3ds, p2ds, mean=True)+ error_dict = get_error_dict(errors_full)+ if verbose:+ pprint(error_dict)++ max_error = 0+ min_error = 0+ for k, v in error_dict.items():+ num, percents = v+ max_error = max(percents[-1], max_error)+ min_error = max(percents[0], min_error)+ mu = max(max(max_error, end_mu), min_error)++ good = errors_norm < mu+ extra_good = subset_extra(extra, good)+ self.bundle_adjust(p2ds[:, good], extra_good,+ loss='linear',+ ftol=ftol, max_nfev=max(200, max_nfev),+ verbose=verbose)++ error = self.average_error(p2ds, median=True)++ p3ds = self.triangulate(p2ds)+ errors_full = self.reprojection_error(p3ds, p2ds, mean=False)+ error_dict = get_error_dict(errors_full)+ if verbose:+ pprint(error_dict)++ if verbose:+ print('error: ', error)++ return error++ def bundle_adjust(self, p2ds, extra=None,+ loss='linear',+ threshold=50,+ ftol=1e-4,+ max_nfev=1000,+ weights=None,+ start_params=None,+ verbose=True):+ """Given an CxNx2 array of 2D points,+ where N is the number of points and C is the number of cameras,+ this performs bundle adjustsment to fine-tune the parameters of the cameras"""++ assert p2ds.shape[0] == len(self.cameras), \+ "Invalid points shape, first dim should be equal to" \+ " number of cameras ({}), but shape is {}".format(+ len(self.cameras), p2ds.shape+ )++ if extra is not None:+ extra['ids_map'] = remap_ids(extra['ids'])++ x0, n_cam_params = self._initialize_params_bundle(p2ds, extra)++ if start_params is not None:+ x0 = start_params+ n_cam_params = len(self.cameras[0].get_params())++ error_fun = self._error_fun_bundle++ jac_sparse = self._jac_sparsity_bundle(p2ds, n_cam_params, extra)++ f_scale = threshold+ opt = optimize.least_squares(error_fun,+ x0,+ jac_sparsity=jac_sparse,+ f_scale=f_scale,+ x_scale='jac',+ loss=loss,+ ftol=ftol,+ method='trf',+ tr_solver='lsmr',+ verbose=2 * verbose,+ max_nfev=max_nfev,+ args=(p2ds, n_cam_params, extra))+ best_params = opt.x++ for i, cam in enumerate(self.cameras):+ a = i * n_cam_params+ b = (i + 1) * n_cam_params+ cam.set_params(best_params[a:b])++ error = self.average_error(p2ds)+ return error++ @jit(parallel=True, forceobj=True)+ def _error_fun_bundle(self, params, p2ds, n_cam_params, extra):+ """Error function for bundle adjustment"""+ good = ~np.isnan(p2ds)+ n_cams = len(self.cameras)++ for i in range(n_cams):+ cam = self.cameras[i]+ a = i * n_cam_params+ b = (i + 1) * n_cam_params+ cam.set_params(params[a:b])++ n_cams = len(self.cameras)+ sub = n_cam_params * n_cams+ n3d = p2ds.shape[1] * 3+ p3ds_test = params[sub:sub + n3d].reshape(-1, 3)+ errors = self.reprojection_error(p3ds_test, p2ds)+ errors_reproj = errors[good]++ if extra is not None:+ ids = extra['ids_map']+ objp = extra['objp']+ min_scale = np.min(objp[objp > 0])+ n_boards = int(np.max(ids)) + 1+ a = sub + n3d+ rvecs = params[a:a + n_boards * 3].reshape(-1, 3)+ tvecs = params[a + n_boards * 3:a + n_boards * 6].reshape(-1, 3)+ expected = transform_points(objp, rvecs[ids], tvecs[ids])+ errors_obj = 2 * (p3ds_test - expected).ravel() / min_scale+ else:+ errors_obj = np.array([])++ return np.hstack([errors_reproj, errors_obj])++ def _jac_sparsity_bundle(self, p2ds, n_cam_params, extra):+ """Given an CxNx2 array of 2D points,+ where N is the number of points and C is the number of cameras,+ compute the sparsity structure of the jacobian for bundle adjustment"""++ point_indices = np.zeros(p2ds.shape, dtype='int32')+ cam_indices = np.zeros(p2ds.shape, dtype='int32')++ for i in range(p2ds.shape[1]):+ point_indices[:, i] = i++ for j in range(p2ds.shape[0]):+ cam_indices[j] = j++ good = ~np.isnan(p2ds)++ if extra is not None:+ ids = extra['ids_map']+ n_boards = int(np.max(ids)) + 1+ total_board_params = n_boards * (3 + 3) # rvecs + tvecs+ else:+ n_boards = 0+ total_board_params = 0++ n_cams = p2ds.shape[0]+ n_points = p2ds.shape[1]+ total_params_reproj = n_cams * n_cam_params + n_points * 3+ n_params = total_params_reproj + total_board_params++ n_good_values = np.sum(good)+ if extra is not None:+ n_errors = n_good_values + n_points * 3+ else:+ n_errors = n_good_values++ A_sparse = dok_matrix((n_errors, n_params), dtype='int16')++ cam_indices_good = cam_indices[good]+ point_indices_good = point_indices[good]++ # -- reprojection error --+ ix = np.arange(n_good_values)++ ## update camera params based on point error+ for i in range(n_cam_params):+ A_sparse[ix, cam_indices_good * n_cam_params + i] = 1++ ## update point position based on point error+ for i in range(3):+ A_sparse[ix, n_cams * n_cam_params + point_indices_good * 3 + i] = 1++ # -- match for the object points--+ if extra is not None:+ point_ix = np.arange(n_points)++ ## update all the camera parameters+ # A_sparse[n_good_values:n_good_values+n_points*3,+ # 0:n_cams*n_cam_params] = 1++ ## update board rotation and translation based on error from expected+ for i in range(3):+ for j in range(3):+ A_sparse[n_good_values + point_ix * 3 + i,+ total_params_reproj + ids * 3 + j] = 1+ A_sparse[n_good_values + point_ix * 3 + i,+ total_params_reproj + n_boards * 3 + ids * 3 + j] = 1++ ## update point position based on error from expected+ for i in range(3):+ A_sparse[n_good_values + point_ix * 3 + i,+ n_cams * n_cam_params + point_ix * 3 + i] = 1++ return A_sparse++ def _initialize_params_bundle(self, p2ds, extra):+ """Given an CxNx2 array of 2D points,+ where N is the number of points and C is the number of cameras,+ initializes the parameters for bundle adjustment"""++ cam_params = np.hstack([cam.get_params() for cam in self.cameras])+ n_cam_params = len(cam_params) // len(self.cameras)++ total_cam_params = len(cam_params)++ n_cams, n_points, _ = p2ds.shape+ assert n_cams == len(self.cameras), \+ "number of cameras in CameraGroup does not " \+ "match number of cameras in 2D points given"++ p3ds = self.triangulate(p2ds)++ if extra is not None:+ ids = extra['ids_map']+ n_boards = int(np.max(ids[~np.isnan(ids)])) + 1+ total_board_params = n_boards * (3 + 3) # rvecs + tvecs++ # initialize to 0+ rvecs = np.zeros((n_boards, 3), dtype='float64')+ tvecs = np.zeros((n_boards, 3), dtype='float64')++ if 'rvecs' in extra and 'tvecs' in extra:+ rvecs_all = extra['rvecs']+ tvecs_all = extra['tvecs']+ for board_num in range(n_boards):+ point_id = np.where(ids == board_num)[0][0]+ cam_ids_possible = np.where(~np.isnan(p2ds[:, point_id, 0]))[0]+ cam_id = np.random.choice(cam_ids_possible)+ M_cam = self.cameras[cam_id].get_extrinsics_mat()+ M_board_cam = make_M(rvecs_all[cam_id, point_id],+ tvecs_all[cam_id, point_id])+ M_board = np.matmul(np.linalg.inv(M_cam), M_board_cam)+ rvec, tvec = get_rtvec(M_board)+ rvecs[board_num] = rvec+ tvecs[board_num] = tvec+++ else:+ total_board_params = 0++ x0 = np.zeros(total_cam_params + p3ds.size + total_board_params)+ x0[:total_cam_params] = cam_params+ x0[total_cam_params:total_cam_params + p3ds.size] = p3ds.ravel()++ if extra is not None:+ start_board = total_cam_params + p3ds.size+ x0[start_board:start_board + n_boards * 3] = rvecs.ravel()+ x0[start_board + n_boards * 3:start_board + n_boards * 6] = \+ tvecs.ravel()++ return x0, n_cam_params++ def optim_points(self, points, p3ds,+ constraints=[],+ constraints_weak=[],+ scale_smooth=4,+ scale_length=2, scale_length_weak=0.5,+ reproj_error_threshold=15, reproj_loss='soft_l1',+ n_deriv_smooth=1, scores=None, verbose=False):+ """+ Take in an array of 2D points of shape CxNxJx2,+ an array of 3D points of shape NxJx3,+ and an array of constraints of shape Kx2, where+ C: number of camera+ N: number of frames+ J: number of joints+ K: number of constraints++ This function creates an optimized array of 3D points of shape NxJx3.++ Example constraints:+ constraints = [[0, 1], [1, 2], [2, 3]]+ (meaning that lengths of segments 0->1, 1->2, 2->3 are all constant)++ """+ assert points.shape[0] == len(self.cameras), \+ "Invalid points shape, first dim should be equal to" \+ " number of cameras ({}), but shape is {}".format(+ len(self.cameras), points.shape+ )++ n_cams, n_frames, n_joints, _ = points.shape+ constraints = np.array(constraints)+ constraints_weak = np.array(constraints_weak)++ p3ds_intp = np.apply_along_axis(interpolate_data, 0, p3ds)++ p3ds_med = np.apply_along_axis(medfilt_data, 0, p3ds_intp, size=7)++ default_smooth = 1.0 / np.mean(np.abs(np.diff(p3ds_med, axis=0)))+ scale_smooth_full = scale_smooth * default_smooth++ t1 = time.time()++ x0 = self._initialize_params_triangulation(+ p3ds_intp, constraints, constraints_weak)++ x0[~np.isfinite(x0)] = 0++ jac = self._jac_sparsity_triangulation(+ points, constraints, constraints_weak, n_deriv_smooth)++ opt2 = optimize.least_squares(self._error_fun_triangulation,+ x0=x0, jac_sparsity=jac,+ loss='linear',+ ftol=1e-3,+ verbose=2 * verbose,+ args=(points,+ constraints,+ constraints_weak,+ scores,+ scale_smooth_full,+ scale_length,+ scale_length_weak,+ reproj_error_threshold,+ reproj_loss,+ n_deriv_smooth))++ p3ds_new2 = opt2.x[:p3ds.size].reshape(p3ds.shape)++ t2 = time.time()++ if verbose:+ print('optimization took {:.2f} seconds'.format(t2 - t1))++ return p3ds_new2++ def optim_points_possible(self, points, p3ds,+ constraints=[],+ constraints_weak=[],+ scale_smooth=4,+ scale_length=2, scale_length_weak=0.5,+ reproj_error_threshold=15, reproj_loss='soft_l1',+ n_deriv_smooth=1, scores=None, verbose=False):+ """+ Take in an array of 2D points of shape CxNxJxPx2,+ an array of 3D points of shape NxJx3,+ and an array of constraints of shape Kx2, where+ C: number of camera+ N: number of frames+ J: number of joints+ P: number of possible options per point+ K: number of constraints++ This function creates an optimized array of 3D points of shape NxJx3.++ Example constraints:+ constraints = [[0, 1], [1, 2], [2, 3]]+ (meaning that lengths of segments 0->1, 1->2, 2->3 are all constant)++ """+ assert points.shape[0] == len(self.cameras), \+ "Invalid points shape, first dim should be equal to" \+ " number of cameras ({}), but shape is {}".format(+ len(self.cameras), points.shape+ )++ n_cams, n_frames, n_joints, n_possible, _ = points.shape+ constraints = np.array(constraints)+ constraints_weak = np.array(constraints_weak)++ p3ds_intp = np.apply_along_axis(interpolate_data, 0, p3ds)++ p3ds_med = np.apply_along_axis(medfilt_data, 0, p3ds_intp, size=7)++ default_smooth = 1.0 / np.mean(np.abs(np.diff(p3ds_med, axis=0)))+ scale_smooth_full = scale_smooth * default_smooth++ t1 = time.time()++ x0 = self._initialize_params_triangulation_possible(+ p3ds_intp, points, constraints=constraints, constraints_weak=constraints_weak)++ print('getting jacobian...')+ jac = self._jac_sparsity_triangulation_possible(+ points,+ constraints=constraints,+ constraints_weak=constraints_weak,+ n_deriv_smooth=n_deriv_smooth)++ beta = 5++ print('starting optimization...')+ opt2 = optimize.least_squares(self._error_fun_triangulation_possible,+ x0=x0, jac_sparsity=jac,+ loss='linear',+ ftol=1e-3,+ verbose=2 * verbose,+ args=(points,+ beta,+ constraints,+ constraints_weak,+ scores,+ scale_smooth_full,+ scale_length,+ scale_length_weak,+ reproj_error_threshold,+ reproj_loss,+ n_deriv_smooth))+ params = opt2.x++ p3ds_new2 = params[:p3ds.size].reshape(p3ds.shape)++ bad = np.isnan(points[:, :, :, :, 0])+ all_bad = np.all(bad, axis=3)++ n_params_norm = p3ds.size + len(constraints) + len(constraints_weak)++ alphas = np.zeros((n_cams, n_frames, n_joints, n_possible), dtype='float64')+ alphas[~bad] = params[n_params_norm:]++ alphas_exp = np.exp(beta * alphas)+ alphas_exp[bad] = 0+ alphas_sum = np.sum(alphas_exp, axis=3)+ alphas_sum[all_bad] = 1+ alphas_norm = alphas_exp / alphas_sum[:, :, :, None]+ alphas_norm[bad] = np.nan++ t2 = time.time()++ if verbose:+ print('optimization took {:.2f} seconds'.format(t2 - t1))++ return p3ds_new2, alphas_norm++ def triangulate_optim(self, points, init_ransac=False, init_progress=False,+ **kwargs):+ """+ Take in an array of 2D points of shape CxNxJx2, and an array of constraints of shape Kx2, where+ C: number of camera+ N: number of frames+ J: number of joints+ K: number of constraints++ This function creates an optimized array of 3D points of shape NxJx3.++ Example constraints:+ constraints = [[0, 1], [1, 2], [2, 3]]+ (meaning that lengths of segments 0->1, 1->2, 2->3 are all constant)++ """++ assert points.shape[0] == len(self.cameras), \+ "Invalid points shape, first dim should be equal to" \+ " number of cameras ({}), but shape is {}".format(+ len(self.cameras), points.shape+ )++ n_cams, n_frames, n_joints, _ = points.shape+ # constraints = np.array(constraints)+ # constraints_weak = np.array(constraints_weak)++ points_shaped = points.reshape(n_cams, n_frames * n_joints, 2)+ if init_ransac:+ p3ds, picked, p2ds, errors = self.triangulate_ransac(points_shaped, progress=init_progress)+ points = p2ds.reshape(points.shape)+ else:+ p3ds = self.triangulate(points_shaped, progress=init_progress)+ p3ds = p3ds.reshape((n_frames, n_joints, 3))++ c = np.isfinite(p3ds[:, :, 0])+ if np.sum(c) < 20:+ print("warning: not enough 3D points to run optimization")+ return p3ds++ return self.optim_points(points, p3ds, **kwargs)++ @jit(forceobj=True, parallel=True)+ def _error_fun_triangulation(self, params, p2ds,+ constraints=[],+ constraints_weak=[],+ scores=None,+ scale_smooth=10000,+ scale_length=1,+ scale_length_weak=0.2,+ reproj_error_threshold=100,+ reproj_loss='soft_l1',+ n_deriv_smooth=1):+ n_cams, n_frames, n_joints, _ = p2ds.shape++ n_3d = n_frames * n_joints * 3+ n_constraints = len(constraints)+ n_constraints_weak = len(constraints_weak)++ # load params+ p3ds = params[:n_3d].reshape((n_frames, n_joints, 3))+ joint_lengths = np.array(params[n_3d:n_3d + n_constraints])+ joint_lengths_weak = np.array(params[n_3d + n_constraints:])++ # reprojection errors+ p3ds_flat = p3ds.reshape(-1, 3)+ p2ds_flat = p2ds.reshape((n_cams, -1, 2))+ errors = self.reprojection_error(p3ds_flat, p2ds_flat)+ if scores is not None:+ scores_flat = scores.reshape((n_cams, -1))+ errors = errors * scores_flat[:, :, None]+ errors_reproj = errors[~np.isnan(p2ds_flat)]++ rp = reproj_error_threshold+ errors_reproj = np.abs(errors_reproj)+ if reproj_loss == 'huber':+ bad = errors_reproj > rp+ errors_reproj[bad] = rp * (2 * np.sqrt(errors_reproj[bad] / rp) - 1)+ elif reproj_loss == 'linear':+ pass+ elif reproj_loss == 'soft_l1':+ errors_reproj = rp * 2 * (np.sqrt(1 + errors_reproj / rp) - 1)++ # temporal constraint+ errors_smooth = np.diff(p3ds, n=n_deriv_smooth, axis=0).ravel() * scale_smooth++ # joint length constraint+ errors_lengths = np.empty((n_constraints, n_frames), dtype='float64')+ for cix, (a, b) in enumerate(constraints):+ lengths = np.linalg.norm(p3ds[:, a] - p3ds[:, b], axis=1)+ expected = joint_lengths[cix]+ errors_lengths[cix] = 100 * (lengths - expected) / expected+ errors_lengths = errors_lengths.ravel() * scale_length++ errors_lengths_weak = np.empty((n_constraints_weak, n_frames), dtype='float64')+ for cix, (a, b) in enumerate(constraints_weak):+ lengths = np.linalg.norm(p3ds[:, a] - p3ds[:, b], axis=1)+ expected = joint_lengths_weak[cix]+ errors_lengths_weak[cix] = 100 * (lengths - expected) / expected+ errors_lengths_weak = errors_lengths_weak.ravel() * scale_length_weak++ return np.hstack([errors_reproj, errors_smooth,+ errors_lengths, errors_lengths_weak])++ def _error_fun_triangulation_possible(self, params, p2ds,+ beta=2,+ constraints=[],+ constraints_weak=[],+ *args):+ # extract alphas from end of params+ # soft argmax for picking the appropriate points from p2ds+ # pass the points to error_fun_triangulate_possible for residuals+ # add errors to keep the alphas in check+ # return all the errors++ n_cams, n_frames, n_joints, n_possible, _ = p2ds.shape++ n_3d = n_frames * n_joints * 3+ n_constraints = len(constraints)+ n_constraints_weak = len(constraints_weak)+ n_params_norm = n_3d + n_constraints + n_constraints_weak++ # load params+ bad = np.isnan(p2ds[:, :, :, :, 0])+ all_bad = np.all(bad, axis=3)++ alphas = np.zeros((n_cams, n_frames, n_joints, n_possible), dtype='float64')+ alphas[~bad] = params[n_params_norm:]+ params_rest = np.array(params[:n_params_norm])++ # get normalized alphas+ alphas_exp = np.exp(beta * alphas)+ alphas_exp[bad] = 0+ alphas_sum = np.sum(alphas_exp, axis=3)+ alphas_sum[all_bad] = 1+ alphas_norm = alphas_exp / alphas_sum[:, :, :, None]++ # extract the 2D points using soft argmax+ p2ds_test = np.copy(p2ds)+ p2ds_test[bad] = 0+ p2ds_adj = np.sum(alphas_norm[:, :, :, :, None] * p2ds_test, axis=3)+ p2ds_adj[all_bad] = np.nan++ errors = self._error_fun_triangulation(params_rest, p2ds_adj,+ constraints, constraints_weak, *args)++ alphas_test = alphas_norm[~all_bad]+ errors_alphas = (1 - np.std(alphas_test, axis=1)) * 10++ return np.hstack([errors, errors_alphas])++ def _initialize_params_triangulation(self, p3ds,+ constraints=[],+ constraints_weak=[]):+ joint_lengths = np.empty(len(constraints), dtype='float64')+ joint_lengths_weak = np.empty(len(constraints_weak), dtype='float64')++ for cix, (a, b) in enumerate(constraints):+ lengths = np.linalg.norm(p3ds[:, a] - p3ds[:, b], axis=1)+ joint_lengths[cix] = np.median(lengths)++ for cix, (a, b) in enumerate(constraints_weak):+ lengths = np.linalg.norm(p3ds[:, a] - p3ds[:, b], axis=1)+ joint_lengths_weak[cix] = np.median(lengths)++ all_lengths = np.hstack([joint_lengths, joint_lengths_weak])+ med = np.median(all_lengths)+ if med == 0:+ med = 1e-3++ mad = np.median(np.abs(all_lengths - med))++ joint_lengths[joint_lengths == 0] = med+ joint_lengths_weak[joint_lengths_weak == 0] = med+ joint_lengths[joint_lengths > med + mad * 5] = med+ joint_lengths_weak[joint_lengths_weak > med + mad * 5] = med++ return np.hstack([p3ds.ravel(), joint_lengths, joint_lengths_weak])++ def _initialize_params_triangulation_possible(self, p3ds, p2ds, **kwargs):+ # initialize params using above function+ # initialize alphas to 1 for first one and 0 for other possible++ n_cams, n_frames, n_joints, n_possible, _ = p2ds.shape+ good = ~np.isnan(p2ds[:, :, :, :, 0])++ alphas = np.zeros((n_cams, n_frames, n_joints, n_possible), dtype='float64')+ alphas[:, :, :, 0] = 0++ params = self._initialize_params_triangulation(p3ds, **kwargs)+ params_full = np.hstack([params, alphas[good]])++ return params_full++ def _jac_sparsity_triangulation(self, p2ds,+ constraints=[],+ constraints_weak=[],+ n_deriv_smooth=1):+ n_cams, n_frames, n_joints, _ = p2ds.shape+ n_constraints = len(constraints)+ n_constraints_weak = len(constraints_weak)++ p2ds_flat = p2ds.reshape((n_cams, -1, 2))++ point_indices = np.zeros(p2ds_flat.shape, dtype='int32')+ for i in range(p2ds_flat.shape[1]):+ point_indices[:, i] = i++ point_indices_3d = np.arange(n_frames * n_joints) \+ .reshape((n_frames, n_joints))++ good = ~np.isnan(p2ds_flat)+ n_errors_reproj = np.sum(good)+ n_errors_smooth = (n_frames - n_deriv_smooth) * n_joints * 3+ n_errors_lengths = n_constraints * n_frames+ n_errors_lengths_weak = n_constraints_weak * n_frames++ n_errors = n_errors_reproj + n_errors_smooth + \+ n_errors_lengths + n_errors_lengths_weak++ n_3d = n_frames * n_joints * 3+ n_params = n_3d + n_constraints + n_constraints_weak++ point_indices_good = point_indices[good]++ A_sparse = dok_matrix((n_errors, n_params), dtype='int16')++ # constraints for reprojection errors+ ix_reproj = np.arange(n_errors_reproj)+ for k in range(3):+ A_sparse[ix_reproj, point_indices_good * 3 + k] = 1++ # sparse constraints for smoothness in time+ frames = np.arange(n_frames - n_deriv_smooth)+ for j in range(n_joints):+ for n in range(n_deriv_smooth + 1):+ pa = point_indices_3d[frames, j]+ pb = point_indices_3d[frames + n, j]+ for k in range(3):+ A_sparse[n_errors_reproj + pa * 3 + k, pb * 3 + k] = 1++ ## -- strong constraints --+ # joint lengths should change with joint lengths errors+ start = n_errors_reproj + n_errors_smooth+ frames = np.arange(n_frames)+ for cix, (a, b) in enumerate(constraints):+ A_sparse[start + cix * n_frames + frames, n_3d + cix] = 1++ # points should change accordingly to match joint lengths too+ frames = np.arange(n_frames)+ for cix, (a, b) in enumerate(constraints):+ pa = point_indices_3d[frames, a]+ pb = point_indices_3d[frames, b]+ for k in range(3):+ A_sparse[start + cix * n_frames + frames, pa * 3 + k] = 1+ A_sparse[start + cix * n_frames + frames, pb * 3 + k] = 1++ ## -- weak constraints --+ # joint lengths should change with joint lengths errors+ start = n_errors_reproj + n_errors_smooth + n_errors_lengths+ frames = np.arange(n_frames)+ for cix, (a, b) in enumerate(constraints_weak):+ A_sparse[start + cix * n_frames + frames, n_3d + n_constraints + cix] = 1++ # points should change accordingly to match joint lengths too+ frames = np.arange(n_frames)+ for cix, (a, b) in enumerate(constraints_weak):+ pa = point_indices_3d[frames, a]+ pb = point_indices_3d[frames, b]+ for k in range(3):+ A_sparse[start + cix * n_frames + frames, pa * 3 + k] = 1+ A_sparse[start + cix * n_frames + frames, pb * 3 + k] = 1++ return A_sparse++ def _jac_sparsity_triangulation_possible(self, p2ds_full, **kwargs):+ # initialize sparse jacobian using above function+ # extend to include alphas from parameters+ ## TODO: this initialization is really slow for some reason++ n_cams, n_frames, n_joints, n_possible, _ = p2ds_full.shape+ good_full = ~np.isnan(p2ds_full[:, :, :, :, 0])+ any_good = np.any(good_full, axis=3)++ n_alphas = np.sum(good_full)+ n_errors_alphas = np.sum(any_good)++ p2ds = p2ds_full[:, :, :, 0]+ A_sparse = self._jac_sparsity_triangulation(p2ds, **kwargs)++ n_errors, n_params = A_sparse.shape++ B_sparse = dok_matrix((n_errors + n_errors_alphas, n_params + n_alphas), dtype='int16')+ for r, c in zip(*A_sparse.nonzero()):+ B_sparse[r, c] = A_sparse[r, c]++ point_indices_2d = np.arange(n_cams * n_frames * n_joints) \+ .reshape(n_cams, n_frames, n_joints)+ point_indices_2d_rep = np.repeat(point_indices_2d[:, :, :, None], 2, axis=3)+ point_indices_2d_good = point_indices_2d_rep[~np.isnan(p2ds)]+ point_indices_good = point_indices_2d[any_good]++ alpha_indices = np.zeros((n_cams, n_frames, n_joints, n_possible), dtype='int64')+ for pnum in range(n_possible):+ alpha_indices[:, :, :, pnum] = point_indices_2d++ alpha_indices_good = alpha_indices[good_full]++ # alphas should change according to the reprojection error for each corresponding point+ point_indices_2d_good_find = defaultdict(list)+ for ix, p in enumerate(point_indices_2d_good):+ point_indices_2d_good_find[p].append(ix)++ for ix, alpha_index in enumerate(alpha_indices_good):+ B_sparse[point_indices_2d_good_find[alpha_index],+ n_params + ix] = 1++ # alphas should change according to the alpha errors+ point_indices_good_find = dict()+ for ix, p in enumerate(point_indices_good):+ point_indices_good_find[p] = ix++ for ix, alpha_index in enumerate(alpha_indices_good):+ if alpha_index in point_indices_good_find:+ err_ix = n_errors + point_indices_good_find[alpha_index]+ B_sparse[err_ix, n_params + ix] = 1++ return B_sparse++ def copy(self):+ cameras = [cam.copy() for cam in self.cameras]+ metadata = copy(self.metadata)+ return CameraGroup(cameras, metadata)++ def set_rotations(self, rvecs):+ for cam, rvec in zip(self.cameras, rvecs):+ cam.set_rotation(rvec)++ def set_translations(self, tvecs):+ for cam, tvec in zip(self.cameras, tvecs):+ cam.set_translation(tvec)++ def get_rotations(self):+ rvecs = []+ for cam in self.cameras:+ rvec = cam.get_rotation()+ rvecs.append(rvec)+ return np.array(rvecs)++ def get_translations(self):+ tvecs = []+ for cam in self.cameras:+ tvec = cam.get_translation()+ tvecs.append(tvec)+ return np.array(tvecs)++ def get_names(self):+ return [cam.get_name() for cam in self.cameras]++ def set_names(self, names):+ for cam, name in zip(self.cameras, names):+ cam.set_name(name)++ def average_error(self, p2ds, median=False):+ p3ds = self.triangulate(p2ds)+ errors = self.reprojection_error(p3ds, p2ds, mean=True)+ if median:+ return np.median(errors)+ else:+ return np.mean(errors)++ def calibrate_rows(self, all_rows, board,+ init_intrinsics=True, init_extrinsics=True, verbose=True,+ **kwargs):+ assert len(all_rows) == len(self.cameras), \+ "Number of camera detections does not match number of cameras"++ for rows, camera in zip(all_rows, self.cameras):+ size = camera.get_size()++ assert size is not None, \+ "Camera with name {} has no specified frame size".format(camera.get_name())++ if init_intrinsics:+ objp, imgp = board.get_all_calibration_points(rows)+ mixed = [(o, i) for (o, i) in zip(objp, imgp) if len(o) >= 7]+ assert len(objp) != 0 and len(imgp) != 0, "No Charuco board points detected"+ objp, imgp = zip(*mixed)+ matrix = cv2.initCameraMatrix2D(objp, imgp, tuple(size))+ camera.set_camera_matrix(matrix)++ for i, (row, cam) in enumerate(zip(all_rows, self.cameras)):+ all_rows[i] = board.estimate_pose_rows(cam, row)++ charuco_frames = [f['framenum'][1] for f in all_rows[0]]+ merged = merge_rows(all_rows)+ imgp, extra = extract_points(merged, board, min_cameras=2)++ if init_extrinsics:+ rtvecs = extract_rtvecs(merged)+ if verbose:+ pprint(get_connections(rtvecs, self.get_names()))+ rvecs, tvecs = get_initial_extrinsics(rtvecs)+ self.set_rotations(rvecs)+ self.set_translations(tvecs)++ error = self.bundle_adjust_iter(imgp, extra, verbose=verbose, error_threshold=1)++ return error, merged, charuco_frames++ def get_rows_videos(self, videos, board, verbose=True):+ all_rows = []++ for cix, (cam, cam_videos) in enumerate(zip(self.cameras, videos)):+ rows_cam = []+ for vnum, vidname in enumerate(cam_videos):+ if verbose: print(vidname)+ rows = board.detect_video(vidname, prefix=vnum, progress=verbose)+ if verbose: print("{} boards detected".format(len(rows)))+ rows_cam.extend(rows)+ all_rows.append(rows_cam)++ return all_rows++ def set_camera_sizes_videos(self, videos):+ for cix, (cam, cam_videos) in enumerate(zip(self.cameras, videos)):+ rows_cam = []+ for vnum, vidname in enumerate(cam_videos):+ params = get_video_params(vidname)+ size = (params['width'], params['height'])+ cam.set_size(size)++ def calibrate_videos(self, videos, board,+ init_intrinsics=True, init_extrinsics=True, verbose=True,+ **kwargs):+ """Takes as input a list of list of video filenames, one list of each camera.+ Also takes a board which specifies what should be detected in the videos"""++ all_rows = self.get_rows_videos(videos, board, verbose=verbose)+ if init_extrinsics:+ self.set_camera_sizes_videos(videos)++ error, merged, charuco_frames = self.calibrate_rows(all_rows, board,+ init_intrinsics=init_intrinsics,+ init_extrinsics=init_extrinsics,+ verbose=verbose, **kwargs)+ return error, merged, charuco_frames++ def get_dicts(self):+ out = []+ for cam in self.cameras:+ out.append(cam.get_dict())+ return out++ def from_dicts(arr):+ cameras = []+ for d in arr:+ if 'fisheye' in d and d['fisheye']:+ cam = FisheyeCamera.from_dict(d)+ else:+ cam = Camera.from_dict(d)+ cameras.append(cam)+ return CameraGroup(cameras)++ @staticmethod+ def from_names(names, fisheye=False):+ cameras = []+ for name in names:+ if fisheye:+ cam = FisheyeCamera(name=name)+ else:+ cam = Camera(name=name)+ cameras.append(cam)+ return CameraGroup(cameras)++ def load_dicts(self, arr):+ for cam, d in zip(self.cameras, arr):+ cam.load_dict(d)++ def dump(self, fname):+ dicts = self.get_dicts()+ names = ['cam_{}'.format(i) for i in range(len(dicts))]+ master_dict = dict(zip(names, dicts))+ master_dict['metadata'] = self.metadata+ with open(fname, 'w') as f:+ toml.dump(master_dict, f)++ def load(fname):+ master_dict = toml.load(fname)+ keys = sorted(master_dict.keys())+ items = [master_dict[k] for k in keys if k != 'metadata']+ cgroup = CameraGroup.from_dicts(items)+ if 'metadata' in master_dict:+ cgroup.metadata = master_dict['metadata']+ return cgroup++ def resize_cameras(self, scale):+ for cam in self.cameras:+ cam.resize_camera(scale)diff --git a/src/pipelines/calibration_pipeline/calibration_pipeline_orchestrator.py b/src/pipelines/calibration_pipeline/calibration_pipeline_orchestrator.pynew file mode 100644index 00000000..f2262466--- /dev/null+++ b/src/pipelines/calibration_pipeline/calibration_pipeline_orchestrator.py@@ -0,0 +1,154 @@+import time+import logging+import traceback+from pathlib import Path+from typing import Union++import cv2++from src.cameras.multicam_manager.cv_camera_manager import OpenCVCameraManager+from src.config.home_dir import create_session_id, get_session_path+from src.core_processor.fps.timestamp_manager import TimestampManager+from src.core_processor.show_cam_window import show_cam_window+from src.core_processor.utils.image_fps_writer import write_fps_to_image+from src.pipelines.calibration_pipeline.anipose_camera_calibration.anipose_camera_calibration import \+ AniposeCameraCalibrator+from src.pipelines.calibration_pipeline.charuco_board_detection.charuco_board_detector import CharucoBoardDetector+from src.qt_visualizer_and_gui.qt_visualizer_and_gui import QTVisualizerAndGui++logger = logging.getLogger(__name__)+++class CalibrationPipelineOrchestrator:++ def __init__(self, session_id):+ self._session_id = session_id+ self._calibration_start_time_unix_ns = time.time_ns()+ self._charuco_board_detector = CharucoBoardDetector()+ self._visualizer_gui = QTVisualizerAndGui(source='calibration')+ self._open_cv_camera_manager = OpenCVCameraManager(session_id=self._session_id)++ @property+ def session_id(self):+ return self._session_id++ @property+ def session_folder_path(self):+ return get_session_path(self._session_id)++ def record_videos(+ self,+ show_visualizer_gui=True,+ show_camera_views_in_windows=True,+ save_video_in_frame_loop=False,++ ):+ """open all cameras and start recording, detect charuco boards until the user closes the windows, then send recorded videos to `aniposelib` for processing+ produces `[session_id]_calibration.toml` file that can be used to estiamte 3d trajectorires from synchronized images"""+ with self._open_cv_camera_manager.start_capture_session_all_cams() as connected_cameras_dict:+ try:+ if show_visualizer_gui:+ self._visualizer_gui.setup_and_launch(self._open_cv_camera_manager.available_webcam_ids)++ timestamp_manager = TimestampManager(self._open_cv_camera_manager.available_webcam_ids,+ self._calibration_start_time_unix_ns)++ should_continue = True+ while should_continue:++ timestamp_manager.increment_main_loop_timestamp_logger(time.perf_counter_ns())++ for this_webcam_id, this_open_cv_camera in connected_cameras_dict.items():++ if not this_open_cv_camera.new_frame_ready:+ continue++ this_cam_latest_frame = this_open_cv_camera.latest_frame++ if this_cam_latest_frame is None:+ continue++ # log timestamp+ this_cam_this_frame_timestamp_ns = this_cam_latest_frame.timestamp+ this_cam_this_frame_number = this_cam_latest_frame.frame_number+ timestamp_manager.increment_frame_processed_for_webcam(this_webcam_id,+ this_cam_this_frame_timestamp_ns,+ this_cam_this_frame_number)++ # save frame to video file+ if save_video_in_frame_loop:+ # save this frame straight to the video file (no risk of memory overflow, but can't handle higher numbers of cameras)+ this_open_cv_camera.video_recorder.save_frame_to_video_file(this_cam_latest_frame)+ else:+ # can handle large numbers of cameras, but will eventually fill up RAM and cause a crash+ this_open_cv_camera.video_recorder.append_frame_to_list(this_cam_latest_frame)++ # detect charuco board+ this_charuco_frame_payload = self._charuco_board_detector.detect_charuco_board(+ this_cam_latest_frame)++ if show_camera_views_in_windows:+ should_continue = show_cam_window(+ this_webcam_id, this_charuco_frame_payload.annotated_image, timestamp_manager+ )++ if show_visualizer_gui:+ write_fps_to_image(+ this_charuco_frame_payload.annotated_image,+ timestamp_manager.median_frames_per_second_for_webcam(this_webcam_id),+ )+ self._visualizer_gui.update_camera_view_image(this_webcam_id,+ this_charuco_frame_payload.annotated_image)++ # exit loop when user presses ESC key+ exit_key = cv2.waitKey(1)+ if exit_key == 27:+ logger.info("ESC has been pressed.")+ should_continue = False++ except:+ logger.error("Printing traceback")+ traceback.print_exc()+ finally:+ if show_camera_views_in_windows:+ # logger.info(f"Destroy window {this_open_cv_camera.webcam_id_as_str}")+ # cv2.destroyWindow(this_open_cv_camera.webcam_id_as_str)+ cv2.destroyAllWindows()+ for this_open_cv_camera in connected_cameras_dict.values():+ if not save_video_in_frame_loop:+ this_open_cv_camera.video_recorder.save_frame_list_to_disk()+ this_open_cv_camera.video_recorder.close()++ if show_visualizer_gui:+ self._visualizer_gui.close()++ def run_anipose_camera_calibration(self,+ charuco_square_size: Union[int, float] = 1,+ ):+ anipose_camera_calibrator = AniposeCameraCalibrator(self.session_id,+ self._charuco_board_detector.charuco_board_object,+ charuco_square_size=charuco_square_size)+ anipose_camera_calibrator.calibrate_camera_capture_volume()+++if __name__ == "__main__":+ print('running `calibration_pipeline` as a `__main__` file')++ record_new = True+ if record_new:+ session_id = create_session_id('calibration')+ session_path = Path(get_session_path(session_id))+ logger.info(f'Creating `calibration only` session folder at: {str(session_path)}')+ session_path.mkdir(parents=True, exist_ok=False)++ calibration_orchestrator = CalibrationPipelineOrchestrator(session_id)+ calibration_orchestrator.record_videos(show_visualizer_gui=False,+ save_video_in_frame_loop=False,+ show_camera_views_in_windows=True,+ )++ else:+ session_id = "session_06-05-2022-10_31_34_calibration"+ calibration_orchestrator = CalibrationPipelineOrchestrator(session_id)++ calibration_orchestrator.run_anipose_camera_calibration(charuco_square_size=39)diff --git a/src/pipelines/calibration_pipeline/charuco_board_detection/__init__.py b/src/pipelines/calibration_pipeline/charuco_board_detection/__init__.pynew file mode 100644index 00000000..e69de29bdiff --git a/src/core_processor/camera_calibration/charuco_board_detection/charuco_board_detector.py b/src/pipelines/calibration_pipeline/charuco_board_detection/charuco_board_detector.pysimilarity index 84%rename from src/core_processor/camera_calibration/charuco_board_detection/charuco_board_detector.pyrename to src/pipelines/calibration_pipeline/charuco_board_detection/charuco_board_detector.pyindex d710b270..944e1e00 100644--- a/src/core_processor/camera_calibration/charuco_board_detection/charuco_board_detector.py+++ b/src/pipelines/calibration_pipeline/charuco_board_detection/charuco_board_detector.py@@ -3,12 +3,12 @@ import loggingimport cv2from src.cameras.capture.dataclasses.frame_payload import FramePayload-from src.core_processor.camera_calibration.charuco_board_detection.dataclasses.charuco_board_definition import CharucoBoard-from src.core_processor.camera_calibration.charuco_board_detection.dataclasses.charuco_frame_payload import \+from src.pipelines.calibration_pipeline.charuco_board_detection.dataclasses.charuco_board_definition import CharucoBoard+from src.pipelines.calibration_pipeline.charuco_board_detection.dataclasses.charuco_frame_payload import \CharucoFramePayload-from src.core_processor.camera_calibration.charuco_board_detection.dataclasses.charuco_view_data import CharucoViewData+from src.pipelines.calibration_pipeline.charuco_board_detection.dataclasses.charuco_view_data import CharucoViewData-from src.core_processor.camera_calibration.charuco_board_detection.charuco_image_annotator import (+from src.pipelines.calibration_pipeline.charuco_board_detection.charuco_image_annotator import (annotate_image_with_charuco_data,)@@ -17,10 +17,10 @@ logger = logging.getLogger(__name__)class CharucoBoardDetector:def __init__(self):- charuco_board_object = CharucoBoard()- self.charuco_board = charuco_board_object.charuco_board- self.aruco_marker_dict = charuco_board_object.aruco_marker_dict- self.number_of_charuco_corners = charuco_board_object.number_of_charuco_corners+ self.charuco_board_object = CharucoBoard()+ self.charuco_board = self.charuco_board_object.charuco_board+ self.aruco_marker_dict = self.charuco_board_object.aruco_marker_dict+ self.number_of_charuco_corners = self.charuco_board_object.number_of_charuco_cornersdef detect_charuco_board(self, raw_frame_payload: FramePayload) -> CharucoFramePayload:diff --git a/src/core_processor/camera_calibration/charuco_board_detection/charuco_image_annotator.py b/src/pipelines/calibration_pipeline/charuco_board_detection/charuco_image_annotator.pysimilarity index 95%rename from src/core_processor/camera_calibration/charuco_board_detection/charuco_image_annotator.pyrename to src/pipelines/calibration_pipeline/charuco_board_detection/charuco_image_annotator.pyindex 4cf47ded..5dcef17b 100644--- a/src/core_processor/camera_calibration/charuco_board_detection/charuco_image_annotator.py+++ b/src/pipelines/calibration_pipeline/charuco_board_detection/charuco_image_annotator.py@@ -3,7 +3,7 @@ from typing import Listimport cv2import numpy as np-from src.core_processor.camera_calibration.charuco_board_detection.dataclasses.charuco_view_data import CharucoViewData+from src.pipelines.calibration_pipeline.charuco_board_detection.dataclasses.charuco_view_data import CharucoViewDatadef _board_text(diff --git a/src/pipelines/calibration_pipeline/charuco_board_detection/dataclasses/__init__.py b/src/pipelines/calibration_pipeline/charuco_board_detection/dataclasses/__init__.pynew file mode 100644index 00000000..e69de29bdiff --git a/src/pipelines/calibration_pipeline/charuco_board_detection/dataclasses/charuco_board_definition.py b/src/pipelines/calibration_pipeline/charuco_board_detection/dataclasses/charuco_board_definition.pynew file mode 100644index 00000000..60511d6f--- /dev/null+++ b/src/pipelines/calibration_pipeline/charuco_board_detection/dataclasses/charuco_board_definition.py@@ -0,0 +1,26 @@+from dataclasses import dataclass+from typing import Dict++import cv2+import numpy as np+++@dataclass+class CharucoBoard:+ aruco_marker_dict: Dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_4X4_250)+ number_of_squares_width: int = 7+ number_of_squares_height: int = 5+ black_square_side_length: int = 1+ aruco_marker_length_proportional: float = .8++ def __post_init__(self):+ self.charuco_board = cv2.aruco.CharucoBoard_create(self.number_of_squares_width,+ self.number_of_squares_height,+ self.black_square_side_length,+ self.aruco_marker_length_proportional,+ self.aruco_marker_dict)++ self.number_of_charuco_corners = (self.number_of_squares_width - 1) * (self.number_of_squares_height - 1)+++diff --git a/src/core_processor/camera_calibration/charuco_board_detection/dataclasses/charuco_frame_payload.py b/src/pipelines/calibration_pipeline/charuco_board_detection/dataclasses/charuco_frame_payload.pysimilarity index 76%rename from src/core_processor/camera_calibration/charuco_board_detection/dataclasses/charuco_frame_payload.pyrename to src/pipelines/calibration_pipeline/charuco_board_detection/dataclasses/charuco_frame_payload.pyindex 87aa2645..8cc07b49 100644--- a/src/core_processor/camera_calibration/charuco_board_detection/dataclasses/charuco_frame_payload.py+++ b/src/pipelines/calibration_pipeline/charuco_board_detection/dataclasses/charuco_frame_payload.py@@ -3,7 +3,7 @@ from dataclasses import dataclassimport numpy as npfrom src.cameras.capture.dataclasses.frame_payload import FramePayload-from src.core_processor.camera_calibration.charuco_board_detection.dataclasses.charuco_view_data import \+from src.pipelines.calibration_pipeline.charuco_board_detection.dataclasses.charuco_view_data import \CharucoViewDatadiff --git a/src/core_processor/camera_calibration/charuco_board_detection/dataclasses/charuco_view_data.py b/src/pipelines/calibration_pipeline/charuco_board_detection/dataclasses/charuco_view_data.pysimilarity index 100%rename from src/core_processor/camera_calibration/charuco_board_detection/dataclasses/charuco_view_data.pyrename to src/pipelines/calibration_pipeline/charuco_board_detection/dataclasses/charuco_view_data.pydiff --git a/src/core_processor/camera_calibration/charuco_board_detection/shared_board_view_detection.py b/src/pipelines/calibration_pipeline/charuco_board_detection/shared_board_view_detection.pysimilarity index 100%rename from src/core_processor/camera_calibration/charuco_board_detection/shared_board_view_detection.pyrename to src/pipelines/calibration_pipeline/charuco_board_detection/shared_board_view_detection.pydiff --git a/src/pipelines/session_pipeline/__init__.py b/src/pipelines/session_pipeline/__init__.pynew file mode 100644index 00000000..e69de29bdiff --git a/src/session_pipeline/session_pipeline_orchestrator.py b/src/pipelines/session_pipeline/session_pipeline_orchestrator.pysimilarity index 94%rename from src/session_pipeline/session_pipeline_orchestrator.pyrename to src/pipelines/session_pipeline/session_pipeline_orchestrator.pyindex 81c3d2a7..0f8f3afc 100644--- a/src/session_pipeline/session_pipeline_orchestrator.py+++ b/src/pipelines/session_pipeline/session_pipeline_orchestrator.py@@ -64,7 +64,7 @@ class SessionPipelineOrchestrator:charuco_frame_payload = self._camera_calibrator.calibrate(cv_cam)if cb and charuco_frame_payload.annotated_image is not None:- writer.record(charuco_frame_payload.raw_frame_payload)+ writer.append_frame_to_list(charuco_frame_payload.raw_frame_payload)await cb(charuco_frame_payload.annotated_image)except Exception as e:logger.error("Printing traceback")@@ -82,7 +82,7 @@ class SessionPipelineOrchestrator:frame_width=cv_cam.image_width(),frame_height=cv_cam.image_height(),)- writer.save_to_disk(options)+ writer.save_frame_list_to_disk(options)def run(self,@@ -92,10 +92,7 @@ class SessionPipelineOrchestrator:detect_skeleton=True,save_video=True,):- """- Opens Cameras using OpenCV and begins image processing for charuco board- If return images is true, the images are returned to the caller- """+with self._open_cv_camera_manager.start_capture_session_all_cams() as connected_cameras_dict:try:if show_visualizer_gui:@@ -126,7 +123,7 @@ class SessionPipelineOrchestrator:this_cam_this_frame_timestamp_ns)if save_video:- this_open_cv_camera.video_recorder.record(this_cam_latest_frame)+ this_open_cv_camera.video_recorder.append_frame_to_list(this_cam_latest_frame)if calibrate_cameras:undistorted_annotated_image = self._camera_calibrator.calibrate(this_open_cv_camera)@@ -163,7 +160,7 @@ class SessionPipelineOrchestrator:finally:for this_open_cv_camera in connected_cameras_dict.values():if save_video:- this_open_cv_camera.video_recorder.save_to_disk(+ this_open_cv_camera.video_recorder.save_frame_list_to_disk(self.session_folder_path / 'synchronized_videos')logger.info(f"Destroy window {this_open_cv_camera.webcam_id_as_str}")cv2.destroyWindow(this_open_cv_camera.webcam_id_as_str)diff --git a/src/session_pipeline/timestamp_handler.py b/src/pipelines/session_pipeline/timestamp_handler.pysimilarity index 100%rename from src/session_pipeline/timestamp_handler.pyrename to src/pipelines/session_pipeline/timestamp_handler.pydiff --git a/src/qt_visualizer_and_gui/qt_visualizer_and_gui.py b/src/qt_visualizer_and_gui/qt_visualizer_and_gui.pyindex da46ea0a..fe7c299a 100644--- a/src/qt_visualizer_and_gui/qt_visualizer_and_gui.py+++ b/src/qt_visualizer_and_gui/qt_visualizer_and_gui.py@@ -16,7 +16,7 @@ logger = logging.getLogger(__name__)class QTVisualizerAndGui:- def __init__(self):+ def __init__(self, source: str = None):# https://pyqtgraph.readthedocs.io/en/latest/config_options.htmlself._camera0_id = '0'pg.setConfigOptions(imageAxisOrder='row-major')@@ -83,11 +83,13 @@ class QTVisualizerAndGui:def _update_timestamp_difference_plot(self, timestamp_manager: TimestampManager):for webcam_id in self._webcam_ids_list:plot_item = self._dict_of_timestamp_difference_line_plots[webcam_id]- plot_item.setData(np.diff(timestamp_manager.get_timestamps_from_camera_in_seconds_from_session_start(webcam_id)))+ plot_item.setData(+ np.diff(timestamp_manager.get_timestamps_from_camera_in_seconds_from_session_start(webcam_id)))def _update_timestamp_difference_histogram(self, timestamp_manager: TimestampManager):for webcam_id in self._webcam_ids_list:- this_cam_timestamp_diffs = np.diff(timestamp_manager.get_timestamps_from_camera_in_seconds_from_session_start(webcam_id))+ this_cam_timestamp_diffs = np.diff(+ timestamp_manager.get_timestamps_from_camera_in_seconds_from_session_start(webcam_id))num_samples = this_cam_timestamp_diffs.shape[0]counts, bin_edges = np.histogram(this_cam_timestamp_diffs,bins=np.linspace(0, 100, 100))@@ -219,7 +221,7 @@ class QTVisualizerAndGui:self._dict_of_timestamp_difference_histograms[this_webcam_id] = this_histogram_plot_itemself._timestamp_diff_histgoram_dock = Dock("Camera Timestamp Difference histogram")- self._timestamp_diff_histgoram_dock .addWidget(timestamp_difference_histogram_plot_widget)+ self._timestamp_diff_histgoram_dock.addWidget(timestamp_difference_histogram_plot_widget)self._main_dock_area.addDock(self._timestamp_diff_histgoram_dock, 'right', self._timestamp_difference_plot_dock)def _pause(self):@@ -266,8 +268,8 @@ class QTVisualizerAndGui:self.opengl_3d_plot_widget.addItem(self.origin_z_axis_gl_lineplot_item)self.opengl_charuco_scatter_item = gl.GLScatterPlotItem(pos=(0, 0, 0), color=(1, 0, 1), size=1,- pxMode=False)+ pxMode=False)self.opengl_3d_plot_dock = Dock("3d View Port")self.opengl_3d_plot_dock.addWidget(self.opengl_3d_plot_widget)- self._main_dock_area.addDock(self.opengl_3d_plot_dock, 'right', self._camera_views_dock )+ self._main_dock_area.addDock(self.opengl_3d_plot_dock, 'right', self._camera_views_dock)diff --git a/src/tests/video_writer/test_video_writer.py b/src/tests/video_writer/test_video_writer.pyindex a5ee370d..f954ed71 100644--- a/src/tests/video_writer/test_video_writer.py+++ b/src/tests/video_writer/test_video_writer.py@@ -23,7 +23,7 @@ class VideoWriterTestCase(TestCase):image=np.random.randint(0, 4, (720, 1280, 3)), timestamp=time.time_ns())vw = VideoRecorder()- vw.record(example_payload)+ vw.append_frame_to_list(example_payload)assert vw.frame_count == 1@@ -33,7 +33,7 @@ class VideoWriterTestCase(TestCase):image=np.random.randint(0, 4, (720, 1280, 3)), timestamp=time.time_ns())vw = VideoRecorder()- vw.record(example_payload)+ vw.append_frame_to_list(example_payload)save_options = SaveOptions(writer_dir=file_path,@@ -43,7 +43,7 @@ class VideoWriterTestCase(TestCase):fps=1,)- vw.save_to_disk(save_options)+ vw.save_frame_list_to_disk(save_options)expected_path = Path().joinpath(file_path, "movie.mp4")assert save_options.full_path == expected_path, "Paths do not match"diff --git a/versioneer.py b/versioneer.pyindex 2b545405..958cc008 100644--- a/versioneer.py+++ b/versioneer.py@@ -1623,7 +1623,7 @@ def get_cmdclass():print("UPDATING %s" % target_versionfile)write_to_version_file(target_versionfile, versions)- _build_exe.run(self)+ _build_exe.record_videos(self)os.unlink(target_versionfile)with open(cfg.versionfile_source, "w") as f:LONG = LONG_VERSION_PY[cfg.VCS]@@ -1656,7 +1656,7 @@ def get_cmdclass():print("UPDATING %s" % target_versionfile)write_to_version_file(target_versionfile, versions)- _py2exe.run(self)+ _py2exe.record_videos(self)os.unlink(target_versionfile)with open(cfg.versionfile_source, "w") as f:LONG = LONG_VERSION_PY[cfg.VCS]