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

Convert VIcon calib

Browse Source
This commit is contained in:
davidpagnon 2023-07-31 16:46:47 +02:00
parent 054c8373d6
commit 8263579b92
4 changed files with 172 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Pose2Sim/Demo/User/Config.toml
View File

@ -29,7 +29,7 @@ pose_model = 'BODY_25B' #CUSTOM, BODY_25B, BODY_25, BODY_135, BLAZEPOSE, HALPE_2
calibration_type = 'convert' # 'convert' or 'calculate'
[calibration.convert]
convert_from = 'qualisys' # 'qualisys', 'optitrack', or 'vicon' (coming)
convert_from = 'qualisys' # 'qualisys', 'optitrack', or 'vicon'
[calibration.convert.qualisys]
binning_factor = 1 # Usually 1, except when filming in 540p where it usually is 2

106
Pose2Sim/calibration.py
View File

@ -42,7 +42,7 @@ import warnings
import matplotlib.pyplot as plt
from mpl_interactions import zoom_factory, panhandler
from Pose2Sim.common import RT_qca2cv, rotate_cam, euclidean_distance, natural_sort
from Pose2Sim.common import RT_qca2cv, rotate_cam, quat2mat, euclidean_distance, natural_sort
## AUTHORSHIP INFORMATION
@ -162,7 +162,7 @@ def read_qca(qca_path, binning_factor):
r33 = float(tag.get('r33'))
# Rotation (by-column to by-line)
R += [np.array([r11, r21, r31, r12, r22, r32, r13, r23, r33]).reshape(3,3)]
R += [np.array([r11, r12, r13, r21, r22, r23, r31, r32, r33]).reshape(3,3).T]
T += [np.array([tx, ty, tz])]
# Cameras names by natural order
@ -201,12 +201,15 @@ def calib_optitrack_fun(config):
pass
def calib_vicon_fun(config):
def calib_vicon_fun(file_to_convert_path, binning_factor=1):
'''
Convert a Vicon calibration file
Convert a Vicon .xcp calibration file
Converts from camera view to object view,
and converts rotation with Rodrigues formula
INPUTS:
- a Config.toml file
- file_to_convert_path: path of the .qca.text file to convert
- binning_factor: always 1 with Vicon calibration
OUTPUTS:
- ret: residual reprojection error in _mm_: list of floats
@ -218,8 +221,85 @@ def calib_vicon_fun(config):
- T: extrinsic translation: list of arrays of floats
'''
ret, C, S, D, K, R, T = read_vicon(file_to_convert_path)
RT = [RT_qca2cv(r,t) for r, t in zip(R, T)]
R = [rt[0] for rt in RT]
T = [rt[1] for rt in RT]
pass
R = [np.array(cv2.Rodrigues(r)[0]).flatten() for r in R]
T = np.array(T)
return ret, C, S, D, K, R, T
def read_vicon(vicon_path):
'''
Reads a Vicon .xcp calibration file
Returns 6 lists of size N (N=number of cameras)
INPUTS:
- vicon_path: path to .xcp calibration file: string
OUTPUTS:
- ret: residual reprojection error in _mm_: list of floats
- C: camera name: list of strings
- S: image size: list of list of floats
- D: distorsion: list of arrays of floats
- K: intrinsic parameters: list of 3x3 arrays of floats
- R: extrinsic rotation: list of 3x3 arrays of floats
- T: extrinsic translation: list of arrays of floats
'''
root = etree.parse(vicon_path).getroot()
ret, C, S, D, K, R, T = [], [], [], [], [], [], []
vid_id = []
# Camera name and image size
for i, tag in enumerate(root.findall('Camera')):
C += [tag.attrib.get('DEVICEID')]
S += [[float(t) for t in tag.attrib.get('SENSOR_SIZE').split()]]
ret += [float(tag.findall('KeyFrames/KeyFrame')[0].attrib.get('WORLD_ERROR'))]
# if tag.attrib.get('model') in ('Miqus Video', 'Miqus Video UnderWater', 'none'):
vid_id += [i]
# Intrinsic parameters: distorsion and intrinsic matrix
for cam_elem in root.findall('Camera'):
try:
dist = cam_elem.findall('KeyFrames/KeyFrame')[0].attrib.get('VICON_RADIAL2').split()[3:5]
except:
dist = cam_elem.findall('KeyFrames/KeyFrame')[0].attrib.get('VICON_RADIAL').split()
D += [[float(d) for d in dist] + [0.0, 0.0]]
fu = float(cam_elem.findall('KeyFrames/KeyFrame')[0].attrib.get('FOCAL_LENGTH'))
fv = fu / float(cam_elem.attrib.get('PIXEL_ASPECT_RATIO'))
cam_center = cam_elem.findall('KeyFrames/KeyFrame')[0].attrib.get('PRINCIPAL_POINT').split()
cu, cv = [float(c) for c in cam_center]
K += [np.array([fu, 0., cu, 0., fv, cv, 0., 0., 1.]).reshape(3,3)]
# Extrinsic parameters: rotation matrix and translation vector
for cam_elem in root.findall('Camera'):
rot = cam_elem.findall('KeyFrames/KeyFrame')[0].attrib.get('ORIENTATION').split()
R_quat = [float(r) for r in rot]
R_mat = quat2mat(R_quat, scalar_idx=3)
R += [R_mat]
trans = cam_elem.findall('KeyFrames/KeyFrame')[0].attrib.get('POSITION').split()
T += [[float(t)/1000 for t in trans]]
# Cameras names by natural order
C_vid = [C[v] for v in vid_id]
C_vid_id = [C_vid.index(c) for c in natural_sort(C_vid)]
C_id = [vid_id[c] for c in C_vid_id]
C = [C[c] for c in C_id]
S = [S[c] for c in C_id]
D = [D[c] for c in C_id]
K = [K[c] for c in C_id]
R = [R[c] for c in C_id]
T = [T[c] for c in C_id]
return ret, C, S, D, K, R, T
def calib_board_fun(calib_dir, intrinsics_config_dict, extrinsics_config_dict):
@ -998,16 +1078,16 @@ def recap_calibrate(ret, calib_path, calib_full_type):
ret_m, ret_px = [], []
for c, cam in enumerate(calib.keys()):
if cam != 'metadata':
fm = calib[cam]['matrix'][0][0]
f_px = calib[cam]['matrix'][0][0]
Dm = euclidean_distance(calib[cam]['translation'], [0,0,0])
if calib_full_type=='convert_qualisys':
ret_m.append( np.around(ret[c]*1000, decimals=3) )
ret_px.append( np.around(ret[c] / Dm * fm, decimals=3) )
if calib_full_type=='convert_qualisys' or calib_full_type=='convert_vicon':
ret_m.append( np.around(ret[c], decimals=3) )
ret_px.append( np.around(ret[c] / (Dm*1000) * f_px, decimals=3) )
elif calib_full_type=='calculate_board':
ret_px.append( np.around(ret[c], decimals=3) )
ret_m.append( np.around(ret[c]*1000 * Dm / fm, decimals=3) )
ret_m.append( np.around(ret[c]*(Dm*1000) / f_px, decimals=3) )
logging.info(f'\n--> Residual (RMS) calibration errors for each camera are respectively {ret_px} px, which corresponds to {ret_m} mm.\n')
logging.info(f'\n--> Residual (RMS) calibration errors for each camera are respectively {ret_px} px, \nwhich corresponds to {ret_m} mm.\n')
logging.info(f'Calibration file is stored at {calib_path}.')
@ -1040,6 +1120,8 @@ def calibrate_cams_all(config):
elif convert_filetype=='optitrack':
print('See Readme.md to retrieve calibration values.')
elif convert_filetype=='vicon':
convert_ext = '.xcp'
binning_factor = 1
print('Not supported yet')
file_to_convert_path = glob.glob(os.path.join(calib_dir, f'*{convert_ext}*'))[0]

