def guess_init(
model, # pylint: disable=too-many-locals, too-many-arguments
focal_length,
j2d,
weights,
init_pose,
use_gt_guess):
"""Initialize the camera depth using the torso points."""
cids = _np.arange(0, 12)
j2d_here = j2d[cids]
smpl_ids = [8, 5, 2, 1, 4, 7, 21, 19, 17, 16, 18, 20]
# The initial pose is a t-pose with all body parts planar to the camera
# (i.e., they have their maximum length).
opt_pose = _ch.array(init_pose) # pylint: disable=no-member
(_, A_global) = _global_rigid_transformation(opt_pose,
model.J,
model.kintree_table,
xp=_ch)
Jtr = _ch.vstack([g[:3, 3] for g in A_global]) # pylint: disable=no-member
Jtr = Jtr[smpl_ids].r
if use_gt_guess:
# More robust estimate:
# Since there is no fore'lengthening', only foreshortening, take the longest
# visible body part as a robust estimate in the case of noise-free data.
longest_part = None
longest_part_length = -1.
for conn in connections_lsp:
if (conn[0] < 12 and conn[1] < 12 and weights[conn[0]] > 0.
and weights[conn[1]] > 0.):
part_length_proj_gt = _np.sqrt(
_np.sum(_np.array(j2d_here[conn[0]] -
j2d_here[conn[1]])**2,
axis=0))
if part_length_proj_gt > longest_part_length:
longest_part_length = part_length_proj_gt
longest_part = conn
#length2d = _np.sqrt(_np.sum(
# _np.array(j2d_here[longest_part[0]] - j2d_here[longest_part[1]])**2, axis=0))
part_length_3D = _np.sqrt(
_np.sum(_np.array(Jtr[longest_part[0]] - Jtr[longest_part[1]])**2,
axis=0))
est_d = focal_length * (part_length_3D / longest_part_length)
else:
diff3d = _np.array([Jtr[9] - Jtr[3], Jtr[8] - Jtr[2]])
mean_height3d = _np.mean(_np.sqrt(_np.sum(diff3d**2, axis=1)))
diff2d = _np.array(
[j2d_here[9] - j2d_here[3], j2d_here[8] - j2d_here[2]])
mean_height2d = _np.mean(_np.sqrt(_np.sum(diff2d**2, axis=1)))
f = focal_length # pylint: disable=redefined-outer-name
if mean_height2d == 0.:
_LOGGER.warn(
"Depth can not be correctly estimated. Guessing wildly.")
est_d = 60.
else:
est_d = f * (mean_height3d / mean_height2d)
init_t = _np.array([0., 0., est_d])
return init_t
def optimize_on_joints(j2d,
model,
cam,
img,
prior,
try_both_orient,
body_orient,
exp_logistic,
n_betas=10,
inner_penetration=False,
silh=None,
conf=None,
viz=False):
"""Run the optimization."""
if silh is not None:
raise NotImplementedError("Silhouette fitting is not supported in "
"this code release due to dependencies on "
"proprietary code for the "
"distance computation.")
t0 = _time()
# define the mapping LSP joints -> SMPL joints
if j2d.shape[0] == 14:
cids = range(12) + [13]
elif j2d.shape[0] == 91:
cids = range(j2d.shape[0])
else:
raise Exception("Unknown number of joints: %d! Mapping not defined!" %
j2d.shape[0])
# joint ids for SMPL
smpl_ids = [8, 5, 2, 1, 4, 7, 21, 19, 17, 16, 18, 20]
# weight given to each joint during optimization;
if j2d.shape[0] == 14:
weights = [1, 1, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1]
else:
weights = [1] * (len(smpl_ids) + len(landmark_mesh_91))
# The non-skeleton vertex ids are added later.
if try_both_orient:
flipped_orient = _cv2.Rodrigues(body_orient)[0].dot(
_cv2.Rodrigues(_np.array([0., _np.pi, 0]))[0])
flipped_orient = _cv2.Rodrigues(flipped_orient)[0].ravel()
orientations = [body_orient, flipped_orient]
else:
orientations = [body_orient]
if try_both_orient:
errors = []
svs = []
cams = []
# rends = []
for o_id, orient in enumerate(orientations):
# initialize betas
betas = _ch.zeros(n_betas) # pylint: disable=no-member
init_pose = _np.hstack((orient, prior.weights.dot(prior.means)))
# 2D joint error term
# make the SMPL joint depend on betas
Jdirs = _np.dstack([
model.J_regressor.dot(model.shapedirs[:, :, i])
for i in range(len(betas))
])
# pylint: disable=no-member
J_onbetas = _ch.array(Jdirs).dot(betas) + model.J_regressor.dot(
model.v_template.r)
# instantiate the model
sv = verts_decorated(trans=_ch.zeros(3),
pose=_ch.array(init_pose),
v_template=model.v_template,
J=model.J_regressor,
betas=betas,
shapedirs=model.shapedirs[:, :, :n_betas],
weights=model.weights,
kintree_table=model.kintree_table,
bs_style=model.bs_style,
f=model.f,
bs_type=model.bs_type,
posedirs=model.posedirs)
# get joint positions as a function of model pose, betas and trans
(_, A_global) = _global_rigid_transformation(sv.pose,
J_onbetas,
model.kintree_table,
xp=_ch)
Jtr = _ch.vstack([g[:3, 3] for g in A_global]) + sv.trans
if j2d.shape[0] == 14:
# add the "fake" joint for the head
head_id = _HEAD_REGR[0]
Jtr = _ch.vstack((Jtr, sv[head_id]))
if o_id == 0:
smpl_ids.append(len(Jtr) - 1)
else:
# add the plain vertex IDs on the mesh surface.
for vertex_id in landmark_mesh_91.values():
Jtr = _ch.vstack((Jtr, sv[vertex_id]))
# add the joint id
# for SMPL it's the last one added
if o_id == 0:
smpl_ids.append(len(Jtr) - 1)
weights = _np.array(weights, dtype=_np.float64)
if conf is not None:
weights *= conf[cids]
# we'll project the joints on the image plane
cam.v = Jtr
# data term: difference between observed and estimated joints
obj_j2d = lambda w, sigma: (w * weights.reshape((-1, 1)) * _GMOf(
(j2d[cids] - cam[smpl_ids]), sigma))
# pose prior
pprior = lambda w: w * prior(sv.pose) # pylint: disable=cell-var-from-loop
# joint angles prior
# 55: left elbow, should bend -np.pi/2
# 58: right elbow, should bend np.pi/2
# 12: left knee, should bend np.pi/2
# 15: right knee, should bend np.pi/2
if exp_logistic:
_LOGGER.info('USING LOGISTIC')
# Skinny Logistic function. as 50-> inf we get a step function at
# 0.1. (0.1) is a margin bc 0 is still ok.
my_exp = lambda x: 1 / (1 + _ch.exp(100 * (0.1 + -x)))
else:
x_0 = 0 #10
alpha = 10
my_exp = lambda x: alpha * _ch.exp((x - x_0)) # pylint: disable=cell-var-from-loop
obj_angle = lambda w: w * _ch.concatenate([
my_exp(sv.pose[55]), # pylint: disable=cell-var-from-loop
my_exp(-sv.pose[58]), # pylint: disable=cell-var-from-loop
my_exp(-sv.pose[12]), # pylint: disable=cell-var-from-loop
my_exp(-sv.pose[15])
]) # pylint: disable=cell-var-from-loop
if viz:
from body.mesh.sphere import Sphere
from body.mesh.meshviewer import MeshViewer
import matplotlib.pyplot as plt
# set up visualization
# openGL window
mv = MeshViewer(window_width=120, window_height=120)
# and ids
show_ids = _np.array(smpl_ids)[weights > 0]
vc = _np.ones((len(Jtr), 3))
vc[show_ids] = [0, 1, 0]
plt.ion()
def on_step(_):
"""Create visualization."""
# show optimized joints in 3D
# pylint: disable=cell-var-from-loop
mv.set_dynamic_meshes([_Mesh(v=sv.r, f=[]),
Sphere(center=cam.t.r,
radius=.1).to_mesh()] \
+ [Sphere(center=jc, radius=.01).to_mesh(vc[ijc])
for ijc, jc in enumerate(Jtr.r)])
plt.figure(1, figsize=(10, 10))
plt.subplot(1, 2, 1)
# show optimized joints in 2D
tmp_img = img.copy()
for coord, target_coord in zip(
_np.around(cam.r[smpl_ids]).astype(int),
_np.around(j2d[cids]).astype(int)):
if (coord[0] < tmp_img.shape[1] and coord[0] >= 0
and coord[1] < tmp_img.shape[0] and coord[1] >= 0):
_cv2.circle(tmp_img, tuple(coord), 3, [0, 0, 255])
if (target_coord[0] < tmp_img.shape[1]
and target_coord[0] >= 0
and target_coord[1] < tmp_img.shape[0]
and target_coord[1] >= 0):
_cv2.circle(tmp_img, tuple(target_coord), 3,
[0, 255, 0])
plt.imshow(tmp_img)
plt.draw()
plt.show()
on_step(_)
else:
on_step = None
sp = _SphereCollisions(pose=sv.pose,
betas=sv.betas,
model=model,
regs=_REGRESSORS)
sp.no_hands = True
# configuration used with conf joints
opt_weights = zip([4.04 * 1e2, 4.04 * 1e2, 57.4, 4.78],
[1e2, 5 * 1e1, 1e1, .5 * 1e1])
for stage, (w, wbetas) in enumerate(opt_weights):
_LOGGER.info('stage %01d', stage)
objs = {}
#if stage < 2:
objs['j2d'] = obj_j2d(1., 100) # TODO: evaluate.
objs['pose'] = pprior(w)
# WEIGHT FOR ANGLE
if exp_logistic:
# Set to high weight always.
objs['pose_exp'] = obj_angle(5 * 1e3)
else:
objs['pose_exp'] = obj_angle(0.317 * w)
objs['betas'] = wbetas * betas
if inner_penetration:
objs['sph_coll'] = 1e3 * sp
try:
_ch.minimize(objs.values(),
x0=[sv.betas, sv.pose],
method='dogleg',
callback=on_step,
options={
'maxiter': 100,
'e_3': .0001,
'disp': 0
})
except AssertionError:
# Divergence detected.
_LOGGER.warn("Diverging optimization! Breaking!")
break
t1 = _time()
_LOGGER.info('elapsed %.05f', (t1 - t0))
if try_both_orient:
errors.append((objs['j2d'].r**2).sum())
svs.append(sv)
cams.append(cam)
# rends.append(rend)
if try_both_orient and errors[0] > errors[1]:
choose_id = 1
else:
choose_id = 0
if viz:
plt.ioff()
return (svs[choose_id], cams[choose_id].r, cams[choose_id].t.r,
cams[choose_id].rt.r)
def initialize_camera(
model, # pylint: disable=too-many-arguments, too-many-locals
j2d,
center,
img,
init_t,
init_pose,
conf,
is_gt,
flength=1000.,
pix_thsh=25.,
viz=False):
"""Initialize the camera."""
# try to optimize camera translation and rotation based on torso joints
# right shoulder, left shoulder, right hip, left hip
torso_cids = _CID_TORSO
torso_smpl_ids = [2, 1, 17, 16]
# initialize camera rotation and translation
rt = _ch.zeros(3) # pylint: disable=no-member
_LOGGER.info('Initializing camera: guessing translation via similarity')
init_t = guess_init(model, flength, j2d, conf, init_pose, is_gt)
t = _ch.array(init_t) # pylint: disable=no-member
# check how close the shoulders are
try_both_orient = _np.linalg.norm(j2d[8] - j2d[9]) < pix_thsh
opt_pose = _ch.array(init_pose) # pylint: disable=no-member
(_, A_global) = _global_rigid_transformation(opt_pose,
model.J,
model.kintree_table,
xp=_ch)
Jtr = _ch.vstack([g[:3, 3] for g in A_global]) # pylint: disable=no-member
# initialize the camera
cam = _ProjectPoints(f=_np.array([flength, flength]),
rt=rt,
t=t,
k=_np.zeros(5),
c=center)
# we are going to project the SMPL joints
cam.v = Jtr
if viz:
viz_img = img.copy()
# draw the target joints
for coord in _np.around(j2d).astype(int):
if (coord[0] < img.shape[1] and coord[0] >= 0
and coord[1] < img.shape[0] and coord[1] >= 0):
_cv2.circle(viz_img, tuple(coord), 3, [0, 255, 0])
import matplotlib.pyplot as plt
plt.ion()
# draw optimized joints at each iteration
def on_step(_):
"""Draw a visualization."""
plt.figure(1, figsize=(5, 5))
plt.subplot(1, 1, 1)
viz_img = img.copy()
for coord in _np.around(cam.r[torso_smpl_ids]).astype(int):
if (coord[0] < viz_img.shape[1] and coord[0] >= 0
and coord[1] < viz_img.shape[0] and coord[1] >= 0):
_cv2.circle(viz_img, tuple(coord), 3, [0, 0, 255])
plt.imshow(viz_img[:, :, ::-1])
plt.draw()
plt.show()
else:
on_step = None
free_variables = [cam.t, opt_pose[:3]] # pylint: disable=no-member
_ch.minimize(
[(j2d[torso_cids] - cam[torso_smpl_ids]).T * conf[torso_cids], 1e2 *
(cam.t[2] - init_t[2])], # pylint: disable=no-member
# The same for verbose output.
#{'cam': (j2d[torso_cids] - cam[torso_smpl_ids]).T * conf[torso_cids],
# # Reduce the weight here to avoid the 'small people' problem.
# 'cam_t': 1e2*(cam.t[2]-init_t[2])}, # pylint: disable=no-member
x0=free_variables,
method='dogleg',
callback=on_step,
options={
'maxiter': 100,
'e_3': .0001,
'disp': 0
})
if viz:
plt.ioff()
return (cam, try_both_orient, opt_pose[:3].r)
def get_mesh_to_deformation(filename, model, cam, label_folder):
image = cv2.imread(filename, -1)
#if image.shape[2] != 4:
# print "No segmented image "
# exit()
mesh = copy(_TEMPLATE_MESH)
mesh.vc = _COLORS['pink']
# render ply
model.betas[:len(cam['betas'])] = cam['betas']
model.pose[:] = cam['pose']
model.trans[:] = cam['trans']
mesh.v = model.r
# transformation for each v [4,4,6890]
inv_trasformation = np.zeros((4, 4, 6890))
(A, A_global) = _global_rigid_transformation(model.pose,
model.J,
model.kintree_table,
xp=ch)
T = A.dot(model.weights.T)
T0 = np.array(T)
for i in range(6890):
m_inv = np.linalg.inv(T0[:, :, i])
inv_trasformation[:, :, i] = m_inv
#inv_trasformation.append(m_inv.tolist())
#pdb.set_trace()
contor_list = []
list_path = filename.split("/")
path = ''.join(e + "/" for e in list_path[:-2])
todo_vertices = [0] * mesh.v.shape[0]
new_vertices_u_v = []
vertices_u_v = []
dist_vertices = [0.0] * mesh.v.shape[0]
my_normals = np.array(
compute_normals.compute_normals(mesh.v.copy().astype(float).tolist(),
mesh.f.copy().astype(int).tolist()))
for v in range(mesh.v.shape[0]):
vtx1 = (((mesh.v[v, 0] + cam['t'][0]) * cam['f']) /
(mesh.v[v, 2] + cam['t'][2]) + image.shape[1] / 2.0)
vty1 = (((mesh.v[v, 1] + cam['t'][1]) * cam['f']) /
(mesh.v[v, 2] + cam['t'][2]) + image.shape[0] / 2.0)
new_vertices_u_v.append([vtx1, vty1, mesh.v[v, 2]])
vertices_u_v.append([vtx1, vty1, mesh.v[v, 2]])
image_label = cv2.imread(path + label_folder + list_path[-1][:-4] + ".png",
0)
to_draw = np.ones(image.shape, dtype=np.uint8)
for i in range(14):
label = np.where(image_label == label_map_cnn[i + 1],
np.ones(image_label.shape, dtype=np.uint8) * 255,
np.zeros(image_label.shape, dtype=np.uint8))
contours, hierarchy = cv2.findContours(label, cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
to_draw = np.zeros(image.shape, dtype=np.uint8)
cv2.drawContours(to_draw, contours, -1, label_colours[i], 3)
#cv2.imshow('ImageWindow', label)
#cv2.waitKey()
for s, c in enumerate(contours):
for p, point in enumerate(c):
dist = limit_deformation[i + 1]
id_close = -1
for v in range(mesh.v.shape[0]):
if ((int(pose_partes[v]) == (i + 1))):
d = distancia(
vertices_u_v[v],
[point[0][0], point[0][1], vertices_u_v[v][2]])
d = (d * vertices_u_v[v][2]) / cam['f']
if d < dist:
id_close = v
dist = d
if id_close > -1 and intersect_is_valid(
s, p, contours, vertices_u_v[id_close], point[0]):
if dist > dist_vertices[id_close]:
dist_vertices[id_close] = dist
todo_vertices[id_close] = 1
new_vertices_u_v[id_close][0] = point[0][0]
new_vertices_u_v[id_close][1] = point[0][1]
#cv2.line(image, (int(new_vertices_u_v[id_close][0]), int(new_vertices_u_v[id_close][1])), (int(vertices_u_v[id_close][0]), int(vertices_u_v[id_close][1])), label_colours[i], 2)
#cv2.imshow('ImageWindow', image)
#cv2.waitKey()
new_vertices = mesh.v.copy().astype(float).tolist()
label_map_cnn_color = [1, 6, 19, 1, 12, 13, 11, 5, 4, 7, 10, 14, 13, 3, 8]
#image = np.ones_like(image)*255
for v in range(mesh.v.shape[0]):
if todo_vertices[v] == 1:
new_vertices[v][0] = (
(new_vertices_u_v[v][0] - image.shape[1] / 2.0) *
(mesh.v[v, 2] + cam['t'][2])) / cam['f'] - cam['t'][0]
new_vertices[v][1] = (
(new_vertices_u_v[v][1] - image.shape[0] / 2.0) *
(mesh.v[v, 2] + cam['t'][2])) / cam['f'] - cam['t'][1]
lineThickness = 2
#cv2.line(image, (int(new_vertices_u_v[v][0]), int(new_vertices_u_v[v][1])), (int(vertices_u_v[v][0]), int(vertices_u_v[v][1])), label_colours[label_map_cnn_color[int(pose_partes[v])]], lineThickness)
#cv2.imwrite(filename + "deform.jpg", image)
return mesh.v.copy().astype(float).tolist(), new_vertices, mesh.f.copy(
).astype(int).tolist(), todo_vertices, inv_trasformation