def init_particles(this):
# Initialize the particles on each node
maxNp = np.max(this.nParticles)
if this.display > 3:
ms = [Mesh(v=this.scanMesh.v, f=this.scanMesh.f).set_vertex_colors('SeaGreen')]
# Number of particles on each node
n = np.zeros((this.nNodes))
k = 0
for p in range(int(maxNp)):
Pw, r_abs, t, Zp, Zs = get_sample_from_model.get_sample(this.body, this.nB[this.torso], this.nB, this.nBshape[this.torso], add_global_rotation=this.init_with_global_rotation, init_torso_rotation=this.init_torso_rotation)
this.body.r_abs = r_abs
t = t + this.init_torso_location
this.body.t = t
for i in this.nodeIdx:
this.body.Zs[i] = Zs
this.body.Zp = Zp
for i in this.nodeIdx:
if n[i] < this.nParticles[i]:
this.b[i]['x'][:,p] = particles.get_from_sbm_with_init_location_noise(this, this.body, i)
n[i] = n[i]+1
if this.display > 4:
partSet = this.body.partSet
P = ba.get_sbm_points_per_part(this.body)
for part in partSet:
ms.append(Mesh(v=P[part], f=this.body.partFaces[part]).set_vertex_colors(this.body.colors[part]))
if this.display > 4:
mv = MeshViewer()
mv.set_static_meshes(ms)
def show_mesh(this, P, dbstop=False, mesh=None, scan=None, filename=None, Pwo=None):
# P is a list of parts
mv = MeshViewer()
#mv.set_background_color(np.array([1.0, 1.0, 1.0]))
ms = [Mesh(v=P[part], f=this.partFaces[part]).set_vertex_colors('light blue') for part in this.partSet]
if mesh is not None:
ms2 = [Mesh(v=mesh[part], f=this.partFaces[part]).set_vertex_colors('SeaGreen') for part in this.partSet]
ms = ms + ms2
if scan is not None:
s = [Mesh(v=scan.v, f=scan.f).set_vertex_colors('firebrick')]
ms = ms + s
if Pwo is not None:
ms2 = [Mesh(v=Pwo[part], f=this.partFaces[part]).set_vertex_colors('turquoise3') for part in this.partSet]
ms = ms + ms2
mv.set_static_meshes(ms)
if filename is not None:
time.sleep(1)
mv.save_snapshot(filename, blocking=True)
if dbstop:
import pdb
pdb.set_trace()
else:
time.sleep(4)
def show_all_particles(this, faustId, nParticles, s):
"""
@params: this, a dpmp object
@params: faustId, ID of the faust scan, only used for the filename of the picture
@params: nParticles, how many particles to show
@params: step, only used for the filename of the picture
"""
ms = [Mesh(v=this.scanMesh.v, f=this.scanMesh.f).set_vertex_colors('firebrick')]
for i in range(0, nParticles):
for part in this.body.partSet:
P = particles.particle_to_points(this, this.b[part]['x'][:,i], part)
ms.append(Mesh(v=P, f=this.body.partFaces[part]).set_vertex_colors(this.body.colors[part]))
mv = MeshViewer()
#mv.set_background_color(np.array([1.0, 1.0, 1.0]))
mv.set_static_meshes(ms)
time.sleep(4)
mv.save_snapshot('particles_'+faustId+'_'+str(s)+'.png', blocking=True)
def show_all_particles(this, faustId, nParticles, s):
"""
@params: this, a dpmp object
@params: faustId, ID of the faust scan, only used for the filename of the picture
@params: nParticles, how many particles to show
@params: step, only used for the filename of the picture
"""
ms = [
Mesh(v=this.scanMesh.v,
f=this.scanMesh.f).set_vertex_colors('firebrick')
]
for i in range(0, nParticles):
for part in this.body.partSet:
P = particles.particle_to_points(this, this.b[part]['x'][:, i],
part)
ms.append(
Mesh(v=P, f=this.body.partFaces[part]).set_vertex_colors(
this.body.colors[part]))
mv = MeshViewer()
#mv.set_background_color(np.array([1.0, 1.0, 1.0]))
mv.set_static_meshes(ms)
time.sleep(4)
mv.save_snapshot('particles_' + faustId + '_' + str(s) + '.png',
blocking=True)
def show_mesh(this,
P,
dbstop=False,
mesh=None,
scan=None,
filename=None,
Pwo=None):
# P is a list of parts
mv = MeshViewer()
#mv.set_background_color(np.array([1.0, 1.0, 1.0]))
ms = [
Mesh(v=P[part], f=this.partFaces[part]).set_vertex_colors('light blue')
for part in this.partSet
]
if mesh is not None:
ms2 = [
Mesh(v=mesh[part],
f=this.partFaces[part]).set_vertex_colors('SeaGreen')
for part in this.partSet
]
ms = ms + ms2
if scan is not None:
s = [Mesh(v=scan.v, f=scan.f).set_vertex_colors('firebrick')]
ms = ms + s
if Pwo is not None:
ms2 = [
Mesh(v=Pwo[part],
f=this.partFaces[part]).set_vertex_colors('turquoise3')
for part in this.partSet
]
ms = ms + ms2
mv.set_static_meshes(ms)
if filename is not None:
time.sleep(1)
mv.save_snapshot(filename, blocking=True)
if dbstop:
import pdb
pdb.set_trace()
else:
time.sleep(4)
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)
