Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

3D human pose #184

Open
wants to merge 10 commits into
base: master
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
210 changes: 210 additions & 0 deletions examples/human_pose_3d/cameras.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,210 @@
"""Utilities to deal with the cameras of human3.6m"""

from xml.dom import minidom
import math
import numpy as np

CAMERA_ID_TO_NAME = {
1: "54138969",
2: "55011271",
3: "58860488",
4: "60457274",
}


def eulerAnglesToRotationMatrix(theta):
R_x = np.array([[1, 0, 0],

[0, math.cos(theta[0]), -math.sin(theta[0])],

[0, math.sin(theta[0]), math.cos(theta[0])]

])
R_y = np.array([[math.cos(theta[1]), 0, math.sin(theta[1])],

[0, 1, 0],

[-math.sin(theta[1]), 0, math.cos(theta[1])]

])

R_z = np.array([[math.cos(theta[2]), -math.sin(theta[2]), 0],

[math.sin(theta[2]), math.cos(theta[2]), 0],

[0, 0, 1]

])

R = np.dot(R_x, np.dot(R_y, R_z))
return R


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

# print(f"\n****************In camera loop {P[:32].shape} {P[:32]}*****************\n")

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
# print(f"\n P shape: {P.shape}")
# print(f"\n R in camera_to_world_frame shape: {R.shape} {type(R)}")
# print(f"\n T shape: {T.shape}")
# print(f"\n P.T shape: {(P.T).shape}")
# print(f"\n R.T shape: {(R.T).shape}")
# print(f"\n R.T.dot( P.T ) shape: {(R.T.dot( P.T )).shape}")
X_cam = R.T.dot( P.T ) + T # rotate and translate

return X_cam.T


def load_camera_params(w0, subject, camera):
"""Load h36m camera parameters

Args
w0: 300-long array read from XML metadata
subect: int subject id
camera: int camera id
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
"""

# Get the 15 numbers for this subject and camera
w1 = np.zeros(15)
start = 6 * ((camera-1)*11 + (subject-1))
w1[:6] = w0[start:start+6]
w1[6:] = w0[(265+(camera-1)*9 - 1): (264+camera*9)]

def rotationMatrix(r):
R1, R2, R3 = [np.zeros((3, 3)) for _ in range(3)]

# [1 0 0; 0 cos(obj.Params(1)) -sin(obj.Params(1)); 0 sin(obj.Params(1)) cos(obj.Params(1))]
R1[0:] = [1, 0, 0]
R1[1:] = [0, np.cos(r[0]), -np.sin(r[0])]
R1[2:] = [0, np.sin(r[0]), np.cos(r[0])]

# [cos(obj.Params(2)) 0 sin(obj.Params(2)); 0 1 0; -sin(obj.Params(2)) 0 cos(obj.Params(2))]
R2[0:] = [ np.cos(r[1]), 0, np.sin(r[1])]
R2[1:] = [0, 1, 0]
R2[2:] = [-np.sin(r[1]), 0, np.cos(r[1])]

# [cos(obj.Params(3)) -sin(obj.Params(3)) 0; sin(obj.Params(3)) cos(obj.Params(3)) 0; 0 0 1];%
R3[0:] = [np.cos(r[2]), -np.sin(r[2]), 0]
R3[1:] = [np.sin(r[2]), np.cos(r[2]), 0]
R3[2:] = [0, 0, 1]

return (R1.dot(R2).dot(R3))

R = rotationMatrix(w1)
# print(f"\n R in load_camera_params: {R} {type(R)}")
T = w1[3:6][:, np.newaxis]
f = w1[6:8][:, np.newaxis]
c = w1[8:10][:, np.newaxis]
k = w1[10:13][:, np.newaxis]
p = w1[13:15][:, np.newaxis]
name = CAMERA_ID_TO_NAME[camera]

return R, T, f, c, k, p, name


def load_cameras(bpath, subjects=[1,5,6,7,8,9,11]):
"""Loads the cameras of h36m

Args
bpath: path to xml 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 = {}

xmldoc = minidom.parse(bpath)
string_of_numbers = xmldoc.getElementsByTagName('w0')[0].firstChild.data[1:-1]

# Parse into floats
w0 = np.array(list(map(float, string_of_numbers.split(" "))))

assert len(w0) == 300

for s in subjects:
for c in range(4): # There are 4 cameras in human3.6m
rcams[(s, c+1)] = load_camera_params(w0, s, c+1)

return rcams
Loading