-
Notifications
You must be signed in to change notification settings - Fork 7
/
Copy pathcameras.py
142 lines (107 loc) · 3.5 KB
/
cameras.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
"""Utilities to deal with the cameras of human3.6m"""
import h5py
import numpy as np
cam_name_to_id = {
"54138969": 1,
"55011271": 2,
"58860488": 3,
"60457274": 4,
}
def project_point_radial( P, R, T, f, c, k, p ):
"""
Project points from 3d to 2d using camera parameters
including radial and tangential distortion
Args
P: Nx3 points in world coordinates
R: 3x3 Camera rotation matrix
T: 3x1 Camera translation parameters
f: (scalar) Camera focal length
c: 2x1 Camera center
k: 3x1 Camera radial distortion coefficients
p: 2x1 Camera tangential distortion coefficients
Returns
Proj: Nx2 points in pixel space
D: 1xN depth of each point in camera space
radial: 1xN radial distortion per point
tan: 1xN tangential distortion per point
r2: 1xN squared radius of the projected points before distortion
"""
# P is a matrix of 3-dimensional points
assert len(P.shape) == 2
assert P.shape[1] == 3
N = P.shape[0]
X = R.dot( P.T - T ) # rotate and translate
XX = X[:2,:] / X[2,:]
r2 = XX[0,:]**2 + XX[1,:]**2
radial = 1 + np.einsum( 'ij,ij->j', np.tile(k,(1, N)), np.array([r2, r2**2, r2**3]) );
tan = p[0]*XX[1,:] + p[1]*XX[0,:]
XXX = XX * np.tile(radial+tan,(2,1)) + np.outer(np.array([p[1], p[0]]).reshape(-1), r2 )
Proj = (f * XXX) + c
Proj = Proj.T
D = X[2,]
return Proj, D, radial, tan, r2
def world_to_camera_frame(P, R, T):
"""
Convert points from world to camera coordinates
Args
P: Nx3 3d points in world coordinates
R: 3x3 Camera rotation matrix
T: 3x1 Camera translation parameters
Returns
X_cam: Nx3 3d points in camera coordinates
"""
assert len(P.shape) == 2
assert P.shape[1] == 3
X_cam = R.dot( P.T - T ) # rotate and translate
return X_cam.T
def camera_to_world_frame(P, R, T):
"""Inverse of world_to_camera_frame
Args
P: Nx3 points in camera coordinates
R: 3x3 Camera rotation matrix
T: 3x1 Camera translation parameters
Returns
X_cam: Nx3 points in world coordinates
"""
assert len(P.shape) == 2
assert P.shape[1] == 3
X_cam = R.T.dot( P.T ) + T # rotate and translate
return X_cam.T
def load_camera_params( hf, path ):
"""Load h36m camera parameters
Args
hf: hdf5 open file with h36m cameras data
path: path or key inside hf to the camera we are interested in
Returns
R: 3x3 Camera rotation matrix
T: 3x1 Camera translation parameters
f: (scalar) Camera focal length
c: 2x1 Camera center
k: 3x1 Camera radial distortion coefficients
p: 2x1 Camera tangential distortion coefficients
name: String with camera id
"""
R = hf[ path.format('R') ][:]
R = R.T
T = hf[ path.format('T') ][:]
f = hf[ path.format('f') ][:]
c = hf[ path.format('c') ][:]
k = hf[ path.format('k') ][:]
p = hf[ path.format('p') ][:]
name = hf[ path.format('Name') ][:]
name = "".join( [chr(item) for item in name] )
return R, T, f, c, k, p, name
def load_cameras( bpath='cameras.h5', subjects=[1,5,6,7,8,9,11] ):
"""Loads the cameras of h36m
Args
bpath: path to hdf5 file with h36m camera data
subjects: List of ints representing the subject IDs for which cameras are requested
Returns
rcams: dictionary of 4 tuples per subject ID containing its camera parameters for the 4 h36m cams
"""
rcams = {}
with h5py.File(bpath,'r') as hf:
for s in subjects:
for c in range(4): # There are 4 cameras in human3.6m
rcams[(s, c+1)] = load_camera_params(hf, 'subject%d/camera%d/{0}' % (s,c+1) )
return rcams