Free Motion Capture for Everyone πβ¨
TypeScript
bring in old anipose
jonmatthisc8b73b41bce5
diff --git a/freemocap/core_processes/capture_volume_calibration/anipose_camera_calibration/freemocap_anipose.py b/freemocap/core_processes/capture_volume_calibration/anipose_camera_calibration/freemocap_anipose.pynew file mode 100644index 00000000..3d0b8c8a--- /dev/null+++ b/freemocap/core_processes/capture_volume_calibration/anipose_camera_calibration/freemocap_anipose.py@@ -0,0 +1,2163 @@+# Most of this was copied (with permission) from the original `aniposelib` package (https://github.com/lambdaloop/aniposelib), and we're adapting it to our needs here. M+# ore info on Anipoise: https://anipose.readthedocs.io/en/latest/++import itertools+import logging+import multiprocessing+from pathlib import Path+import queue+import time+from collections import defaultdict, Counter+from copy import copy++import cv2+import numpy as np+import toml+from aniposelib.boards import extract_points, extract_rtvecs, get_video_params, merge_rows, CharucoBoard+from aniposelib.utils import get_rtvec, make_M+from numba import jit+from scipy import optimize+from scipy import signal+from scipy.cluster.hierarchy import linkage, fcluster+from scipy.cluster.vq import whiten+from scipy.linalg import inv as inverse+from scipy.sparse import dok_matrix+from tqdm import trange+from typing import List++from skellytracker.trackers.charuco_tracker.charuco_model_info import CharucoModelInfo, CharucoTrackingParams+from skellytracker.process_folder_of_videos import process_list_of_videos++numba_logger = logging.getLogger("numba")+numba_logger.setLevel(logging.WARNING)++logger = logging.getLogger(__name__)+++@jit(nopython=True, parallel=False)+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+++def get_connections(xs, cam_names=None, both=True):+ n_cams = xs.shape[0]+ n_points = xs.shape[1]++ if cam_names is None:+ cam_names = np.arange(n_cams)++ connections = defaultdict(int)++ for rnum in range(n_points):+ ixs = np.where(~np.isnan(xs[:, rnum, 0]))[0]+ keys = [cam_names[ix] for ix in ixs]+ for i in range(len(keys)):+ for j in range(i + 1, len(keys)):+ a = keys[i]+ b = keys[j]+ connections[(a, b)] += 1+ if both:+ connections[(b, a)] += 1++ return connections+++def get_calibration_graph(rtvecs, cam_names=None):+ n_cams = rtvecs.shape[0]+ n_points = rtvecs.shape[1]++ if cam_names is None:+ cam_names = np.arange(n_cams)++ connections = get_connections(rtvecs, np.arange(n_cams))++ components = dict(zip(np.arange(n_cams), range(n_cams)))+ edges = set(connections.items())++ graph = defaultdict(list)++ for edgenum in range(n_cams - 1):+ if len(edges) == 0:+ component_names = dict()+ for k, v in list(components.items()):+ component_names[cam_names[k]] = v+ raise ValueError(+ """+Could not build calibration graph.+Some group of cameras could not be paired by simultaneous calibration board detections.+Check which cameras have different group numbers below to see the missing edges.+{}""".format(+ component_names+ )+ )++ (a, b), weight = max(edges, key=lambda x: x[1])+ graph[a].append(b)+ graph[b].append(a)++ match = components[a]+ replace = components[b]+ for k, v in components.items():+ if match == v:+ components[k] = replace++ for e in edges.copy():+ (a, b), w = e+ if components[a] == components[b]:+ edges.remove(e)++ return graph+++def find_calibration_pairs(graph, source=None):+ pairs = []+ explored = set()++ if source is None:+ source = sorted(graph.keys())[0]++ q = queue.deque()+ q.append(source)++ while len(q) > 0:+ item = q.pop()+ explored.add(item)++ for new in graph[item]:+ if new not in explored:+ q.append(new)+ pairs.append((item, new))+ return pairs+++def compute_camera_matrices(rtvecs, pairs):+ extrinsics = dict()+ source = pairs[0][0]+ extrinsics[source] = np.identity(4)+ for a, b in pairs:+ ext = get_transform(rtvecs, b, a)+ extrinsics[b] = np.matmul(ext, extrinsics[a])+ return extrinsics+++def get_transform(rtvecs, left, right):+ L = []+ for dix in range(rtvecs.shape[1]):+ d = rtvecs[:, dix]+ good = ~np.isnan(d[:, 0])++ if good[left] and good[right]:+ M_left = make_M(d[left, 0:3], d[left, 3:6])+ M_right = make_M(d[right, 0:3], d[right, 3:6])+ M = np.matmul(M_left, inverse(M_right))+ L.append(M)+ L_best = select_matrices(L)+ M_mean = mean_transform(L_best)+ # M_mean = mean_transform_robust(L, M_mean, error=0.5)+ # M_mean = mean_transform_robust(L, M_mean, error=0.2)+ M_mean = mean_transform_robust(L, M_mean, error=0.1)+ return M_mean+++def get_most_common(vals):+ Z = linkage(whiten(vals), "ward")+ n_clust = max(len(vals) / 10, 3)+ clusts = fcluster(Z, t=n_clust, criterion="maxclust")+ cc = Counter(clusts[clusts >= 0])+ most = cc.most_common(n=1)+ top = most[0][0]+ good = clusts == top+ return good+++def select_matrices(Ms):+ Ms = np.array(Ms)+ rvecs = [cv2.Rodrigues(M[:3, :3])[0][:, 0] for M in Ms]+ tvecs = np.array([M[:3, 3] for M in Ms])+ best = get_most_common(np.hstack([rvecs, tvecs]))+ Ms_best = Ms[best]+ return Ms_best+++def mean_transform(M_list):+ rvecs = [cv2.Rodrigues(M[:3, :3])[0][:, 0] for M in M_list]+ tvecs = [M[:3, 3] for M in M_list]++ rvec = np.mean(rvecs, axis=0)+ tvec = np.mean(tvecs, axis=0)++ return make_M(rvec, tvec)+++def mean_transform_robust(M_list, approx=None, error=0.3):+ if approx is None:+ M_list_robust = M_list+ else:+ M_list_robust = []+ for M in M_list:+ rot_error = (M - approx)[:3, :3]+ m = np.max(np.abs(rot_error))+ if m < error:+ M_list_robust.append(M)+ return mean_transform(M_list_robust)+++def get_initial_extrinsics(rtvecs, cam_names=None):+ graph = get_calibration_graph(rtvecs, cam_names)+ pairs = find_calibration_pairs(graph, source=0)+ extrinsics = compute_camera_matrices(rtvecs, pairs)++ n_cams = rtvecs.shape[0]+ rvecs = []+ tvecs = []+ for cnum in range(n_cams):+ rvec, tvec = get_rtvec(extrinsics[cnum])+ rvecs.append(rvec)+ tvecs.append(tvec)+ rvecs = np.array(rvecs)+ tvecs = np.array(tvecs)+ return rvecs, 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 single_camera_reprojection_error(self, p3d, p2d):+ projecting_3d_points_onto_2d_image_plane_og = self.project(p3d)+ projecting_3d_points_onto_2d_image_plane = projecting_3d_points_onto_2d_image_plane_og.reshape(p2d.shape)+ return p2d - projecting_3d_points_onto_2d_image_plane++ 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+ self.charuco_2d_data = None++ 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, kill_event: multiprocessing.Event = None):+ """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 kill_event is not None and kill_event.is_set():+ return None++ if one_point:+ out = out[0]++ return out++ def triangulate_possible(+ self,+ points,+ undistort=True,+ min_cams=2,+ progress=False,+ threshold=0.5,+ kill_event: multiprocessing.Event = None,+ ):+ """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++ if kill_event is not None and kill_event.is_set():+ return None++ 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 #original code from OG anipose+ return out # simplify output so that `triangulate_ransac` can be used exactly the same way as `triangulate`++ def triangulate_ransac(+ self, points, undistort=True, min_cams=2, progress=False, kill_event: multiprocessing.Event = None+ ):+ """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, kill_event=kill_event+ )++ @jit(parallel=True, forceobj=True)+ def reprojection_error(self, points_3d, points_2d, 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(points_3d.shape) == 1 and len(points_2d.shape) == 2:+ points_3d = points_3d.reshape(1, 3)+ points_2d = points_2d.reshape(-1, 1, 2)+ one_point = True++ n_cams, n_points, _ = points_2d.shape+ assert points_3d.shape == (+ n_points,+ 3,+ ), "shapes of 2D and 3D points are not consistent: " "2D={}, 3D={}".format(points_2d.shape, points_3d.shape)++ errors = np.empty((n_cams, n_points, 2))++ for camera_number, cam in enumerate(self.cameras):+ errors[camera_number] = cam.single_camera_reprojection_error(points_3d, points_2d[camera_number])++ 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:+ print(error_dict)+ print("error: {:.2f}, mu: {:.1f}, ratio: {:.3f}".format(error, mu, np.mean(good)))+ print(f"previous error scores: [magenta] {error_list}[/magenta]")++ 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:+ print(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:+ print(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(inverse(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 calculate_center_of_mass 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:+ print(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: List[List[str]], board: "AniposeCharucoBoard", verbose=True):+ num_corners = board.total_size+ if board.total_size != 24:+ raise ValueError("AniposeCharucoBoard only supports 24 corners, got {}".format(board.total_size))+ self._get_charuco_2d_data(videos)++ if self.charuco_2d_data is None:+ raise ValueError("Charuco 2D data has not been initialized. Call _get_charuco_2d_data() first, or check for errors in the video processing.")++ all_rows = []++ num_cameras, num_frames, _, _ = self.charuco_2d_data.shape+ for camera_number in range(num_cameras):+ camera_rows = []+ for frame in range(num_frames):+ # TODO: check each frame based on anipose's conditions+ filled = self.charuco_2d_data[camera_number, frame, :, :]+ filled = filled.astype(np.float32)+ filled = np.reshape(filled, (num_corners, 1, 2)) # Add empty column anipose expects+ mask = (~np.isnan(filled[:, :, 0])) & (~np.isnan(filled[:, :, 1]))+ non_empty_ids = np.where(mask)[0]+ corners = filled[non_empty_ids, :, :]+ non_empty_ids = non_empty_ids.reshape(-1, 1) # Add empty column anipose expects+ if corners.shape[0] != 0:+ row = {+ "framenum": (0, frame),+ "corners": corners,+ "ids": non_empty_ids,+ "filled": filled,+ }+ camera_rows.append(row)+ all_rows.append(camera_rows)+ if verbose:+ print(f"Charuco detection results:")+ for i, rows in enumerate(all_rows):+ print(f"\tCamera {i} has {len(rows)} frames with detected corners.")+++ return all_rows++ def _get_charuco_2d_data(self, videos: List[List[str]]):+ """+ Processes a list of a list of videos to extract Charuco 2D data.++ Should be called once during the initial calibration, and then referenced with self.charuco_2d_data+ """+ video_paths = [Path(video[0]) for video in videos]+ charuco_2d_data = process_list_of_videos(+ model_info=CharucoModelInfo(),+ tracking_params=CharucoTrackingParams(),+ video_paths=video_paths,+ num_processes=min(len(videos), multiprocessing.cpu_count() - 1),+ )++ self.charuco_2d_data = charuco_2d_data+++ 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: List[List[str]],+ board: "AniposeCharucoBoard",+ 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)++ @staticmethod+ 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)+++class AniposeCharucoBoard(CharucoBoard):+ def __init__(+ self,+ squaresX,+ squaresY,+ square_length,+ marker_length,+ marker_bits=4,+ dict_size=50,+ aruco_dict=None,+ manually_verify=False,+ ):+ self.squaresX = squaresX+ self.squaresY = squaresY+ self.square_length = square_length+ self.marker_length = marker_length+ self.manually_verify = manually_verify++ ARUCO_DICTS = {+ (4, 50): cv2.aruco.DICT_4X4_50,+ (5, 50): cv2.aruco.DICT_5X5_50,+ (6, 50): cv2.aruco.DICT_6X6_50,+ (7, 50): cv2.aruco.DICT_7X7_50,+ (4, 100): cv2.aruco.DICT_4X4_100,+ (5, 100): cv2.aruco.DICT_5X5_100,+ (6, 100): cv2.aruco.DICT_6X6_100,+ (7, 100): cv2.aruco.DICT_7X7_100,+ (4, 250): cv2.aruco.DICT_4X4_250,+ (5, 250): cv2.aruco.DICT_5X5_250,+ (6, 250): cv2.aruco.DICT_6X6_250,+ (7, 250): cv2.aruco.DICT_7X7_250,+ (4, 1000): cv2.aruco.DICT_4X4_1000,+ (5, 1000): cv2.aruco.DICT_5X5_1000,+ (6, 1000): cv2.aruco.DICT_6X6_1000,+ (7, 1000): cv2.aruco.DICT_7X7_1000,+ }++ dkey = (marker_bits, dict_size)+ self.dictionary = cv2.aruco.getPredefinedDictionary(ARUCO_DICTS[dkey])++ self.board = cv2.aruco.CharucoBoard(+ size=[squaresX, squaresY],+ squareLength=square_length,+ markerLength=marker_length,+ dictionary=self.dictionary,+ )++ total_size = (squaresX - 1) * (squaresY - 1)++ objp = np.zeros((total_size, 3), np.float64)+ objp[:, :2] = np.mgrid[0: (squaresX - 1), 0: (squaresY - 1)].T.reshape(-1, 2)+ objp *= square_length+ self.objPoints = objp++ self.empty_detection = np.zeros((total_size, 1, 2)) * np.nan+ self.total_size = total_size++ def detect_markers(self, image, camera=None, refine=True):+ if len(image.shape) == 3:+ gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)+ else:+ gray = image++ params = cv2.aruco.DetectorParameters()+ params.cornerRefinementMethod = cv2.aruco.CORNER_REFINE_CONTOUR+ params.adaptiveThreshWinSizeMin = 100+ params.adaptiveThreshWinSizeMax = 700+ params.adaptiveThreshWinSizeStep = 50+ params.adaptiveThreshConstant = 0++ try:+ corners, ids, rejectedImgPoints = cv2.aruco.detectMarkers(gray, self.dictionary, parameters=params)+ except Exception:+ ids = None++ if ids is None:+ return [], []++ if camera is None:+ K = D = None+ else:+ K = camera.get_camera_matrix()+ D = camera.get_distortions()++ if refine:+ detectedCorners, detectedIds, rejectedCorners, recoveredIdxs = cv2.aruco.refineDetectedMarkers(+ gray, self.board, corners, ids, rejectedImgPoints, K, D, parameters=params+ )+ else:+ detectedCorners, detectedIds = corners, ids++ return detectedCorners, detectedIds++ def detect_image(self, image, camera=None):+ if len(image.shape) == 3:+ gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)+ else:+ gray = image++ corners, ids = self.detect_markers(image, camera, refine=True)+ if len(corners) > 0:+ ret, detectedCorners, detectedIds = cv2.aruco.interpolateCornersCharuco(corners, ids, gray, self.board)+ if detectedIds is None:+ detectedCorners = detectedIds = np.float64([])+ else:+ detectedCorners = detectedIds = np.float64([])++ if (+ len(detectedCorners) > 0+ and self.manually_verify+ and not self.manually_verify_board_detection(gray, detectedCorners, detectedIds)+ ):+ detectedCorners = detectedIds = np.float64([])++ return detectedCorners, detectedIds++ def manually_verify_board_detection(self, image, corners, ids=None):+ height, width = image.shape[:2]+ image = cv2.aruco.drawDetectedCornersCharuco(image, corners, ids)+ cv2.putText(+ image,+ "(a) Accept (d) Reject",+ (int(width / 1.35), int(height / 16)),+ cv2.FONT_HERSHEY_SIMPLEX,+ 0.5,+ 255,+ 1,+ cv2.LINE_AA,+ )+ cv2.imshow("verify_detection", image)+ while 1:+ key = cv2.waitKey(0) & 0xFF+ if key == ord("a"):+ cv2.putText(+ image,+ "Accepted!",+ (int(width / 2.5), int(height / 1.05)),+ cv2.FONT_HERSHEY_SIMPLEX,+ 1,+ 255,+ 2,+ cv2.LINE_AA,+ )+ cv2.imshow("verify_detection", image)+ cv2.waitKey(100)+ return True+ elif key == ord("d"):+ cv2.putText(+ image,+ "Rejected!",+ (int(width / 2.5), int(height / 1.05)),+ cv2.FONT_HERSHEY_SIMPLEX,+ 1,+ 255,+ 2,+ cv2.LINE_AA,+ )+ cv2.imshow("verify_detection", image)+ cv2.waitKey(100)+ return False++ def estimate_pose_points(self, camera, corners, ids):+ if corners is None or ids is None or len(corners) < 5:+ return None, None++ n_corners = corners.size // 2+ corners = np.reshape(corners, (n_corners, 1, 2))++ K = camera.get_camera_matrix()+ D = camera.get_distortions()++ ret, rvec, tvec = cv2.aruco.estimatePoseCharucoBoard(corners, ids, self.board, K, D, None, None)++ return rvec, tvec