Free Motion Capture for Everyone πβ¨
TypeScript
added script to load empties into blender
jon matthisd738ea4286ec
diff --git a/freemocap/dev_scratchpad/dev_alpha/FMC_MultiCamera.py b/freemocap/dev_scratchpad/dev_alpha/FMC_MultiCamera.pyindex 0d5c667f..310ce0f3 100644--- a/freemocap/dev_scratchpad/dev_alpha/FMC_MultiCamera.py+++ b/freemocap/dev_scratchpad/dev_alpha/FMC_MultiCamera.py@@ -316,7 +316,7 @@ class FMC_MultiCamera:self.multi_cam_tuple_queue.put(this_multi_cam_tuple, np.mean(these_timestamps))- # rich_console.log('Created a multi_cam_tuple - queue size: {}'.format(self.multi_cam_tuple_queue.qsize()))+ rich_console.log('Created a multi_cam_tuple - queue size: {}'.format(self.multi_cam_tuple_queue.qsize()))def stitch_multicam_image(self, multi_cam_tuple):diff --git a/freemocap/dev_scratchpad/dev_alpha/default_json_definitions/freemocap_mediapipe_json.json b/freemocap/dev_scratchpad/dev_alpha/default_json_definitions/freemocap_mediapipe_json.jsonindex 20a7bfb2..72880b7b 100644--- a/freemocap/dev_scratchpad/dev_alpha/default_json_definitions/freemocap_mediapipe_json.json+++ b/freemocap/dev_scratchpad/dev_alpha/default_json_definitions/freemocap_mediapipe_json.json@@ -1,5 +1,4 @@{- "mediapipe_skel_info": {"body": {"body_mass_segments": {"head": {@@ -457,94 +456,280 @@"right_hip","left_knee","right_knee",+ "right_ankle","left_ankle","left_heel","right_heel","left_foot_index","right_foot_index"- ]- },- "hands": {- "body_segments": {- "constrain_length_bool": false,- "left_upper_arm": {- "parent": "left_shoulder",- "child": "left_elbow",- "anthropometric_definition": {- "proportional_length": 0.186,- "proportional_mass": 0.028,- "center_of_mass_proximal": 0.436,- "center_of_mass_distal": 0.564+ ],+ "tracked_point_to_rigify_fk_rig_bone_connections":{+ "nose": {+ "head": {+ "track_type":"bone_tail",+ "weight":1.0},- "spherical_joint_limits": {- "azimuth": [- 0,- 360- ],- "elevation": [- 0,- 360- ]- }+ "left_eye_inner": {},+ "left_eye": {},+ "left_eye_outer": {},+ "right_eye_inner": {},+ "right_eye": {},+ "right_eye_outer": {},+ "left_ear": {},+ "right_ear": {},+ "mouth_left": {},+ "mouth_right": {},+ "left_shoulder": {+ "shoulder.L": {+ "track_type":"bone_tail",+ "weight":1.0},- "etc": {}+ "upper_arm.L": {+ "track_type":"bone_head",+ "weight":1.0+ }},- "virtual_markers": {- "neck": {- "left_shoulder": 0.5,- "right_shoudler": 0.5+ "right_shoulder": {+ "shoulder.R": {+ "track_type":"bone_tail",+ "weight":1.0},- "center_of_mass": {- "etc": 0.9+ "upper_arm.R": {+ "track_type":"bone_head",+ "weight":1.0}},- "dotto_names": [- "nose",- "left_eye_inner",- "butt",- "etc"- ]- },- "face": {- "body_segments": {- "constrain_length_bool": false,- "left_upper_arm": {- "parent": "left_shoulder",- "child": "left_elbow",- "anthropometric_definition": {- "proportional_length": 0.186,- "proportional_mass": 0.028,- "center_of_mass_proximal": 0.436,- "center_of_mass_distal": 0.564- },- "spherical_joint_limits": {- "azimuth": [- 0,- 360- ],- "elevation": [- 0,- 360- ]- }+ "left_elbow": {+ "upper_arm.L": {+ "track_type":"bone_tail",+ "weight":1.0},- "etc": {}+ "forearm.L": {+ "track_type":"bone_head",+ "weight":1.0+ }},- "virtual_markers": {- "neck": {- "left_shoulder": 0.5,- "right_shoudler": 0.5+ "right_elbow": {+ "upper_arm.R": {+ "track_type":"bone_tail",+ "weight":1.0},- "center_of_mass": {- "etc": 0.9+ "forearm.R": {+ "track_type":"bone_head",+ "weight":1.0}},- "dotto_names": [- "nose",- "left_eye_inner",- "butt",- "etc"- ]+ "left_wrist": {+ "forearm.L": {+ "track_type":"bone_tail",+ "weight":1.0+ },+ "hand.L": {+ "track_type":"bone_head",+ "weight":1.0+ }+ },+ "right_wrist": {+ "forearm.R": {+ "track_type":"bone_tail",+ "weight":1.0+ },+ "hand.R": {+ "track_type":"bone_head",+ "weight":1.0+ }+ },+ "left_pinky": {+ "hand.L": {+ "track_type":"bone_tail",+ "weight":1.0+ }+ },+ "right_pinky": {+ "hand.R": {+ "track_type":"bone_tail",+ "weight":1.0+ }+ },+ "left_index": {},+ "right_index": {},+ "left_thumb": {},+ "right_thumb": {},+ "left_hip": {+ "pelvis.L": {+ "track_type":"bone_tail",+ "weight":1.0+ },+ "thigh.L": {+ "track_type":"bone_head",+ "weight":1.0+ }+ },+ "right_hip": {+ "pelvis.R": {+ "track_type":"bone_tail",+ "weight":1.0+ },+ "thigh.R": {+ "track_type":"bone_head",+ "weight":1.0+ }+ },+ "left_knee": {+ "thigh.L": {+ "track_type":"bone_tail",+ "weight":1.0+ },+ "shin.L": {+ "track_type":"bone_head",+ "weight":1.0+ }+ },+ "right_knee": {+ "shin.R": {+ "track_type":"bone_tail",+ "weight":1.0+ },+ "foot.R": {+ "track_type":"bone_head",+ "weight":1.0+ }+ },+ "right_ankle": {+ "shin.R": {+ "track_type":"bone_tail",+ "weight":1.0+ },+ "foot.R": {+ "track_type":"bone_head",+ "weight":1.0+ }+ },+ "left_ankle": {+ "shin.L": {+ "track_type":"bone_tail",+ "weight":1.0+ },+ "foot.L": {+ "track_type":"bone_head",+ "weight":1.0+ }+ },+ "left_heel": {+ "foot.L": {+ "track_type":"bone_tail",+ "weight":1.0+ },+ "toe.L": {+ "track_type":"bone_head",+ "weight":1.0+ }+ },+ "right_heel": {+ "foot.R": {+ "track_type":"bone_tail",+ "weight":1.0+ },+ "toe.R": {+ "track_type":"bone_head",+ "weight":1.0+ }+ },+ "left_foot_index": {+ "toe.L": {+ "track_type":"bone_tail",+ "weight":1.0+ }+ },+ "right_foot_index":{+ "toe.R": {+ "track_type":"bone_tail",+ "weight":1.0+ }+ }}}+},+ "hands": {+ "body_segments": {+ "constrain_length_bool": false,+ "left_upper_arm": {+ "parent": "left_shoulder",+ "child": "left_elbow",+ "anthropometric_definition": {+ "proportional_length": 0.186,+ "proportional_mass": 0.028,+ "center_of_mass_proximal": 0.436,+ "center_of_mass_distal": 0.564+ },+ "spherical_joint_limits": {+ "azimuth": [+ 0,+ 360+ ],+ "elevation": [+ 0,+ 360+ ]+ }+ },+ "etc": {}+ },+ "virtual_markers": {+ "neck": {+ "left_shoulder": 0.5,+ "right_shoudler": 0.5+ },+ "center_of_mass": {+ "etc": 0.9+ }+ },+ "dotto_names": [+ "nose",+ "left_eye_inner",+ "butt",+ "etc"+ ]+ },+ "face": {+ "body_segments": {+ "constrain_length_bool": false,+ "left_upper_arm": {+ "parent": "left_shoulder",+ "child": "left_elbow",+ "anthropometric_definition": {+ "proportional_length": 0.186,+ "proportional_mass": 0.028,+ "center_of_mass_proximal": 0.436,+ "center_of_mass_distal": 0.564+ },+ "spherical_joint_limits": {+ "azimuth": [+ 0,+ 360+ ],+ "elevation": [+ 0,+ 360+ ]+ }+ },+ "etc": {}+ },+ "virtual_markers": {+ "neck": {+ "left_shoulder": 0.5,+ "right_shoudler": 0.5+ },+ "center_of_mass": {+ "etc": 0.9+ }+ },+ "dotto_names": [+ "nose",+ "left_eye_inner",+ "butt",+ "etc"+ ]+ }}\ No newline at end of filediff --git a/freemocap/dev_scratchpad/fit_skel_to_dots.ipynb b/freemocap/dev_scratchpad/fit_skel_to_dots.ipynbnew file mode 100644index 00000000..93f5396a--- /dev/null+++ b/freemocap/dev_scratchpad/fit_skel_to_dots.ipynb@@ -0,0 +1,100 @@+{+ "cells": [+ {+ "cell_type": "code",+ "execution_count": null,+ "metadata": {},+ "outputs": [],+ "source": [+ "import numpy as np\n",+ "from scipy import optimize\n",+ "import json\n",+ "from rich import print"+ ]+ },+ {+ "cell_type": "markdown",+ "metadata": {},+ "source": [+ "# load data"+ ]+ },+ {+ "cell_type": "code",+ "execution_count": null,+ "metadata": {},+ "outputs": [],+ "source": [+ "skel_fr_mar_dim_og = np.load(r\"C:\\Users\\jonma\\Dropbox\\FreeMoCapProject\\FreeMocap_Data\\sesh_2022-01-03_19_14_39\\DataArrays\\mediaPipeSkel_3d_smoothed.npy\")"+ ]+ },+ {+ "cell_type": "markdown",+ "metadata": {},+ "source": [+ "# load mediapipe definitons"+ ]+ },+ {+ "cell_type": "code",+ "execution_count": null,+ "metadata": {},+ "outputs": [],+ "source": [+ "mediapipe_json = open(r\"C:\\Users\\jonma\\Dropbox\\GitKrakenRepos\\freemocap\\freemocap\\dev_scratchpad\\dev_alpha\\default_json_definitions\\freemocap_mediapipe_json.json\")\n",+ "mediapipe_formatting_dict = json.load(mediapipe_json)\n"+ ]+ },+ {+ "cell_type": "code",+ "execution_count": null,+ "metadata": {},+ "outputs": [],+ "source": [+ "#take a look, if you want!\n",+ "#print(mediapipe_definitions_dict)"+ ]+ },+ {+ "cell_type": "markdown",+ "metadata": {},+ "source": [+ "# get skeleton constraints, based on parent-child relationships defined in the jason\n"+ ]+ },+ {+ "cell_type": "code",+ "execution_count": null,+ "metadata": {},+ "outputs": [],+ "source": [+ "\n"+ ]+ }+ ],+ "metadata": {+ "interpreter": {+ "hash": "82e606d03bb5ea8b8ecb178c4510354cd9c1f96cd94fcd95b722a3ff15cb0c24"+ },+ "kernelspec": {+ "display_name": "Python 3.7.11 64-bit ('fmc43-pip': conda)",+ "language": "python",+ "name": "python3"+ },+ "language_info": {+ "codemirror_mode": {+ "name": "ipython",+ "version": 3+ },+ "file_extension": ".py",+ "mimetype": "text/x-python",+ "name": "python",+ "nbconvert_exporter": "python",+ "pygments_lexer": "ipython3",+ "version": "3.7.12"+ },+ "orig_nbformat": 4+ },+ "nbformat": 4,+ "nbformat_minor": 2+}diff --git a/freemocap/dev_scratchpad/scratch_load_npy.ipynb b/freemocap/dev_scratchpad/scratch_load_npy.ipynbdeleted file mode 100644index e69de29b..00000000diff --git a/freemocap/dev_scratchpad/simple_camera.py b/freemocap/dev_scratchpad/simple_camera.pyindex ebcb0a9b..db2127b5 100644--- a/freemocap/dev_scratchpad/simple_camera.py+++ b/freemocap/dev_scratchpad/simple_camera.py@@ -1,12 +1,12 @@import cv2-# cap = cv2.VideoCapture(0, cv2.CAP_DSHOW)-cap = cv2.VideoCapture(2, cv2.CAP_ANY)+cap = cv2.VideoCapture(0, cv2.CAP_DSHOW)+# cap = cv2.VideoCapture(2, cv2.CAP_ANY)cap.set(cv2.CAP_PROP_EXPOSURE, -7)-cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1280)-cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 720)+cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1920)+cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 10800)cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc('M', 'J', 'P', 'G'))diff --git a/freemocap/fmc_anipose.py b/freemocap/fmc_anipose.pyindex 2dee34aa..1ebfc128 100644--- a/freemocap/fmc_anipose.py+++ b/freemocap/fmc_anipose.py@@ -1,7 +1,3 @@--#NOTE - this is altered version of code from Anipose (`aniposelib/cameras.py`) - We should make this alteration in a forked repo of aniposelib rather than copying the py file and altering it in our repo--import cv2import numpy as npfrom copy import copy@@ -16,14 +12,10 @@ from tqdm import trangefrom pprint import pprintimport 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-+from .boards import merge_rows, extract_points, \+ extract_rtvecs, get_video_params+from .utils import get_initial_extrinsics, make_M, get_rtvec, \+ get_connections@jit(nopython=True, parallel=True)def triangulate_simple(points, camera_mats):@@ -32,8 +24,8 @@ def triangulate_simple(points, camera_mats):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]+ 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]@@ -49,7 +41,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)@@ -59,50 +51,45 @@ 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 Nonenew_extra = {- "objp": extra["objp"][ixs],- "ids": extra["ids"][ixs],- "rvecs": extra["rvecs"][:, ixs],- "tvecs": extra["tvecs"][:, ixs],+ '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+ ids = remap_ids(extra['ids'])+ n_ids = np.max(ids)+1good = ~np.isnan(imgp[:, :, 0])ixs = np.arange(n_points)- cam_counts = np.zeros((n_ids, n_cams), dtype="int64")+ 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")+ 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- ):+ if cam_totals[cam_num] >= n_samp or \+ cam_counts_random[board_id, cam_num] < 1:breakfinal_ixs = sorted(include)@@ -110,7 +97,6 @@ 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)@@ -124,12 +110,12 @@ 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:## pick points, prioritizing points seen by more cameras- arr = np.copy(num_cams[subset]).astype("float64")+ 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]@@ -140,18 +126,15 @@ 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- vpad = np.pad(values, (padsize, padsize), mode="reflect")+ 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)@@ -161,7 +144,6 @@ def interpolate_data(vals):out[:] = 0return out-def remap_ids(ids):unique_ids = np.unique(ids)ids_out = np.copy(ids)@@ -169,37 +151,34 @@ def remap_ids(ids):ids_out[ids == num] = ireturn ids_out-def transform_points(points, rvecs, tvecs):"""Rotate points by given rotation vectors and translate.Rodrigues' rotation formula is used."""theta = np.linalg.norm(rvecs, axis=1)[:, np.newaxis]- with np.errstate(invalid="ignore"):+ with np.errstate(invalid='ignore'):v = rvecs / thetav = 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- )+ rotated = cos_theta * points + \+ sin_theta * np.cross(v, points) + \+ dot * (1 - cos_theta) * vreturn rotated + tvecsclass 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,- ):+ 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)@@ -211,21 +190,21 @@ class Camera: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(),+ '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"])+ 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()@@ -239,7 +218,7 @@ class Camera:return self.distdef set_camera_matrix(self, matrix):- self.matrix = np.array(matrix, dtype="float64")+ self.matrix = np.array(matrix, dtype='float64')def set_focal_length(self, fx, fy=None):if fy is None:@@ -256,16 +235,16 @@ class Camera:return (fx + fy) / 2.0def set_distortions(self, dist):- self.dist = np.array(dist, dtype="float64").ravel()+ self.dist = np.array(dist, dtype='float64').ravel()def set_rotation(self, rvec):- self.rvec = np.array(rvec, dtype="float64").ravel()+ self.rvec = np.array(rvec, dtype='float64').ravel()def get_rotation(self):return self.rvecdef set_translation(self, tvec):- self.tvec = np.array(tvec, dtype="float64").ravel()+ self.tvec = np.array(tvec, dtype='float64').ravel()def get_translation(self):return self.tvec@@ -298,7 +277,7 @@ class Camera:self.set_camera_matrix(new_matrix)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()@@ -313,7 +292,7 @@ class Camera:self.set_translation(params[3:6])self.set_focal_length(params[6])- dist = np.zeros(5, dtype="float64")+ dist = np.zeros(5, dtype='float64')dist[0] = params[7]if self.extra_dist:dist[1] = params[8]@@ -323,32 +302,24 @@ class Camera:shape = points.shapepoints = 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"),- )+ 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.shapepoints = points.reshape(-1, 1, 2)- out = cv2.undistortPoints(- points, self.matrix.astype("float64"), self.dist.astype("float64")- )+ 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"),- )+ out, _ = cv2.projectPoints(points, self.rvec, self.tvec,+ self.matrix.astype('float64'),+ self.dist.astype('float64'))return outdef reprojection_error(self, p3d, p2d):@@ -356,28 +327,24 @@ class Camera:return p2d - projdef 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,- )-+ 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,- ):+ 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)@@ -393,41 +360,33 @@ class FisheyeCamera(Camera):def get_dict(self):d = super().get_dict()- d["fisheye"] = True+ d['fisheye'] = Truereturn ddef distort_points(self, points):shape = points.shapepoints = 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"),- )+ 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.shapepoints = points.reshape(-1, 1, 2)- out = cv2.fisheye.undistortPoints(- points.astype("float64"),- self.matrix.astype("float64"),- self.dist.astype("float64"),- )+ 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"),- )+ out, _ = cv2.fisheye.projectPoints(points,+ self.rvec, self.tvec,+ self.matrix.astype('float64'),+ self.dist.astype('float64'))return outdef set_params(self, params):@@ -435,7 +394,7 @@ class FisheyeCamera(Camera):self.set_translation(params[3:6])self.set_focal_length(params[6])- dist = np.zeros(4, dtype="float64")+ dist = np.zeros(4, dtype='float64')dist[0] = params[7]if self.extra_dist:dist[1] = params[8]@@ -444,7 +403,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()@@ -464,9 +423,7 @@ class FisheyeCamera(Camera):rvec=self.get_rotation().copy(),tvec=self.get_translation().copy(),name=self.get_name(),- extra_dist=self.extra_dist,- )-+ extra_dist=self.extra_dist)class CameraGroup:def __init__(self, cameras, metadata={}):@@ -484,8 +441,9 @@ class CameraGroup:for name in names:if name not in cur_names_dict:raise IndexError(- "name {} not part of camera names: {}".format(name, cur_names)- )+ "name {} not part of camera names: {}".format(+ name, cur_names+ ))indices.append(cur_names_dict[name])return self.subset_cameras(indices)@@ -496,7 +454,7 @@ class CameraGroup:n_points = points.shape[0]n_cams = len(self.cameras)- out = np.empty((n_cams, n_points, 2), dtype="float64")+ 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)@@ -506,12 +464,11 @@ class CameraGroup:"""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"+ 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 = Falseif len(points.shape) == 2:@@ -542,7 +499,6 @@ class CameraGroup: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:@@ -550,9 +506,8 @@ class CameraGroup:return out- def triangulate_possible(- self, points, undistort=True, min_cams=2, progress=False, threshold=0.5- ):+ 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 pointsby triangulating all possible points and picking the ones withbest reprojection error@@ -562,22 +517,21 @@ class CameraGroup:P: number of possible options per point"""- assert points.shape[0] == len(self.cameras), (- "Invalid points shape, first dim should be equal to"+ 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]))+ 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- ):+ 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))@@ -586,10 +540,10 @@ class CameraGroup: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")+ 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)@@ -618,24 +572,24 @@ class CameraGroup:if err < best_error:best_point = {- "error": err,- "point": p3d[:3],- "points": pts,- "picked": picked,- "joint_ix": point_ix,+ 'error': err,+ 'point': p3d[:3],+ 'points': pts,+ 'picked': picked,+ 'joint_ix': point_ix}best_error = errif best_error < threshold:breakif best_point is not None:- out[point_ix] = best_point["point"]- picked = best_point["picked"]+ 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"]+ errors[point_ix] = best_point['error']+ points_2d[cnums, point_ix] = best_point['points']return out, picked_vals, points_2d, errors@@ -643,20 +597,21 @@ class CameraGroup:"""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"+ 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.shapepoints_ransac = points.reshape(n_cams, n_points, 1, 2)- return self.triangulate_possible(- points_ransac, undistort=undistort, min_cams=min_cams, progress=progress- )+ 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):@@ -672,12 +627,9 @@ class CameraGroup:one_point = Truen_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- )+ 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))@@ -688,7 +640,7 @@ class CameraGroup: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 = np.sum(good, axis=0).astype('float64')denom[denom < 1.5] = np.nanerrors = np.sum(errors_norm, axis=0) / denom@@ -700,20 +652,13 @@ 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,- n_samp_iter=100,- n_samp_full=1000,- error_threshold=0.3,- verbose=False,- ):++ 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.@@ -722,29 +667,30 @@ class CameraGroup:This is inspired by the algorithm for Fast Global Registration by Zhou, Park, and Koltun"""- assert p2ds.shape[0] == len(self.cameras), (- "Invalid points shape, first dim should be equal to"+ 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 = p2dsextra_full = extra- p2ds, extra = resample_points(p2ds_full, extra_full, n_samp=n_samp_full)+ 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)+ 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))+ 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)+ 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)@@ -761,8 +707,7 @@ class CameraGroup:good = errors_norm < muextra_good = subset_extra(extra, good)p2ds_samp, extra_samp = resample_points(- p2ds[:, good], extra_good, n_samp=n_samp_iter- )+ p2ds[:, good], extra_good, n_samp=n_samp_iter)error = np.median(errors_norm)@@ -771,22 +716,16 @@ class CameraGroup:if verbose:pprint(error_dict)- print(- "error: {:.2f}, mu: {:.1f}, ratio: {:.3f}".format(- error, mu, np.mean(good)- )- )-- self.bundle_adjust(- p2ds_samp,- extra_samp,- loss="linear",- ftol=ftol,- max_nfev=max_nfev,- verbose=verbose,- )+ print('error: {:.2f}, mu: {:.1f}, ratio: {:.3f}'.format(error, mu, np.mean(good)))++ 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)+ 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)@@ -804,14 +743,10 @@ class CameraGroup:good = errors_norm < muextra_good = subset_extra(extra, good)- self.bundle_adjust(- p2ds[:, good],- extra_good,- loss="linear",- ftol=ftol,- max_nfev=max(200, max_nfev),- verbose=verbose,- )+ 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)@@ -822,35 +757,30 @@ class CameraGroup:pprint(error_dict)if verbose:- print("error: ", error)+ 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,- ):+ 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"+ 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"])+ extra['ids_map'] = remap_ids(extra['ids'])x0, n_cam_params = self._initialize_params_bundle(p2ds, extra)@@ -863,20 +793,18 @@ class CameraGroup: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),- )+ 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.xfor i, cam in enumerate(self.cameras):@@ -902,18 +830,18 @@ class CameraGroup:n_cams = len(self.cameras)sub = n_cam_params * n_camsn3d = p2ds.shape[1] * 3- p3ds_test = params[sub : sub + n3d].reshape(-1, 3)+ p3ds_test = params[sub:sub+n3d].reshape(-1, 3)errors = self.reprojection_error(p3ds_test, p2ds)errors_reproj = errors[good]if extra is not None:- ids = extra["ids_map"]- objp = extra["objp"]+ 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)+ a = sub+n3d+ rvecs = params[a:a+n_boards*3].reshape(-1, 3)+ tvecs = params[a+n_boards*3:a+n_boards*6].reshape(-1, 3)expected = transform_points(objp, rvecs[ids], tvecs[ids])errors_obj = 2 * (p3ds_test - expected).ravel() / min_scaleelse:@@ -921,13 +849,14 @@ 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,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")+ 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@@ -938,9 +867,9 @@ class CameraGroup:good = ~np.isnan(p2ds)if extra is not None:- ids = extra["ids_map"]+ ids = extra['ids_map']n_boards = int(np.max(ids)) + 1- total_board_params = n_boards * (3 + 3) # rvecs + tvecs+ total_board_params = n_boards * (3 + 3) # rvecs + tvecselse:n_boards = 0total_board_params = 0@@ -956,7 +885,7 @@ class CameraGroup:else:n_errors = n_good_values- A_sparse = dok_matrix((n_errors, n_params), dtype="int16")+ A_sparse = dok_matrix((n_errors, n_params), dtype='int16')cam_indices_good = cam_indices[good]point_indices_good = point_indices[good]@@ -983,21 +912,17 @@ class CameraGroup:## update board rotation and translation based on error from expectedfor i in range(3):for j in range(3):- A_sparse[- n_good_values + point_ix * 3 + i,- total_params_reproj + ids * 3 + j,- ] = 1- A_sparse[- n_good_values + point_ix * 3 + i,- total_params_reproj + n_boards * 3 + ids * 3 + j,- ] = 1+ A_sparse[n_good_values + point_ix*3 + i,+ total_params_reproj + ids*3 + j] = 1+ A_sparse[n_good_values + point_ix*3 + i,+ total_params_reproj + n_boards*3 + ids*3 + j] = 1+## update point position based on error from expectedfor i in range(3):- A_sparse[- n_good_values + point_ix * 3 + i,- n_cams * n_cam_params + point_ix * 3 + i,- ] = 1+ A_sparse[n_good_values + point_ix*3 + i,+ n_cams*n_cam_params + point_ix*3 + i] = 1+return A_sparse@@ -1012,67 +937,59 @@ class CameraGroup: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 "+ 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"]+ 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")- tvecs = np.zeros((n_boards, 3), dtype="float64")+ 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"]+ 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_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] = rvectvecs[board_num] = tvec+else:total_board_params = 0x0 = 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] = tvecs.ravel()+ 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,- ):+ 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,@@ -1089,12 +1006,11 @@ class CameraGroup:(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"+ 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.shapeconstraints = np.array(constraints)@@ -1104,66 +1020,52 @@ class CameraGroup:p3ds_med = np.apply_along_axis(medfilt_data, 0, p3ds_intp, size=7)- default_smooth = 1.0 / np.mean(np.abs(np.diff(p3ds_med, axis=0)))+ default_smooth = 1.0/np.mean(np.abs(np.diff(p3ds_med, axis=0)))scale_smooth_full = scale_smooth * default_smootht1 = time.time()x0 = self._initialize_params_triangulation(- p3ds_intp, constraints, constraints_weak- )+ p3ds_intp, constraints, constraints_weak)x0[~np.isfinite(x0)] = 0jac = 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)+ 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))+ 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,- ):++ 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,@@ -1181,12 +1083,11 @@ class CameraGroup:(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"+ 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.shapeconstraints = np.array(constraints)@@ -1196,60 +1097,50 @@ class CameraGroup:p3ds_med = np.apply_along_axis(medfilt_data, 0, p3ds_intp, size=7)- default_smooth = 1.0 / np.mean(np.abs(np.diff(p3ds_med, axis=0)))+ default_smooth = 1.0/np.mean(np.abs(np.diff(p3ds_med, axis=0)))scale_smooth_full = scale_smooth * default_smootht1 = time.time()x0 = self._initialize_params_triangulation_possible(- p3ds_intp,- points,- constraints=constraints,- constraints_weak=constraints_weak,- )+ p3ds_intp, points, constraints=constraints, constraints_weak=constraints_weak)- print("getting jacobian...")+ print('getting jacobian...')jac = self._jac_sparsity_triangulation_possible(points,constraints=constraints,constraints_weak=constraints_weak,- n_deriv_smooth=n_deriv_smooth,- )+ 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,- ),- )+ 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)+ 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 = np.zeros((n_cams, n_frames, n_joints, n_possible), dtype='float64')alphas[~bad] = params[n_params_norm:]alphas_exp = np.exp(beta * alphas)@@ -1262,13 +1153,13 @@ class CameraGroup:t2 = time.time()if verbose:- print("optimization took {:.2f} seconds".format(t2 - t1))+ 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- ):++ 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, whereC: number of camera@@ -1284,22 +1175,19 @@ class CameraGroup:"""- assert points.shape[0] == len(self.cameras), (- "Invalid points shape, first dim should be equal to"+ 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)+ 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- )+ 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)@@ -1312,31 +1200,29 @@ class CameraGroup: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,- ):+ 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_3d = n_frames*n_joints*3n_constraints = len(constraints)n_constraints_weak = len(constraints_weak)# load paramsp3ds = params[:n_3d].reshape((n_frames, n_joints, 3))- joint_lengths = np.array(params[n_3d : n_3d + n_constraints])- joint_lengths_weak = np.array(params[n_3d + n_constraints :])+ joint_lengths = np.array(params[n_3d:n_3d+n_constraints])+ joint_lengths_weak = np.array(params[n_3d+n_constraints:])# reprojection errorsp3ds_flat = p3ds.reshape(-1, 3)@@ -1349,39 +1235,40 @@ class CameraGroup:rp = reproj_error_thresholderrors_reproj = np.abs(errors_reproj)- if reproj_loss == "huber":+ if reproj_loss == 'huber':bad = errors_reproj > rp- errors_reproj[bad] = rp * (2 * np.sqrt(errors_reproj[bad] / rp) - 1)- elif reproj_loss == "linear":+ 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)+ elif reproj_loss == 'soft_l1':+ errors_reproj = rp*2*(np.sqrt(1+errors_reproj/rp)-1)# temporal constrainterrors_smooth = np.diff(p3ds, n=n_deriv_smooth, axis=0).ravel() * scale_smooth# joint length constraint- errors_lengths = np.empty((n_constraints, n_frames), dtype="float64")+ 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[cix] = 100*(lengths - expected)/expected+ errors_lengths = errors_lengths.ravel() * scale_l0ength- errors_lengths_weak = np.empty((n_constraints_weak, n_frames), dtype="float64")+ 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)/expectederrors_lengths_weak = errors_lengths_weak.ravel() * scale_length_weak- return np.hstack(- [errors_reproj, errors_smooth, errors_lengths, errors_lengths_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- ):+ 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@@ -1390,16 +1277,16 @@ class CameraGroup:n_cams, n_frames, n_joints, n_possible, _ = p2ds.shape- n_3d = n_frames * n_joints * 3+ n_3d = n_frames*n_joints*3n_constraints = len(constraints)n_constraints_weak = len(constraints_weak)- n_params_norm = n_3d + n_constraints + n_constraints_weak+ n_params_norm = n_3d+n_constraints+n_constraints_weak# load paramsbad = np.isnan(p2ds[:, :, :, :, 0])all_bad = np.all(bad, axis=3)- alphas = np.zeros((n_cams, n_frames, n_joints, n_possible), dtype="float64")+ 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])@@ -1416,25 +1303,26 @@ class CameraGroup: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- )+ 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)) * 10return 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")++ 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)@@ -1448,8 +1336,8 @@ class CameraGroup:joint_lengths[joint_lengths == 0] = medjoint_lengths_weak[joint_lengths_weak == 0] = med- joint_lengths[joint_lengths > med + mad * 5] = med- joint_lengths_weak[joint_lengths_weak > med + mad * 5] = med+ joint_lengths[joint_lengths > med+mad*5] = med+ joint_lengths_weak[joint_lengths_weak > med+mad*5] = medreturn np.hstack([p3ds.ravel(), joint_lengths, joint_lengths_weak])@@ -1460,7 +1348,7 @@ class CameraGroup:n_cams, n_frames, n_joints, n_possible, _ = p2ds.shapegood = ~np.isnan(p2ds[:, :, :, :, 0])- alphas = np.zeros((n_cams, n_frames, n_joints, n_possible), dtype="float64")+ alphas = np.zeros((n_cams, n_frames, n_joints, n_possible), dtype='float64')alphas[:, :, :, 0] = 0params = self._initialize_params_triangulation(p3ds, **kwargs)@@ -1468,37 +1356,38 @@ class CameraGroup:return params_full- def _jac_sparsity_triangulation(- self, p2ds, constraints=[], constraints_weak=[], n_deriv_smooth=1- ):+ def _jac_sparsity_triangulation(self, p2ds,+ constraints=[],+ constraints_weak=[],+ n_deriv_smooth=1):n_cams, n_frames, n_joints, _ = p2ds.shapen_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")+ 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))+ point_indices_3d = np.arange(n_frames*n_joints)\+ .reshape((n_frames, n_joints))good = ~np.isnan(p2ds_flat)n_errors_reproj = np.sum(good)- n_errors_smooth = (n_frames - n_deriv_smooth) * n_joints * 3+ n_errors_smooth = (n_frames-n_deriv_smooth) * n_joints * 3n_errors_lengths = n_constraints * n_framesn_errors_lengths_weak = n_constraints_weak * n_frames- n_errors = (- n_errors_reproj + n_errors_smooth + n_errors_lengths + n_errors_lengths_weak- )+ n_errors = n_errors_reproj + n_errors_smooth + \+ n_errors_lengths + n_errors_lengths_weak- n_3d = n_frames * n_joints * 3+ n_3d = n_frames*n_joints*3n_params = n_3d + n_constraints + n_constraints_weakpoint_indices_good = point_indices[good]- A_sparse = dok_matrix((n_errors, n_params), dtype="int16")+ A_sparse = dok_matrix((n_errors, n_params), dtype='int16')# constraints for reprojection errorsix_reproj = np.arange(n_errors_reproj)@@ -1506,20 +1395,20 @@ class CameraGroup:A_sparse[ix_reproj, point_indices_good * 3 + k] = 1# sparse constraints for smoothness in time- frames = np.arange(n_frames - n_deriv_smooth)+ frames = np.arange(n_frames-n_deriv_smooth)for j in range(n_joints):- for n in range(n_deriv_smooth + 1):+ for n in range(n_deriv_smooth+1):pa = point_indices_3d[frames, j]- pb = point_indices_3d[frames + n, j]+ pb = point_indices_3d[frames+n, j]for k in range(3):- A_sparse[n_errors_reproj + pa * 3 + k, pb * 3 + k] = 1+ A_sparse[n_errors_reproj + pa*3 + k, pb*3 + k] = 1## -- strong constraints --# joint lengths should change with joint lengths errorsstart = n_errors_reproj + n_errors_smoothframes = np.arange(n_frames)for cix, (a, b) in enumerate(constraints):- A_sparse[start + cix * n_frames + frames, n_3d + cix] = 1+ A_sparse[start + cix*n_frames + frames, n_3d+cix] = 1# points should change accordingly to match joint lengths tooframes = np.arange(n_frames)@@ -1527,15 +1416,15 @@ class CameraGroup:pa = point_indices_3d[frames, a]pb = point_indices_3d[frames, b]for k in range(3):- A_sparse[start + cix * n_frames + frames, pa * 3 + k] = 1- A_sparse[start + cix * n_frames + frames, pb * 3 + k] = 1+ A_sparse[start + cix*n_frames + frames, pa*3 + k] = 1+ A_sparse[start + cix*n_frames + frames, pb*3 + k] = 1## -- weak constraints --# joint lengths should change with joint lengths errorsstart = n_errors_reproj + n_errors_smooth + n_errors_lengthsframes = np.arange(n_frames)for cix, (a, b) in enumerate(constraints_weak):- A_sparse[start + cix * n_frames + frames, n_3d + n_constraints + cix] = 1+ A_sparse[start + cix*n_frames + frames, n_3d + n_constraints + cix] = 1# points should change accordingly to match joint lengths tooframes = np.arange(n_frames)@@ -1543,8 +1432,8 @@ class CameraGroup:pa = point_indices_3d[frames, a]pb = point_indices_3d[frames, b]for k in range(3):- A_sparse[start + cix * n_frames + frames, pa * 3 + k] = 1- A_sparse[start + cix * n_frames + frames, pb * 3 + k] = 1+ A_sparse[start + cix*n_frames + frames, pa*3 + k] = 1+ A_sparse[start + cix*n_frames + frames, pb*3 + k] = 1return A_sparse@@ -1565,22 +1454,17 @@ class CameraGroup:n_errors, n_params = A_sparse.shape- B_sparse = dok_matrix(- (n_errors + n_errors_alphas, n_params + n_alphas), dtype="int16"- )+ 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 = 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"- )+ 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@@ -1592,7 +1476,8 @@ class CameraGroup: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+ B_sparse[point_indices_2d_good_find[alpha_index],+ n_params + ix] = 1# alphas should change according to the alpha errorspoint_indices_good_find = dict()@@ -1648,34 +1533,21 @@ class CameraGroup:else:return np.mean(errors)- def calibrate_rows(- self,- all_rows,- board,- init_intrinsics=True,- init_extrinsics=True,- verbose=True,- **kwargs- ):-- all_rowsOG = all_rows.copy()+ 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"- """Assumes camera sizes are set properly"""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()- )+ 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) > 5]-- assert len(objp) != 0 and len(imgp) != 0, "No Charuco board points detected"-+ mixed = [(o, i) for (o, i) in zip(objp, imgp) if len(o) >= 7]objp, imgp = zip(*mixed)matrix = cv2.initCameraMatrix2D(objp, imgp, tuple(size))camera.set_camera_matrix(matrix)@@ -1683,23 +1555,20 @@ class CameraGroup: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)-+ pprint(get_connections(rtvecs, self.get_names()))+ rvecs, tvecs = get_initial_extrinsics(rtvecs, self.get_names())+ self.set_rotations(rvecs)+ self.set_translations(tvecs)error = self.bundle_adjust_iter(imgp, extra, verbose=verbose, **kwargs)- return error, merged, charuco_frames+ return errordef get_rows_videos(self, videos, board, verbose=True):all_rows = []@@ -1707,11 +1576,9 @@ class CameraGroup:for cix, (cam, cam_videos) in enumerate(zip(self.cameras, videos)):rows_cam = []for vnum, vidname in enumerate(cam_videos):- if verbose:- print(vidname)+ if verbose: print(vidname)rows = board.detect_video(vidname, prefix=vnum, progress=verbose)- if verbose:- print("{} boards detected".format(len(rows)))+ if verbose: print("{} boards detected".format(len(rows)))rows_cam.extend(rows)all_rows.append(rows_cam)@@ -1722,36 +1589,24 @@ class CameraGroup:rows_cam = []for vnum, vidname in enumerate(cam_videos):params = get_video_params(vidname)- size = (params["width"], params["height"])+ size = (params['width'], params['height'])cam.set_size(size)- def calibrate_videos(- self,- videos,- board,- init_intrinsics=True,- init_extrinsics=True,- verbose=True,- **kwargs- ):+ 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"""-- print('Detecting Charuco boards in each video...')- all_rows = self.get_rows_videos(videos, board, verbose=True)+ 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+ error = self.calibrate_rows(all_rows, board,+ init_intrinsics=init_intrinsics,+ init_extrinsics=init_extrinsics,+ verbose=verbose, **kwargs)+ return error, all_rowsdef get_dicts(self):out = []@@ -1762,7 +1617,7 @@ class CameraGroup:def from_dicts(arr):cameras = []for d in arr:- if "fisheye" in d and d["fisheye"]:+ if 'fisheye' in d and d['fisheye']:cam = FisheyeCamera.from_dict(d)else:cam = Camera.from_dict(d)@@ -1785,19 +1640,19 @@ class CameraGroup:def dump(self, fname):dicts = self.get_dicts()- names = ["cam_{}".format(i) for i in range(len(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:+ 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"]+ 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"]+ if 'metadata' in master_dict:+ cgroup.metadata = master_dict['metadata']return cgroupdef resize_cameras(self, scale):diff --git a/freemocap/fmc_blender.py b/freemocap/fmc_blender.pyindex eae0c2fe..7824c848 100644--- a/freemocap/fmc_blender.py+++ b/freemocap/fmc_blender.py@@ -337,7 +337,7 @@ def my_handler(scene):# col[1] = 0.0# if math.isnan(col[2]):# col[2] = 0.0- coord = Vector(((float(col[0])*0.001), (float(col[2])*0.001), (float(col[1]))* -0.001))+ coord = Vector(((float(col[0])*0.001), (float(col[2])*0.001), (float(col[1]))* 0.001))if len(order_of_markers) > 0:empty = order_of_markers[current_marker]empty.location = coorddiff --git a/freemocap_blender_addon/freemocap_blender_addon/load_freemocap_data_as_empties_script.py b/freemocap_blender_addon/freemocap_blender_addon/load_freemocap_data_as_empties_script.pynew file mode 100644index 00000000..0ddbe7ce--- /dev/null+++ b/freemocap_blender_addon/freemocap_blender_addon/load_freemocap_data_as_empties_script.py@@ -0,0 +1,8 @@+import numpy as np+import bpy++from pathlib import Path++from rich import print, inspect++diff --git a/freemocap_blender_addon/load_freemocap_data_into_blender.py b/freemocap_blender_addon/load_freemocap_data_into_blender.pynew file mode 100644index 00000000..f52a836e--- /dev/null+++ b/freemocap_blender_addon/load_freemocap_data_into_blender.py@@ -0,0 +1,123 @@+import numpy as np+import bpy++from pathlib import Path+import json+from mathutils import Vector++from rich import print, inspect+from rich.progress import track+from rich.console import Console+console = Console()++console.rule()+console.rule()+console.rule('Hello! getting started with FreeMoCap Loading Stuff :D')+console.rule()++#load mediapipe 3d skeleton data++sessionID = "sesh_2021-12-30_13_57_13_cleanish"++freemocap_data_path = Path("C:/Users/jonma/Dropbox/FreeMoCapProject/FreeMocap_Data")+session_path = freemocap_data_path / sessionID+data_path = session_path / "DataArrays"+mediapipe_skel_data_path = data_path / "mediaPipeSkel_3d_smoothed.npy"++console.rule(style = "color({})".format("blue"))+console.print("Loading in data from FreeMoCap SessionID: " + sessionID)+console.rule(style = "color({})".format("blue"))++mediapipe_skel_fr_mar_dim = np.load(str(mediapipe_skel_data_path))+mediapipe_skel_fr_mar_dim = mediapipe_skel_fr_mar_dim/1000 #convert from mm to meters++number_of_frames, number_of_tracked_points, number_of_dimensions = mediapipe_skel_fr_mar_dim.shape+#load default mediapipe layout definitions++mediapipe_definitions_json_path = Path(r"C:/Users/jonma/Dropbox/GitKrakenRepos/freemocap/freemocap/dev_scratchpad/dev_alpha/default_json_definitions/freemocap_mediapipe_json.json")++with open(mediapipe_definitions_json_path ) as json_file:+ mediapipe_definitions_dict = json.load(json_file)++tracked_point_names = mediapipe_definitions_dict["body"]["tracked_point_names"]++bpy.ops.object.empty_add(type='ARROWS',location=(0,0,0))++#plop empties at origin with names from mediapipe+empty_scale_factor = .05++for this_tracked_point_number in range(len(tracked_point_names)):+ bpy.ops.object.add(type='EMPTY')+ this_empty = bpy.context.active_object+ this_empty = bpy.context.active_object+ this_empty.name = tracked_point_names[this_tracked_point_number]+ this_empty.scale = (empty_scale_factor,empty_scale_factor,empty_scale_factor)+++bpy.context.scene.unit_settings.length_unit = 'METERS'+bpy.ops.object.mode_set(mode='OBJECT')+++for frame_number in track(range(number_of_frames), description="[cyan]Loading tracked point trajectories as keyframed empties...", total=mediapipe_skel_fr_mar_dim.shape[0] ):+ bpy.context.scene.frame_set(frame_number)+ for this_empty_name in tracked_point_names:+ this_empty = bpy.data.objects[this_empty_name]+ this_empty.location = (mediapipe_skel_fr_mar_dim[frame_number,tracked_point_names.index(this_empty_name),0],+ mediapipe_skel_fr_mar_dim[frame_number,tracked_point_names.index(this_empty_name),1],+ mediapipe_skel_fr_mar_dim[frame_number,tracked_point_names.index(this_empty_name),2])+ this_empty.keyframe_insert(data_path='location', frame=frame_number)++++####################+### There are now EMPTIES in the scene!+####################+console.rule(style = "color({})".format("magenta"))+console.print("[magenta] Generating Meta Rig \o/")+console.rule(style = "color({})".format("magenta"))++bpy.ops.object.armature_human_metarig_add()+metarig = bpy.context.active_object++metarig_scale = .85+metarig.scale = (metarig_scale,metarig_scale,metarig_scale)+++bpy.ops.pose.rigify_generate()+console.rule(style = "color({})".format("magenta"))++console.rule(style = "color({})".format("yellow"))+console.print("[yellow] Driving limb FK whosits with empty location data")+console.rule(style = "color({})".format("yellow"))+++## if not nan+## 1) copy_location - empty -> limb_FK & limb_IK(?)+## 2) set bpy.context.object.pose.bones["upper_arm_parent.L"].IK_FK = 1 (will later set this via reproj error or something)+## 3) add keyframes after each step with bpy.ops.anim.keyframe_insert_button(all=True)+## if nan+## 1) i think just skip? later we can interpolate IK/FK and whatnot+## on first frame+## set all IK stretch to 0+#+#for frame_number in track(range(number_of_frames), description="[yellow]Driving rig FK controllers with empty locations...", total=mediapipe_skel_fr_mar_dim.shape[0] ):+# bpy.context.scene.frame_set(frame_number)+# for this_empty_number, this_empty_name in enumerate(tracked_point_names):+#+# this_point_connections_dict = mediapipe_definitions_dict["body"]['tracked_point_to_rigify_fk_rig_bone_connections'][this_empty_name]+#+# this_empty_location = Vector(mediapipe_skel_fr_mar_dim[frame_number,this_empty_number,:])+#+# if this_point_connections_dict:+# for this_bone_name, this_bone_connection_dict in this_point_connections_dict.items():+#+# bpy.ops.object.mode_set(mode='EDIT')+# if this_bone_connection_dict["track_type"] == "bone_tail":+# if this_bone_name == "head" or this_bone_name == "toe":+# this_bone_obj = bpy.data.objects["rig"].data.bones[this_bone_name]+## this_bone_obj.tail = this_empty_location+# inspect(this_bone_obj, methods=True)+# break+#+#+#