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)