fulyvv/pose2sim
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix lens distortion correction. Still to be fully tested

Browse Source
This commit is contained in:
davidpagnon 2024-01-03 03:58:10 +01:00
parent 76c39fcafd
commit 47bbb17d0c
1 changed files with 31 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
Pose2Sim/triangulation.py
View File

@ -40,6 +40,7 @@ import numpy as np
import json import json
import itertools as it import itertools as it
import pandas as pd import pandas as pd
import cv2
import toml import toml
from tqdm import tqdm from tqdm import tqdm
from scipy import interpolate from scipy import interpolate
@ -253,7 +254,7 @@ def recap_triangulate(config, error, nb_cams_excluded, keypoints_names, cam_excl
logging.info(f'\n3D coordinates are stored at {trc_path}.') 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, *calib_params): 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, Triangulates 2D keypoint coordinates. If reprojection error is above threshold,
tries swapping left and right sides. If still above, removes a camera until error tries swapping left and right sides. If still above, removes a camera until error
@ -282,7 +283,13 @@ def triangulation_from_best_cameras(config, coords_2D_kpt, coords_2D_kpt_swapped
error_threshold_triangulation = config.get('triangulation').get('reproj_error_threshold_triangulation') error_threshold_triangulation = config.get('triangulation').get('reproj_error_threshold_triangulation')
min_cameras_for_triangulation = config.get('triangulation').get('min_cameras_for_triangulation') min_cameras_for_triangulation = config.get('triangulation').get('min_cameras_for_triangulation')
handle_LR_swap = config.get('triangulation').get('handle_LR_swap') handle_LR_swap = config.get('triangulation').get('handle_LR_swap')
undistort_points = config.get('triangulation').get('undistort_points') undistort_points = config.get('triangulation').get('undistort_points')
if undistort_points:
calib_params_K = calib_params['K']
calib_params_dist = calib_params['dist']
calib_params_R = calib_params['R']
calib_params_T = calib_params['T']
# Initialize # Initialize
x_files, y_files, likelihood_files = coords_2D_kpt x_files, y_files, likelihood_files = coords_2D_kpt
@ -294,7 +301,14 @@ def triangulation_from_best_cameras(config, coords_2D_kpt, coords_2D_kpt_swapped
while error_min > error_threshold_triangulation and n_cams - nb_cams_off >= min_cameras_for_triangulation: while error_min > error_threshold_triangulation and n_cams - nb_cams_off >= min_cameras_for_triangulation:
# Create subsets with "nb_cams_off" cameras excluded # Create subsets with "nb_cams_off" cameras excluded
id_cams_off = np.array(list(it.combinations(range(n_cams), nb_cams_off))) id_cams_off = np.array(list(it.combinations(range(n_cams), nb_cams_off)))
if undistort_points:
calib_params_K_filt = [calib_params_K]*len(id_cams_off)
calib_params_dist_filt = [calib_params_dist]*len(id_cams_off)
calib_params_R_filt = [calib_params_R]*len(id_cams_off)
calib_params_T_filt = [calib_params_T]*len(id_cams_off)
projection_matrices_filt = [projection_matrices]*len(id_cams_off) projection_matrices_filt = [projection_matrices]*len(id_cams_off)
x_files_filt = np.vstack([x_files.copy()]*len(id_cams_off)) x_files_filt = np.vstack([x_files.copy()]*len(id_cams_off))
y_files_filt = np.vstack([y_files.copy()]*len(id_cams_off)) y_files_filt = np.vstack([y_files.copy()]*len(id_cams_off))
x_files_swapped_filt = np.vstack([x_files_swapped.copy()]*len(id_cams_off)) x_files_swapped_filt = np.vstack([x_files_swapped.copy()]*len(id_cams_off))
@ -310,7 +324,13 @@ def triangulation_from_best_cameras(config, coords_2D_kpt, coords_2D_kpt_swapped
likelihood_files_filt[i][id_cams_off[i]] = np.nan likelihood_files_filt[i][id_cams_off[i]] = np.nan
nb_cams_excluded_filt = [np.count_nonzero(np.nan_to_num(x)==0) for x in likelihood_files_filt] # count nans and zeros nb_cams_excluded_filt = [np.count_nonzero(np.nan_to_num(x)==0) for x in likelihood_files_filt] # count nans and zeros
if undistort_points:
calib_params_K_filt = [ [ c[i] for i in range(n_cams) if not np.isnan(x_files_filt[j][i]) ] for j, c in enumerate(calib_params_K_filt) ]
calib_params_dist_filt = [ [ c[i] for i in range(n_cams) if not np.isnan(x_files_filt[j][i]) ] for j, c in enumerate(calib_params_dist_filt) ]
calib_params_R_filt = [ [ c[i] for i in range(n_cams) if not np.isnan(x_files_filt[j][i]) ] for j, c in enumerate(calib_params_R_filt) ]
calib_params_T_filt = [ [ c[i] for i in range(n_cams) if not np.isnan(x_files_filt[j][i]) ] for j, c in enumerate(calib_params_T_filt) ]
projection_matrices_filt = [ [ p[i] for i in range(n_cams) if not np.isnan(x_files_filt[j][i]) ] for j, p in enumerate(projection_matrices_filt) ] projection_matrices_filt = [ [ p[i] for i in range(n_cams) if not np.isnan(x_files_filt[j][i]) ] for j, p in enumerate(projection_matrices_filt) ]
x_files_filt = np.array([ [ xx for ii, xx in enumerate(x) if not np.isnan(xx) ] for x in x_files_filt ]) x_files_filt = np.array([ [ xx for ii, xx in enumerate(x) if not np.isnan(xx) ] for x in x_files_filt ])
y_files_filt = np.array([ [ xx for ii, xx in enumerate(x) if not np.isnan(xx) ] for x in y_files_filt ]) y_files_filt = np.array([ [ xx for ii, xx in enumerate(x) if not np.isnan(xx) ] for x in y_files_filt ])
x_files_swapped_filt = np.array([ [ xx for ii, xx in enumerate(x) if not np.isnan(xx) ] for x in x_files_swapped_filt ]) x_files_swapped_filt = np.array([ [ xx for ii, xx in enumerate(x) if not np.isnan(xx) ] for x in x_files_swapped_filt ])
@ -322,7 +342,10 @@ def triangulation_from_best_cameras(config, coords_2D_kpt, coords_2D_kpt_swapped
# Reprojection # Reprojection
if undistort_points: 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))] coords_2D_kpt_calc_filt = np.array([[cv2.projectPoints(np.array(Q_filt[i][:-1]), calib_params_R_filt[i][j], calib_params_T_filt[i][j], calib_params_K_filt[i][j], calib_params_dist_filt[i][j])[0].ravel()
for j in range(n_cams-nb_cams_off)]
for i in range(len(id_cams_off))])
coords_2D_kpt_calc_filt = [[coords_2D_kpt_calc_filt[i,:,0], coords_2D_kpt_calc_filt[i,:,1]] for i in range(len(id_cams_off))]
else: 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 = [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) coords_2D_kpt_calc_filt = np.array(coords_2D_kpt_calc_filt, dtype=object)
@ -365,9 +388,13 @@ def triangulation_from_best_cameras(config, coords_2D_kpt, coords_2D_kpt_swapped
# Reprojection # Reprojection
if undistort_points: 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]) coords_2D_kpt_calc_off_swap = np.array([[[cv2.projectPoints(np.array(Q_filt_off_swap[id_off,id_swapped][:-1]), calib_params_R_filt[id_off][j], calib_params_T_filt[id_off][j], calib_params_K_filt[id_off][j], calib_params_dist_filt[id_off][j])[0].ravel()
for j in range(n_cams-nb_cams_off)]
for id_swapped in range(len(id_cams_swapped))] for id_swapped in range(len(id_cams_swapped))]
for id_off in range(len(id_cams_off))]) for id_off in range(len(id_cams_off))])
coords_2D_kpt_calc_off_swap = np.array([[[coords_2D_kpt_calc_off_swap[id_off,id_swapped,:,0], coords_2D_kpt_calc_off_swap[id_off,id_swapped,:,1]]
for id_swapped in range(len(id_cams_swapped))]
for id_off in range(len(id_cams_off))])
else: else:
coords_2D_kpt_calc_off_swap = np.array([[reprojection(projection_matrices_filt[id_off], Q_filt_off_swap[id_off, id_swapped]) 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_swapped in range(len(id_cams_swapped))]
@ -545,7 +572,7 @@ def triangulate_all(config):
# undistort points # undistort points
if 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)] 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)] undistorted_points = [cv2.undistortPoints(points[i], calib_params['K'][i], calib_params['dist'][i], None, calib_params['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]) 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]) 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 # 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