Skip to content

Free Motion Capture for Everyone πŸ’€βœ¨

TypeScript

that's calibration, baybee! ;D

Jon272fc3a5d044
diff --git a/freemocap/fmc_anipose.py b/freemocap/fmc_anipose.py
index 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.shape
ids = remap_ids(extra['ids'])
- n_ids = np.max(ids)+1
+ n_ids = np.max(ids) + 1
good = ~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:
break
final_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 + 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)
@@ -144,6 +152,7 @@ def interpolate_data(vals):
out[:] = 0
return 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] = i
return 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) * v
return 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_cams
n3d = 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_scale
else:
@@ -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 + tvecs
else:
n_boards = 0
total_board_params = 0
@@ -915,17 +923,15 @@ class CameraGroup:
## 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
-
+ 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
-
+ A_sparse[n_good_values + point_ix * 3 + i,
+ n_cams * n_cam_params + point_ix * 3 + i] = 1
return 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 0
rvecs = 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_smooth
t1 = 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_smooth
t1 = 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 * 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:])
+ 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)
@@ -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':
pass
elif 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 constraint
errors_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) / expected
errors_lengths = errors_lengths.ravel() * scale_l0ength
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[cix] = 100 * (lengths - expected) / expected
errors_lengths_weak = errors_lengths_weak.ravel() * scale_length_weak
return 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 * 3
n_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 params
bad = 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] = 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
+ 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])
@@ -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 * 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_errors_lengths + n_errors_lengths_weak
- n_3d = n_frames*n_joints*3
+ n_3d = n_frames * n_joints * 3
n_params = n_3d + n_constraints + n_constraints_weak
point_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 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
+ 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)
@@ -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 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
+ 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)
@@ -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] = 1
return 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_frames
def 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_frames
def 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.txt
index b0c81c8a..e5286eca 100644
--- a/requirements.txt
+++ b/requirements.txt
@@ -33,4 +33,5 @@ pyqtgraph==0.12.4
pyqt5==5.15.6
pyopengl==3.1.6
mediapipe~=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 file
diff --git a/src/api/routes/session/session_router.py b/src/api/routes/session/session_router.py
index 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)"""
+ pass
diff --git a/src/cameras/capture/dataclasses/frame_payload.py b/src/cameras/capture/dataclasses/frame_payload.py
index 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 = False
image: np.ndarray = None
timestamp: Union[int, float] = None
+ frame_number: int = None
webcam_id: str = None
diff --git a/src/cameras/capture/opencv_camera/opencv_camera.py b/src/cameras/capture/opencv_camera/opencv_camera.py
index 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 = config
self._name = f"Camera_{self._config.webcam_id}"
@@ -24,6 +25,14 @@ class OpenCVCamera:
self._video_recorder: VideoRecorder = None
self._running_thread: VideoCaptureThread = None
self._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
@property
def new_frame_ready(self):
@@ -56,6 +65,24 @@ class OpenCVCamera:
self._new_frame_ready = False
return 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 success
def 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 parameters
self._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) / 2
self._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.py
index 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)
@contextmanager
def start_capture_session_single_cam(
diff --git a/src/cameras/persistence/video_writer/video_recorder.py b/src/cameras/persistence/video_writer/video_recorder.py
index 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 pd
from src.cameras.capture.dataclasses.frame_payload import FramePayload
from src.cameras.persistence.video_writer.save_options_dataclass import SaveOptions
from src.config.data_paths import freemocap_data_path
+from src.config.home_dir import get_session_path
logger = 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_name
self._image_width = image_width
self._image_height = image_height
self._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
@property
def 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 e
finally:
- 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))) ** -1
except:
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.py
index 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 dataclass
import cv2
-import numpy as np
from src.cameras.capture.dataclasses.frame_payload import FramePayload
from src.cameras.capture.opencv_camera.opencv_camera import OpenCVCamera
from 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 CharucoViewData
logger = 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.py
deleted file mode 100644
index 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.py
index 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 print
from src.cameras.capture.dataclasses.frame_payload import FramePayload
from 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 BoardDetector
from 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_data
diff --git a/src/core_processor/fps/timestamp_manager.py b/src/core_processor/fps/timestamp_manager.py
index 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, Union
import 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_second
diff --git a/src/core_processor/camera_calibration/charuco_board_detection/__init__.py b/src/pipelines/__init__.py
similarity index 100%
rename from src/core_processor/camera_calibration/charuco_board_detection/__init__.py
rename to src/pipelines/__init__.py
diff --git a/src/core_processor/camera_calibration/charuco_board_detection/dataclasses/__init__.py b/src/pipelines/calibration_pipeline/__init__.py
similarity index 100%
rename from src/core_processor/camera_calibration/charuco_board_detection/dataclasses/__init__.py
rename to src/pipelines/calibration_pipeline/__init__.py
diff --git a/src/session_pipeline/__init__.py b/src/pipelines/calibration_pipeline/anipose_camera_calibration/__init__.py
similarity index 100%
rename from src/session_pipeline/__init__.py
rename to src/pipelines/calibration_pipeline/anipose_camera_calibration/__init__.py
diff --git a/src/pipelines/calibration_pipeline/anipose_camera_calibration/anipose_camera_calibration.py b/src/pipelines/calibration_pipeline/anipose_camera_calibration/anipose_camera_calibration.py
new file mode 100644
index 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__":
+ pass
diff --git a/src/pipelines/calibration_pipeline/anipose_camera_calibration/freemocap_anipose.py b/src/pipelines/calibration_pipeline/anipose_camera_calibration/freemocap_anipose.py
new file mode 100644
index 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.py
new file mode 100644
index 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__.py
new file mode 100644
index 00000000..e69de29b
diff --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.py
similarity index 84%
rename from src/core_processor/camera_calibration/charuco_board_detection/charuco_board_detector.py
rename to src/pipelines/calibration_pipeline/charuco_board_detection/charuco_board_detector.py
index 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 logging
import cv2
from 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_corners
def 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.py
similarity index 95%
rename from src/core_processor/camera_calibration/charuco_board_detection/charuco_image_annotator.py
rename to src/pipelines/calibration_pipeline/charuco_board_detection/charuco_image_annotator.py
index 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 List
import cv2
import 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 CharucoViewData
def _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__.py
new file mode 100644
index 00000000..e69de29b
diff --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.py
new file mode 100644
index 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.py
similarity index 76%
rename from src/core_processor/camera_calibration/charuco_board_detection/dataclasses/charuco_frame_payload.py
rename to src/pipelines/calibration_pipeline/charuco_board_detection/dataclasses/charuco_frame_payload.py
index 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 dataclass
import numpy as np
from 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 \
CharucoViewData
diff --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.py
similarity index 100%
rename from src/core_processor/camera_calibration/charuco_board_detection/dataclasses/charuco_view_data.py
rename to src/pipelines/calibration_pipeline/charuco_board_detection/dataclasses/charuco_view_data.py
diff --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.py
similarity index 100%
rename from src/core_processor/camera_calibration/charuco_board_detection/shared_board_view_detection.py
rename to src/pipelines/calibration_pipeline/charuco_board_detection/shared_board_view_detection.py
diff --git a/src/pipelines/session_pipeline/__init__.py b/src/pipelines/session_pipeline/__init__.py
new file mode 100644
index 00000000..e69de29b
diff --git a/src/session_pipeline/session_pipeline_orchestrator.py b/src/pipelines/session_pipeline/session_pipeline_orchestrator.py
similarity index 94%
rename from src/session_pipeline/session_pipeline_orchestrator.py
rename to src/pipelines/session_pipeline/session_pipeline_orchestrator.py
index 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.py
similarity index 100%
rename from src/session_pipeline/timestamp_handler.py
rename to src/pipelines/session_pipeline/timestamp_handler.py
diff --git a/src/qt_visualizer_and_gui/qt_visualizer_and_gui.py b/src/qt_visualizer_and_gui/qt_visualizer_and_gui.py
index 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.html
self._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_item
self._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.py
index 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.py
index 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]