60
Pose2Sim/common.py
View File

@ -180,6 +180,66 @@ def rotate_cam(r, t, ang_x=0, ang_y=0, ang_z=0):
return r, t
def quat2rod(quat, scalar_idx=0):
'''
Converts quaternion to Rodrigues vector
INPUT:
- quat: quaternion. np.array of size 4
- scalar_idx: index of scalar part of quaternion. Default: 0, sometimes 3
OUTPUT:
- rod: Rodrigues vector. np.array of size 3
'''
if scalar_idx == 0:
w, qx, qy, qz = np.array(quat)
if scalar_idx == 3:
qx, qy, qz, w = np.array(quat)
else:
print('Error: scalar_idx should be 0 or 3')
rodx = qx * np.tan(w/2)
rody = qy * np.tan(w/2)
rodz = qz * np.tan(w/2)
rod = np.array([rodx, rody, rodz])
return rod
def quat2mat(quat, scalar_idx=0):
'''
Converts quaternion to rotation matrix
INPUT:
- quat: quaternion. np.array of size 4
- scalar_idx: index of scalar part of quaternion. Default: 0, sometimes 3
OUTPUT:
- mat: 3x3 rotation matrix
'''
if scalar_idx == 0:
w, qx, qy, qz = np.array(quat)
elif scalar_idx == 3:
qx, qy, qz, w = np.array(quat)
else:
print('Error: scalar_idx should be 0 or 3')
r11 = 1 - 2 * (qy**2 + qz**2)
r12 = 2 * (qx*qy - qz*w)
r13 = 2 * (qx*qz + qy*w)
r21 = 2 * (qx*qy + qz*w)
r22 = 1 - 2 * (qx**2 + qz**2)
r23 = 2 * (qy*qz - qx*w)
r31 = 2 * (qx*qz - qy*w)
r32 = 2 * (qy*qz + qx*w)
r33 = 1 - 2 * (qx**2 + qy**2)
mat = np.array([r11, r12, r13, r21, r22, r23, r31, r32, r33]).reshape(3,3).T
return mat
def natural_sort(list):
'''
Sorts list of strings with numbers in natural order

25
README.md
View File

@ -40,7 +40,7 @@ To upgrade, type `pip install pose2sim --upgrade`. You will need to update your
3. [With DeepLabCut](#with-deeplabcut)
4. [With AlphaPose](#with-alphapose)
3. [Camera calibration](#camera-calibration)
1. [Convert file](#convert-file)
1. [Convert from Qualisys, Optitrack, or Vicon](#convert-from-qualisys-optitrack-or-vicon)
2. [Calculate from scratch](#calculate-from-scratch)
4. [Camera synchronization](#camera-synchronization)
5. [Tracking, Triangulating, Filtering](#tracking-triangulating-filtering)
@ -235,7 +235,7 @@ N.B.: Markers are not needed in Pose2Sim and were used here for validation
## Camera calibration
> _**Convert a preexisting calibration file, or calculate intrinsic and extrinsic parameters from scratch.**_
### Convert file
### Convert from Qualisys, Optitrack, or Vicon
If you already have a calibration file, set `calibration_type` type to `convert` in your `Config.toml` file.
- **From Qualisys:**
@ -244,7 +244,7 @@ If you already have a calibration file, set `calibration_type` type to `convert`
- set `convert_from` to 'qualisys' in your `Config.toml` file. Change `binning_factor` to 2 if you film in 540p
- **From Optitrack:** Exporting calibration will be available in Motive 3.2. In the meantime:
- Calculate intrinsics with a board (see next section)
- Use their C++ API [to retrieve extrinsic properties](https://docs.optitrack.com/developer-tools/motive-api/motive-api-function-reference#tt_cameraxlocation). Translation can be copied as is in your `Calib.toml` file, but TT_CameraOrientationMatrix first needs to be [converted to a Rodrigues vector](https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html#ga61585db663d9da06b68e70cfbf6a1eac) with OpenCV.
- Use their C++ API [to retrieve extrinsic properties](https://docs.optitrack.com/developer-tools/motive-api/motive-api-function-reference#tt_cameraxlocation). Translation can be copied as is in your `Calib.toml` file, but TT_CameraOrientationMatrix first needs to be [converted to a Rodrigues vector](https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html#ga61585db663d9da06b68e70cfbf6a1eac) with OpenCV. See instructions [here](https://github.com/perfanalytics/pose2sim/issues/28)
- **From Vicon:**
- Not possible yet. [Want to contribute?](#how-to-contribute)
@ -342,7 +342,7 @@ Output:\
## Camera synchronization
> _**Cameras need to be synchronized, so that 2D points correspond to the same position across cameras.**_\
> _**Cameras need to be synchronized, so that 2D points correspond to the same position across cameras.**_\
*N.B.: Skip this step if your cameras are already synchronized.*
If your cameras are not natively synchronized, you can use [this script](https://github.com/perfanalytics/pose2sim/blob/draft/Pose2Sim/Utilities/synchronize_cams.py).
@ -741,6 +741,7 @@ If you want to contribute to Pose2Sim, please follow [this guide](https://docs.g
- Supervised my PhD: @lreveret (INRIA, Université Grenoble Alpes), and @mdomalai (Université de Poitiers).
- Provided the Demo data: @aaiaueil from Université Gustave Eiffel.
- Provided a code snippet for Optitrack calibration: @claraaudap (Université Bretagne Sud).
</br>
@ -756,18 +757,22 @@ If you want to contribute to Pose2Sim, please follow [this guide](https://docs.g
</br>
> - [x] **Calibration:** Convert [Qualisys](https://www.qualisys.com) .qca.txt calibration file.
> - [x] **Calibration:** Convert Optitrack extrinsic calibration file.
> - [x] **Calibration:** Convert Vicon [.xcp calibration file](https://montrealrobotics.ca/diffcvgp/assets/papers/7.pdf).
> - [x] **Calibration:** Easier and clearer calibration procedure: separate intrinsic and extrinsic parameter calculation, edit corner detection if some are wrongly detected (or not visible).
> - [x] **Calibration:** Possibility to evaluate extrinsic parameters from cues on scene.
> - [ ] **Calibration:** Support ChArUco board detection (see [there](https://mecaruco2.readthedocs.io/en/latest/notebooks_rst/Aruco/sandbox/ludovic/aruco_calibration_rotation.html)).
> - [ ] **Calibration:** Calculate calibration with points rather than board. (1) SBA calibration with wand (cf [Argus](https://argus.web.unc.edu), see converter [here](https://github.com/backyardbiomech/DLCconverterDLT/blob/master/DLTcameraPosition.py)). Set world reference frame in the end.
> - [ ] **Calibration:** Alternatively, calibrate with [OpenPose keypoints](https://ietresearch.onlinelibrary.wiley.com/doi/full/10.1049/cvi2.12130). Set world reference frame in the end.
> - [ ] **Calibration:** Smoother Optitrack calibration file conversion.
> - [ ] **Calibration:** Convert Vicon .xcp calibration file.
> - [ ] **Calibration:** Alternatively, self-calibrate with [OpenPose keypoints](https://ietresearch.onlinelibrary.wiley.com/doi/full/10.1049/cvi2.12130). Set world reference frame in the end.
</br>
> - [ ] **Synchronization:** Synchronize cameras on 2D keypoint speeds. Cf [this draft script](https://github.com/perfanalytics/pose2sim/blob/draft/Pose2Sim/Utilities/synchronize_cams.py).
</br>
> - [x] **Person Association:** Automatically choose the main person to triangulate.
> - [ ] **Person Association:** Multiple persons association. See [Dong 2021](https://arxiv.org/pdf/1901.04111.pdf). With a neural network instead of brute force?
> - [ ] **Person Association:** Multiple persons association. 1. Triangulate all the persons whose reprojection error is below a certain threshold (instead of only the one with minimum error), and then track in time with speed cf [Slembrouck 2020](https://link.springer.com/chapter/10.1007/978-3-030-40605-9_15)? or 2. Based on affinity matrices [Dong 2021](https://arxiv.org/pdf/1901.04111.pdf)? or 3. Based on occupancy maps [Yildiz 2012](https://link.springer.com/chapter/10.1007/978-3-642-35749-7_10)? or 4. With a neural network [Huang 2023](https://arxiv.org/pdf/2304.09471.pdf)?
</br>
@ -781,6 +786,9 @@ If you want to contribute to Pose2Sim, please follow [this guide](https://docs.g
> - [ ] **Triangulation:** Offer the possibility of triangulating with Sparse Bundle Adjustment (SBA), Extended Kalman Filter (EKF), Full Trajectory Estimation (FTE) (see [AcinoSet](https://github.com/African-Robotics-Unit/AcinoSet)).
> - [ ] **Triangulation:** Outlier rejection (sliding z-score?) Also solve limb swapping.
> - [ ] **Triangulation:** Implement normalized DLT and RANSAC triangulation, as well as a [triangulation refinement step](https://doi.org/10.1109/TMM.2022.3171102).
</br>
> - [x] **Filtering:** Available filtering methods: Butterworth, Butterworth on speed, LOESS, Gaussian, Median.
> - [ ] **Filtering:** Add Kalman smoothing filter.
@ -795,6 +803,7 @@ If you want to contribute to Pose2Sim, please follow [this guide](https://docs.g
</br>
> - [ ] **GUI:** 3D plot of cameras and of triangulated keypoints.
> - [ ] **GUI:** Demo on Google Colab (see [Sports2D](https://bit.ly/Sports2D_Colab) for OpenPose and Python package installation on Google Drive).
> - [ ] **GUI:** Blender add-on, or webapp (e.g., with [Napari](https://napari.org/stable). See my draft project [Maya-Mocap](https://github.com/davidpagnon/Maya-Mocap) and [BlendOsim](https://github.com/JonathanCamargo/BlendOsim).
</br>