should work on cameras with distortions; still needs to be fully tested
This commit is contained in:
davidpagnon
parent
7e35ce2327
commit
76c39fcafd
@ -128,6 +128,7 @@ interpolation = 'cubic' #linear, slinear, quadratic, cubic, or none
|
||||
interp_if_gap_smaller_than = 10 # do not interpolate bigger gaps
|
||||
show_interp_indices = true # true or false (lowercase). For each keypoint, return the frames that need to be interpolated
|
||||
handle_LR_swap = true # Better if few cameras (eg less than 4) with risk of limb swapping (eg camera facing sagittal plane), otherwise slightly less accurate and slower
|
||||
undistort_points = true # Better if distorted image (parallel lines curvy on the edge or at least one param > 10^-2), but unnecessary (and slightly slower) if distortions are low
|
||||
make_c3d = false # save triangulated data in c3d format in addition to trc
|
||||
|
||||
|
||||
|
||||
@ -0,0 +1,39 @@
|
||||
[cam_01]
|
||||
name = "cam_01"
|
||||
size = [ 1088.0, 1920.0]
|
||||
matrix = [ [ 1681.244873046875, 0.0, 532.97369384375], [ 0.0, 1681.075439453125, 948.137390140625], [ 0.0, 0.0, 1.0]]
|
||||
distortions = [ -0.000721609375, 0.002187234375, 9.5e-06, 1.078125e-05]
|
||||
rotation = [ 1.6882754799999993, 1.0483220499999997, -0.41955852000000016]
|
||||
translation = [ 0.3211048899999996, 0.9563320600000009, 2.8907130499999996]
|
||||
fisheye = false
|
||||
|
||||
[cam_02]
|
||||
name = "cam_02"
|
||||
size = [ 1088.0, 1920.0]
|
||||
matrix = [ [ 1673.729614265625, 0.0, 534.494567875], [ 0.0, 1673.79724121875, 963.225891109375], [ 0.0, 0.0, 1.0]]
|
||||
distortions = [ -0.000747609375, 0.00213728125, 1.51875e-05, 4.546875e-06]
|
||||
rotation = [ 1.34975875, 1.5963809099999993, -1.1983285799999999]
|
||||
translation = [ -0.11152829000000017, 0.7766184800000001, 3.0675519599999994]
|
||||
fisheye = false
|
||||
|
||||
[cam_03]
|
||||
name = "cam_03"
|
||||
size = [ 1088.0, 1920.0]
|
||||
matrix = [ [ 1681.598388671875, 0.0, 513.20837403125], [ 0.0, 1681.509887703125, 955.005126953125], [ 0.0, 0.0, 1.0]]
|
||||
distortions = [ -0.000729765625, 0.00215034375, -8.46875e-06, -8.078125e-06]
|
||||
rotation = [ 0.8109654899999995, -2.1972129299999996, 1.3760277799999996]
|
||||
translation = [ -0.7934803899999996, 0.32283594000000126, 4.353514870000001]
|
||||
fisheye = false
|
||||
|
||||
[cam_04]
|
||||
name = "cam_04"
|
||||
size = [ 1088.0, 1920.0]
|
||||
matrix = [ [ 1675.234985359375, 0.0, 540.106201171875], [ 0.0, 1675.204223640625, 964.0302734375], [ 0.0, 0.0, 1.0]]
|
||||
distortions = [ -0.000744265625, 0.002104171875, 4.328125e-06, 3.109375e-06]
|
||||
rotation = [ 1.4045571699999995, -1.3887412699999993, 0.42535743000000026]
|
||||
translation = [ 0.5030217200000007, 0.04894934000000083, 4.406564460000002]
|
||||
fisheye = false
|
||||
|
||||
[metadata]
|
||||
adjusted = false
|
||||
error = 0.0
|
||||
@ -129,6 +129,7 @@
|
||||
# interp_if_gap_smaller_than = 10 # do not interpolate bigger gaps
|
||||
# show_interp_indices = true # true or false (lowercase). For each keypoint, return the frames that need to be interpolated
|
||||
# handle_LR_swap = true # Better if few cameras (eg less than 4) with risk of limb swapping (eg camera facing sagittal plane), otherwise slightly less accurate and slower
|
||||
# undistort_points = true # Better if distorted image (parallel lines curvy on the edge or at least one param > 10^-2), but unnecessary (and slightly slower) if distortions are low
|
||||
# make_c3d = false # save triangulated data in c3d format in addition to trc
|
||||
|
||||
|
||||
|
||||
@ -129,6 +129,7 @@
|
||||
# interp_if_gap_smaller_than = 10 # do not interpolate bigger gaps
|
||||
# show_interp_indices = true # true or false (lowercase). For each keypoint, return the frames that need to be interpolated
|
||||
# handle_LR_swap = true # Better if few cameras (eg less than 4) with risk of limb swapping (eg camera facing sagittal plane), otherwise slightly less accurate and slower
|
||||
# undistort_points = true # Better if distorted image (parallel lines curvy on the edge or at least one param > 10^-2), but unnecessary (and slightly slower) if distortions are low
|
||||
# make_c3d = false # save triangulated data in c3d format in addition to trc
|
||||
|
||||
|
||||
|
||||
@ -129,6 +129,7 @@
|
||||
# interp_if_gap_smaller_than = 10 # do not interpolate bigger gaps
|
||||
# show_interp_indices = true # true or false (lowercase). For each keypoint, return the frames that need to be interpolated
|
||||
# handle_LR_swap = true # Better if few cameras (eg less than 4) with risk of limb swapping (eg camera facing sagittal plane), otherwise slightly less accurate and slower
|
||||
# undistort_points = true # Better if distorted image (parallel lines curvy on the edge or at least one param > 10^-2), but unnecessary (and slightly slower) if distortions are low
|
||||
# make_c3d = false # save triangulated data in c3d format in addition to trc
|
||||
|
||||
|
||||
|
||||
@ -5,12 +5,14 @@ def test_workflow():
|
||||
|
||||
from Pose2Sim import Pose2Sim
|
||||
|
||||
|
||||
# Calibration
|
||||
config_dict = toml.load('Config.toml')
|
||||
config_dict.get("project").update({"project_dir":"."})
|
||||
Pose2Sim.calibration(config_dict)
|
||||
|
||||
# # Static trial
|
||||
|
||||
# Static trial
|
||||
project_dir = os.path.join("S00_P00_Participant","S00_P00_T00_StaticTrial")
|
||||
config_dict.get("project").update({"project_dir":project_dir})
|
||||
config_dict['filtering']['display_figures'] = False
|
||||
|
||||
@ -128,6 +128,7 @@ interpolation = 'cubic' #linear, slinear, quadratic, cubic, or none
|
||||
interp_if_gap_smaller_than = 10 # do not interpolate bigger gaps
|
||||
show_interp_indices = true # true or false (lowercase). For each keypoint, return the frames that need to be interpolated
|
||||
# handle_LR_swap = true # Better if few cameras (eg less than 4) with risk of limb swapping (eg camera facing sagittal plane), otherwise slightly less accurate and slower
|
||||
# undistort_points = true # Better if distorted image (parallel lines curvy on the edge or at least one param > 10^-2), but unnecessary (and slightly slower) if distortions are low
|
||||
# make_c3d = false # save triangulated data in c3d format in addition to trc
|
||||
|
||||
|
||||
|
||||
@ -129,6 +129,7 @@
|
||||
# interp_if_gap_smaller_than = 10 # do not interpolate bigger gaps
|
||||
# show_interp_indices = true # true or false (lowercase). For each keypoint, return the frames that need to be interpolated
|
||||
# handle_LR_swap = true # Better if few cameras (eg less than 4) with risk of limb swapping (eg camera facing sagittal plane), otherwise slightly less accurate and slower
|
||||
# undistort_points = true # Better if distorted image (parallel lines curvy on the edge or at least one param > 10^-2), but unnecessary (and slightly slower) if distortions are low
|
||||
# make_c3d = false # save triangulated data in c3d format in addition to trc
|
||||
|
||||
|
||||
|
||||
@ -129,6 +129,7 @@
|
||||
# interp_if_gap_smaller_than = 10 # do not interpolate bigger gaps
|
||||
# show_interp_indices = true # true or false (lowercase). For each keypoint, return the frames that need to be interpolated
|
||||
# handle_LR_swap = true # Better if few cameras (eg less than 4) with risk of limb swapping (eg camera facing sagittal plane), otherwise slightly less accurate and slower
|
||||
# undistort_points = true # Better if distorted image (parallel lines curvy on the edge or at least one param > 10^-2), but unnecessary (and slightly slower) if distortions are low
|
||||
# make_c3d = false # save triangulated data in c3d format in addition to trc
|
||||
|
||||
|
||||
|
||||
@ -129,6 +129,7 @@
|
||||
# interp_if_gap_smaller_than = 10 # do not interpolate bigger gaps
|
||||
# show_interp_indices = true # true or false (lowercase). For each keypoint, return the frames that need to be interpolated
|
||||
# handle_LR_swap = true # Better if few cameras (eg less than 4) with risk of limb swapping (eg camera facing sagittal plane), otherwise slightly less accurate and slower
|
||||
# undistort_points = true # Better if distorted image (parallel lines curvy on the edge or at least one param > 10^-2), but unnecessary (and slightly slower) if distortions are low
|
||||
# make_c3d = false # save triangulated data in c3d format in addition to trc
|
||||
|
||||
|
||||
|
||||
@ -129,6 +129,7 @@
|
||||
# interp_if_gap_smaller_than = 10 # do not interpolate bigger gaps
|
||||
# show_interp_indices = true # true or false (lowercase). For each keypoint, return the frames that need to be interpolated
|
||||
# handle_LR_swap = true # Better if few cameras (eg less than 4) with risk of limb swapping (eg camera facing sagittal plane), otherwise slightly less accurate and slower
|
||||
# undistort_points = true # Better if distorted image (parallel lines curvy on the edge or at least one param > 10^-2), but unnecessary (and slightly slower) if distortions are low
|
||||
# make_c3d = false # save triangulated data in c3d format in addition to trc
|
||||
|
||||
|
||||
|
||||
@ -129,6 +129,7 @@
|
||||
# interp_if_gap_smaller_than = 10 # do not interpolate bigger gaps
|
||||
# show_interp_indices = true # true or false (lowercase). For each keypoint, return the frames that need to be interpolated
|
||||
# handle_LR_swap = true # Better if few cameras (eg less than 4) with risk of limb swapping (eg camera facing sagittal plane), otherwise slightly less accurate and slower
|
||||
# undistort_points = true # Better if distorted image (parallel lines curvy on the edge or at least one param > 10^-2), but unnecessary (and slightly slower) if distortions are low
|
||||
# make_c3d = false # save triangulated data in c3d format in addition to trc
|
||||
|
||||
|
||||
|
||||
@ -129,6 +129,7 @@
|
||||
# interp_if_gap_smaller_than = 10 # do not interpolate bigger gaps
|
||||
# show_interp_indices = true # true or false (lowercase). For each keypoint, return the frames that need to be interpolated
|
||||
# handle_LR_swap = true # Better if few cameras (eg less than 4) with risk of limb swapping (eg camera facing sagittal plane), otherwise slightly less accurate and slower
|
||||
# undistort_points = true # Better if distorted image (parallel lines curvy on the edge or at least one param > 10^-2), but unnecessary (and slightly slower) if distortions are low
|
||||
# make_c3d = false # save triangulated data in c3d format in addition to trc
|
||||
|
||||
|
||||
|
||||
@ -129,6 +129,7 @@
|
||||
# interp_if_gap_smaller_than = 10 # do not interpolate bigger gaps
|
||||
# show_interp_indices = true # true or false (lowercase). For each keypoint, return the frames that need to be interpolated
|
||||
# handle_LR_swap = true # Better if few cameras (eg less than 4) with risk of limb swapping (eg camera facing sagittal plane), otherwise slightly less accurate and slower
|
||||
# undistort_points = true # Better if distorted image (parallel lines curvy on the edge or at least one param > 10^-2), but unnecessary (and slightly slower) if distortions are low
|
||||
# make_c3d = false # save triangulated data in c3d format in addition to trc
|
||||
|
||||
|
||||
|
||||
@ -129,6 +129,7 @@
|
||||
# interp_if_gap_smaller_than = 10 # do not interpolate bigger gaps
|
||||
# show_interp_indices = true # true or false (lowercase). For each keypoint, return the frames that need to be interpolated
|
||||
# handle_LR_swap = true # Better if few cameras (eg less than 4) with risk of limb swapping (eg camera facing sagittal plane), otherwise slightly less accurate and slower
|
||||
# undistort_points = true # Better if distorted image (parallel lines curvy on the edge or at least one param > 10^-2), but unnecessary (and slightly slower) if distortions are low
|
||||
# make_c3d = false # save triangulated data in c3d format in addition to trc
|
||||
|
||||
|
||||
|
||||
@ -92,12 +92,13 @@ BODY_25B = Node("CHip", id=None, children=[
|
||||
|
||||
|
||||
## FUNCTIONS
|
||||
def computeP(calib_file):
|
||||
def computeP(calib_file, undistort=False):
|
||||
'''
|
||||
Compute projection matrices from toml calibration file.
|
||||
|
||||
INPUT:
|
||||
- calib_file: calibration .toml file.
|
||||
- undistort: boolean
|
||||
|
||||
OUTPUT:
|
||||
- P: projection matrix as list of arrays
|
||||
@ -109,7 +110,13 @@ def computeP(calib_file):
|
||||
calib = toml.load(calib_file)
|
||||
for cam in list(calib.keys()):
|
||||
if cam != 'metadata':
|
||||
S = np.array(calib[cam]['size'])
|
||||
K = np.array(calib[cam]['matrix'])
|
||||
if undistort:
|
||||
dist = np.array(calib[cam]['distortions'])
|
||||
optim_K = cv2.getOptimalNewCameraMatrix(K, dist, [int(s) for s in S], 1, [int(s) for s in S])[0]
|
||||
Kh = np.block([optim_K, np.zeros(3).reshape(3,1)])
|
||||
else:
|
||||
Kh = np.block([K, np.zeros(3).reshape(3,1)])
|
||||
R, _ = cv2.Rodrigues(np.array(calib[cam]['rotation']))
|
||||
T = np.array(calib[cam]['translation'])
|
||||
@ -120,6 +127,38 @@ def computeP(calib_file):
|
||||
return P
|
||||
|
||||
|
||||
def retrieve_calib_params(calib_file):
|
||||
'''
|
||||
Compute projection matrices from toml calibration file.
|
||||
|
||||
INPUT:
|
||||
- calib_file: calibration .toml file.
|
||||
|
||||
OUTPUT:
|
||||
- S: (h,w) vectors as list of 2x1 arrays
|
||||
- K: intrinsic matrices as list of 3x3 arrays
|
||||
- dist: distortion vectors as list of 4x1 arrays
|
||||
- optim_K: intrinsic matrices for undistorting points as list of 3x3 arrays
|
||||
- R: rotation rodrigue vectors as list of 3x1 arrays
|
||||
- T: translation vectors as list of 3x1 arrays
|
||||
'''
|
||||
|
||||
calib = toml.load(calib_file)
|
||||
|
||||
S, K, dist, optim_K, R, T = [], [], [], [], [], []
|
||||
for c, cam in enumerate(calib.keys()):
|
||||
if cam != 'metadata':
|
||||
S.append(np.array(calib[cam]['size']))
|
||||
K.append(np.array(calib[cam]['matrix']))
|
||||
dist.append(np.array(calib[cam]['distortions']))
|
||||
optim_K.append(cv2.getOptimalNewCameraMatrix(K[c], dist[c], [int(s) for s in S[c]], 1, [int(s) for s in S[c]])[0])
|
||||
R.append(np.array(calib[cam]['rotation']))
|
||||
T.append(np.array(calib[cam]['translation']))
|
||||
calib_params = {'S': S, 'K': K, 'dist': dist, 'optim_K': optim_K, 'R': R, 'T': T}
|
||||
|
||||
return calib_params
|
||||
|
||||
|
||||
def reprojection(P_all, Q):
|
||||
'''
|
||||
Reprojects 3D point on all cameras.
|
||||
@ -231,7 +270,7 @@ def reproj_from_trc_calib_func(*args):
|
||||
filename = os.path.splitext(os.path.basename(input_trc_file))[0]
|
||||
|
||||
# Extract data from calibration file
|
||||
P_all = computeP(input_calib_file)
|
||||
P_all = computeP(input_calib_file, undistort_points=True)
|
||||
|
||||
# Create camera folders
|
||||
reproj_dir = os.path.realpath(output_file_root)
|
||||
|
||||
@ -38,24 +38,62 @@ __status__ = "Development"
|
||||
|
||||
|
||||
## FUNCTIONS
|
||||
def computeP(calib_file):
|
||||
def retrieve_calib_params(calib_file):
|
||||
'''
|
||||
Compute projection matrices from toml calibration file.
|
||||
|
||||
INPUT:
|
||||
- calib_file: calibration .toml file.
|
||||
|
||||
OUTPUT:
|
||||
- S: (h,w) vectors as list of 2x1 arrays
|
||||
- K: intrinsic matrices as list of 3x3 arrays
|
||||
- dist: distortion vectors as list of 4x1 arrays
|
||||
- optim_K: intrinsic matrices for undistorting points as list of 3x3 arrays
|
||||
- R: rotation rodrigue vectors as list of 3x1 arrays
|
||||
- T: translation vectors as list of 3x1 arrays
|
||||
'''
|
||||
|
||||
calib = toml.load(calib_file)
|
||||
|
||||
S, K, dist, optim_K, R, T = [], [], [], [], [], []
|
||||
for c, cam in enumerate(calib.keys()):
|
||||
if cam != 'metadata':
|
||||
S.append(np.array(calib[cam]['size']))
|
||||
K.append(np.array(calib[cam]['matrix']))
|
||||
dist.append(np.array(calib[cam]['distortions']))
|
||||
optim_K.append(cv2.getOptimalNewCameraMatrix(K[c], dist[c], [int(s) for s in S[c]], 1, [int(s) for s in S[c]])[0])
|
||||
R.append(np.array(calib[cam]['rotation']))
|
||||
T.append(np.array(calib[cam]['translation']))
|
||||
calib_params = {'S': S, 'K': K, 'dist': dist, 'optim_K': optim_K, 'R': R, 'T': T}
|
||||
|
||||
return calib_params
|
||||
|
||||
|
||||
def computeP(calib_file, undistort=False):
|
||||
'''
|
||||
Compute projection matrices from toml calibration file.
|
||||
|
||||
INPUT:
|
||||
- calib_file: calibration .toml file.
|
||||
- undistort: boolean
|
||||
|
||||
OUTPUT:
|
||||
- P: projection matrix as list of arrays
|
||||
'''
|
||||
|
||||
K, R, T, Kh, H = [], [], [], [], []
|
||||
P = []
|
||||
|
||||
calib = toml.load(calib_file)
|
||||
|
||||
P = []
|
||||
for cam in list(calib.keys()):
|
||||
if cam != 'metadata':
|
||||
S = np.array(calib[cam]['size'])
|
||||
K = np.array(calib[cam]['matrix'])
|
||||
if undistort:
|
||||
dist = np.array(calib[cam]['distortions'])
|
||||
optim_K = cv2.getOptimalNewCameraMatrix(K, dist, [int(s) for s in S], 1, [int(s) for s in S])[0]
|
||||
Kh = np.block([optim_K, np.zeros(3).reshape(3,1)])
|
||||
else:
|
||||
Kh = np.block([K, np.zeros(3).reshape(3,1)])
|
||||
R, _ = cv2.Rodrigues(np.array(calib[cam]['rotation']))
|
||||
T = np.array(calib[cam]['translation'])
|
||||
|
||||
@ -41,8 +41,8 @@ from anytree import RenderTree
|
||||
from anytree.importer import DictImporter
|
||||
import logging
|
||||
|
||||
from Pose2Sim.common import computeP, weighted_triangulation, reprojection, \
|
||||
euclidean_distance, natural_sort
|
||||
from Pose2Sim.common import retrieve_calib_params, computeP, weighted_triangulation, \
|
||||
reprojection, euclidean_distance, natural_sort
|
||||
from Pose2Sim.skeletons import *
|
||||
|
||||
|
||||
@ -247,6 +247,7 @@ def track_2d_all(config):
|
||||
pose_model = config.get('pose').get('pose_model')
|
||||
tracked_keypoint = config.get('personAssociation').get('tracked_keypoint')
|
||||
frame_range = config.get('project').get('frame_range')
|
||||
tracked_keypoint = config.get('triangulation').get('undistort_points')
|
||||
|
||||
calib_dir = [os.path.join(session_dir, c) for c in os.listdir(session_dir) if ('Calib' or 'calib') in c][0]
|
||||
calib_file = glob.glob(os.path.join(calib_dir, '*.toml'))[0] # lastly created calibration file
|
||||
@ -254,7 +255,7 @@ def track_2d_all(config):
|
||||
poseTracked_dir = os.path.join(project_dir, 'pose-associated')
|
||||
|
||||
# projection matrix from toml calibration file
|
||||
P = computeP(calib_file)
|
||||
P = computeP(calib_file, undistort=undistort_points)
|
||||
|
||||
# selection of tracked keypoint id
|
||||
try: # from skeletons.py
|
||||
|
||||
@ -48,8 +48,8 @@ from anytree import RenderTree
|
||||
from anytree.importer import DictImporter
|
||||
import logging
|
||||
|
||||
from Pose2Sim.common import computeP, weighted_triangulation, reprojection, \
|
||||
euclidean_distance, natural_sort
|
||||
from Pose2Sim.common import retrieve_calib_params, computeP, weighted_triangulation, \
|
||||
reprojection, euclidean_distance, natural_sort
|
||||
from Pose2Sim.skeletons import *
|
||||
|
||||
|
||||
@ -206,6 +206,7 @@ def recap_triangulate(config, error, nb_cams_excluded, keypoints_names, cam_excl
|
||||
likelihood_threshold = config.get('triangulation').get('likelihood_threshold')
|
||||
show_interp_indices = config.get('triangulation').get('show_interp_indices')
|
||||
interpolation_kind = config.get('triangulation').get('interpolation')
|
||||
handle_LR_swap = config.get('triangulation').get('handle_LR_swap')
|
||||
|
||||
# Recap
|
||||
calib_cam1 = calib[list(calib.keys())[0]]
|
||||
@ -236,7 +237,7 @@ def recap_triangulate(config, error, nb_cams_excluded, keypoints_names, cam_excl
|
||||
mean_cam_excluded = np.around(nb_cams_excluded['mean'].mean(), decimals=2)
|
||||
|
||||
logging.info(f'\n--> Mean reprojection error for all points on all frames is {mean_error_px} px, which roughly corresponds to {mean_error_mm} mm. ')
|
||||
logging.info(f'Cameras were excluded if likelihood was below {likelihood_threshold} and if the reprojection error was above {error_threshold_triangulation} px.')
|
||||
logging.info(f'Cameras were excluded if likelihood was below {likelihood_threshold} and if the reprojection error was above {error_threshold_triangulation} px. Limb swapping was {"handled" if handle_LR_swap else "not handled"}.')
|
||||
logging.info(f'In average, {mean_cam_excluded} cameras had to be excluded to reach these thresholds.')
|
||||
cam_excluded_count = {i: v for i, v in zip(cam_names, cam_excluded_count.values())}
|
||||
str_cam_excluded_count = ''
|
||||
@ -252,7 +253,7 @@ def recap_triangulate(config, error, nb_cams_excluded, keypoints_names, cam_excl
|
||||
logging.info(f'\n3D coordinates are stored at {trc_path}.')
|
||||
|
||||
|
||||
def triangulation_from_best_cameras(config, coords_2D_kpt, coords_2D_kpt_swapped, projection_matrices):
|
||||
def triangulation_from_best_cameras(config, coords_2D_kpt, coords_2D_kpt_swapped, projection_matrices, *calib_params):
|
||||
'''
|
||||
Triangulates 2D keypoint coordinates. If reprojection error is above threshold,
|
||||
tries swapping left and right sides. If still above, removes a camera until error
|
||||
@ -281,6 +282,7 @@ def triangulation_from_best_cameras(config, coords_2D_kpt, coords_2D_kpt_swapped
|
||||
error_threshold_triangulation = config.get('triangulation').get('reproj_error_threshold_triangulation')
|
||||
min_cameras_for_triangulation = config.get('triangulation').get('min_cameras_for_triangulation')
|
||||
handle_LR_swap = config.get('triangulation').get('handle_LR_swap')
|
||||
undistort_points = config.get('triangulation').get('undistort_points')
|
||||
|
||||
# Initialize
|
||||
x_files, y_files, likelihood_files = coords_2D_kpt
|
||||
@ -319,6 +321,9 @@ def triangulation_from_best_cameras(config, coords_2D_kpt, coords_2D_kpt_swapped
|
||||
Q_filt = [weighted_triangulation(projection_matrices_filt[i], x_files_filt[i], y_files_filt[i], likelihood_files_filt[i]) for i in range(len(id_cams_off))]
|
||||
|
||||
# Reprojection
|
||||
if undistort_points:
|
||||
coords_2D_kpt_calc_filt = [cv2.projectPoints(Q_filt[i], calib_params['R'][i], calib_params['T'][i], calib_params['K'][i], calib_params['dist'][i]) for i in range(len(id_cams_off))]
|
||||
else:
|
||||
coords_2D_kpt_calc_filt = [reprojection(projection_matrices_filt[i], Q_filt[i]) for i in range(len(id_cams_off))]
|
||||
coords_2D_kpt_calc_filt = np.array(coords_2D_kpt_calc_filt, dtype=object)
|
||||
x_calc_filt = coords_2D_kpt_calc_filt[:,0]
|
||||
@ -359,6 +364,11 @@ def triangulation_from_best_cameras(config, coords_2D_kpt, coords_2D_kpt_swapped
|
||||
for id_off in range(len(id_cams_off))] )
|
||||
|
||||
# Reprojection
|
||||
if undistort_points:
|
||||
coords_2D_kpt_calc_off_swap = np.array([[cv2.projectPoints(Q_filt[id_off, id_swapped], calib_params['R'][id_off], calib_params['T'][id_off], calib_params['K'][id_off], calib_params['dist'][id_off])
|
||||
for id_swapped in range(len(id_cams_swapped))]
|
||||
for id_off in range(len(id_cams_off))])
|
||||
else:
|
||||
coords_2D_kpt_calc_off_swap = np.array([[reprojection(projection_matrices_filt[id_off], Q_filt_off_swap[id_off, id_swapped])
|
||||
for id_swapped in range(len(id_cams_swapped))]
|
||||
for id_off in range(len(id_cams_off))])
|
||||
@ -477,6 +487,7 @@ def triangulate_all(config):
|
||||
interpolation_kind = config.get('triangulation').get('interpolation')
|
||||
interp_gap_smaller_than = config.get('triangulation').get('interp_if_gap_smaller_than')
|
||||
show_interp_indices = config.get('triangulation').get('show_interp_indices')
|
||||
undistort_points = config.get('triangulation').get('undistort_points')
|
||||
|
||||
calib_dir = [os.path.join(session_dir, c) for c in os.listdir(session_dir) if ('Calib' or 'calib') in c][0]
|
||||
calib_file = glob.glob(os.path.join(calib_dir, '*.toml'))[0] # lastly created calibration file
|
||||
@ -484,7 +495,8 @@ def triangulate_all(config):
|
||||
poseTracked_dir = os.path.join(project_dir, 'pose-associated')
|
||||
|
||||
# Projection matrix from toml calibration file
|
||||
P = computeP(calib_file)
|
||||
P = computeP(calib_file, undistort=undistort_points)
|
||||
calib_params = retrieve_calib_params(calib_file)
|
||||
|
||||
# Retrieve keypoints from model
|
||||
try: # from skeletons.py
|
||||
@ -530,21 +542,13 @@ def triangulate_all(config):
|
||||
json_tracked_files_f = [json_tracked_files[c][f] for c in range(n_cams)]
|
||||
x_files, y_files, likelihood_files = extract_files_frame_f(json_tracked_files_f, keypoints_ids)
|
||||
|
||||
# # undistort points draft: start with
|
||||
# points = [np.array(tuple(zip(x_files[i],y_files[i]))).reshape(-1, 1, 2) for i in range(n_cams)]
|
||||
# # calculate optimal matrix optimal_mat cf https://stackoverflow.com/a/76635257/12196632
|
||||
# undistorted_points = [cv2.undistortPoints(points[i], K[i], distortions[i], None, optimal_mat[i]) for i in range(n_cams)]
|
||||
# # then put back into original shape of x_files, y_files
|
||||
# # Points are undistorted and better triangulated, however reprojection error is not accurate if points are not distorted again prior to reprojection
|
||||
# # This is good for slight distortion. For fishey camera, the model does not work anymore. See there for an example https://github.com/lambdaloop/aniposelib/blob/d03b485c4e178d7cff076e9fe1ac36837db49158/aniposelib/cameras.py#L301
|
||||
|
||||
# # undistort points draft: start with
|
||||
# points = [np.array(tuple(zip(x_files[i],y_files[i]))).reshape(-1, 1, 2) for i in range(n_cams)]
|
||||
# # calculate optimal matrix optimal_mat cf https://stackoverflow.com/a/76635257/12196632
|
||||
# undistorted_points = [cv2.undistortPoints(points[i], K[i], distortions[i], None, optimal_mat[i]) for i in range(n_cams)]
|
||||
# # then put back into original shape of x_files, y_files
|
||||
# # Points are undistorted and better triangulated, however reprojection error is not accurate if points are not distorted again prior to reprojection
|
||||
# # This is good for slight distortion. For fishey camera, the model does not work anymore. See there for an example https://github.com/lambdaloop/aniposelib/blob/d03b485c4e178d7cff076e9fe1ac36837db49158/aniposelib/cameras.py#L301
|
||||
# undistort points
|
||||
if undistort_points:
|
||||
points = [np.array(tuple(zip(x_files[i],y_files[i]))).reshape(-1, 1, 2).astype('float32') for i in range(n_cams)]
|
||||
undistorted_points = [cv2.undistortPoints(points[i], K[i], dist[i], None, optim_K[i]) for i in range(n_cams)]
|
||||
x_files = np.array([[u[i][0][0] for i in range(len(u))] for u in undistorted_points])
|
||||
y_files = np.array([[u[i][0][1] for i in range(len(u))] for u in undistorted_points])
|
||||
# This is good for slight distortion. For fishey camera, the model does not work anymore. See there for an example https://github.com/lambdaloop/aniposelib/blob/d03b485c4e178d7cff076e9fe1ac36837db49158/aniposelib/cameras.py#L301
|
||||
|
||||
# Replace likelihood by 0 if under likelihood_threshold
|
||||
with np.errstate(invalid='ignore'):
|
||||
@ -554,9 +558,9 @@ def triangulate_all(config):
|
||||
for keypoint_idx in keypoints_idx:
|
||||
# Triangulate cameras with min reprojection error
|
||||
coords_2D_kpt = np.array( (x_files[:, keypoint_idx], y_files[:, keypoint_idx], likelihood_files[:, keypoint_idx]) )
|
||||
coords_2D_kpt_swapped = np.array(( x_files[:, keypoints_idx_swapped[keypoint_idx]], y_files[:, keypoints_idx_swapped[keypoint_idx]], likelihood_files[:, keypoints_idx_swapped[keypoint_idx]] ))# ADD coords_2D_kpt_swapped TO THE ARGUMENTS OF triangulation_from_best_cameras
|
||||
coords_2D_kpt_swapped = np.array(( x_files[:, keypoints_idx_swapped[keypoint_idx]], y_files[:, keypoints_idx_swapped[keypoint_idx]], likelihood_files[:, keypoints_idx_swapped[keypoint_idx]] ))
|
||||
|
||||
Q_kpt, error_kpt, nb_cams_excluded_kpt, id_excluded_cams_kpt = triangulation_from_best_cameras(config, coords_2D_kpt, coords_2D_kpt_swapped, P)
|
||||
Q_kpt, error_kpt, nb_cams_excluded_kpt, id_excluded_cams_kpt = triangulation_from_best_cameras(config, coords_2D_kpt, coords_2D_kpt_swapped, P, calib_params) # P has been modified if undistort_points=True
|
||||
|
||||
Q.append(Q_kpt)
|
||||
error.append(error_kpt)
|
||||
|
||||
Loading…
Reference in New Issue
Block a user