def fun_2(this, rankId=0): """ @params: rankInd = 0 for the MAP solution Computes the indexes of the particles for the ranked solution, and set the body model accordingly Returns the log posterior (or negative energy) """ best = np.zeros(this.nNodes) for partId in this.nodeIdx: I_sort = np.argsort(-1*this.b[partId]['value'], axis=None).copy() this.mapParticleInd[partId] = I_sort[rankId] best[partId] = this.b[partId]['value'][I_sort[rankId]] if this.verbose > 1: print 'MAP particles' print this.mapParticleInd # Set the object for i in this.nodeIdx: x = this.b[i]['x'][:, this.mapParticleInd[i]] ra, ta, za, zsa = particles.get_part_params(this.particleIdx[i], x) this.body.Zp[i] = za this.body.Zs[i] = zsa this.body.r_abs[i,:] = ra this.body.t[i,:] = ta i = this.torso p = this.mapParticleInd[i] for c in range(this.nCameras): this.camera[c][0:3] = this.b[i]['x'][this.camIdx[i][c][0:3],p] this.camera[c][3:6] = this.b[i]['x'][this.camIdx[i][c][3:6],p] this.camera[c][6] = this.b[i]['x'][this.camIdx[i][c][6],p] this.camera[c][7:9] = this.b[i]['x'][this.camIdx[i][c][7:9],p] this.camera[c][9:14] = np.zeros(5) logB = best[this.torso] return logB
def fun_2(this, rankId=0): """ @params: rankInd = 0 for the MAP solution Computes the indexes of the particles for the ranked solution, and set the body model accordingly Returns the log posterior (or negative energy) """ best = np.zeros(this.nNodes) for partId in this.nodeIdx: I_sort = np.argsort(-1 * this.b[partId]["value"], axis=None).copy() this.mapParticleInd[partId] = I_sort[rankId] best[partId] = this.b[partId]["value"][I_sort[rankId]] if this.verbose > 1: print "MAP particles" print this.mapParticleInd # Set the object for i in this.nodeIdx: x = this.b[i]["x"][:, this.mapParticleInd[i]] ra, ta, za, zsa = particles.get_part_params(this.particleIdx[i], x) this.body.Zp[i] = za this.body.Zs[i] = zsa this.body.r_abs[i, :] = ra this.body.t[i, :] = ta i = this.torso p = this.mapParticleInd[i] for c in range(this.nCameras): this.camera[c][0:3] = this.b[i]["x"][this.camIdx[i][c][0:3], p] this.camera[c][3:6] = this.b[i]["x"][this.camIdx[i][c][3:6], p] this.camera[c][6] = this.b[i]["x"][this.camIdx[i][c][6], p] this.camera[c][7:9] = this.b[i]["x"][this.camIdx[i][c][7:9], p] this.camera[c][9:14] = np.zeros(5) logB = best[this.torso] return logB
def sample_from_nbrs(this, i, x, p_i): """ Sample a new particle by looking at the neighbors. Differently from above, we look at the neighbors and sample the pose and shape deformations. We have to consider the model poseDefModelNeighbors, that has variables [z_parent, z_part, r_rel_part, r_rel_child], and we want to sample the part pose deformations z_part given the relative pose w.r.t parent and child @params this: a dpmp object @params i : the index of the node @params x : the particle array @params p_i : the index of the particle x in the node array b[i]['x'] """ pa = this.body.parent[i] ch = this.body.child[i] # Select the best neighbors particles or at random """ I_pa = np.argmax(this.b[pa]['value']) num_x = this.b[pa]['x'].shape[1] I_pa = np.random.randint(0,num_x,1)[0] x_pa = this.b[pa]['x'][:,I_pa] I_ch = np.argmax(this.b[ch]['value']) num_x = this.b[ch]['x'].shape[1] I_ch = np.random.randint(0,num_x,1)[0] x_ch = this.b[ch]['x'][:,I_ch] """ # Select the current best I_pa = np.argmax(this.b[pa]['value']) I_ch = np.argmax(this.b[ch]['value']) proposedParticle = x[:,p_i].copy() mu = this.body.poseDefModelNeighbors[i]['mu'] C = this.body.poseDefModelNeighbors[i]['C'] # Conditioning varibles # Compute the relative Rodrigues vector between the particle and the neightbors. Note that the relative Rodrigues vectors are # always defined between the part and its parent, so we need r_rel of i w.r.t pa and of ch w.r.t. i x_pa = this.b[pa]['x'][:,I_pa] rpa, tpa, zpa, zspa = particles.get_part_params(this.particleIdx[pa], x_pa) x_ch = this.b[ch]['x'][:,I_ch] rch, tch, zch, zsch = particles.get_part_params(this.particleIdx[ch], x_ch) r, t, z, zs = particles.get_part_params(this.particleIdx[i], proposedParticle) R0_parent, J = cv2.Rodrigues(rpa) R0_part, J = cv2.Rodrigues(r) A = np.matrix(R0_part) B = np.matrix(R0_parent) R = A*B.T*(B*B.T).I r_rel_pa, J = cv2.Rodrigues(R) R0_child, J = cv2.Rodrigues(rch) A = np.matrix(R0_child) B = np.matrix(R0_part) R = A*B.T*(B*B.T).I r_rel_ch, J = cv2.Rodrigues(R) X = np.concatenate((r_rel_pa, r_rel_ch)) # Indexes of the conditioning variables na = len(X) nmu = np.prod(mu.shape) cInd = xrange(nmu-na,nmu) # Indexes of the resulting variables nb = this.nB[i] rInd = xrange(this.body.nPoseBasis[pa], this.body.nPoseBasis[pa]+nb) mu_ab, C_ab = ba.compute_conditioned_gaussian(this.body, rInd, cInd, mu, C, X) proposedParticle[this.particleIdx[i]['zPoseIdx']] = mu_ab # For the shape parameters, we propagate the same shape from the parent proposedParticle[this.particleIdx[i]['zPoseIdx']] = x_pa[this.particleIdx[pa]['zPoseIdx']] return proposedParticle
def fun(this, rankInd): """ @params: rankInd = 0 for the MAP solution Computes the indexes of the particles for the ranked solution, and set the body model accordingly Returns the log posterior (or negative energy) Only works for the same number of particles on each node """ logB = None # Build a matrix (nParticles x nNodes) with the max-marginal values of all nodes B = -np.inf + np.zeros((len(this.b[this.torso]['value']), this.nNodes)) for i in this.nodeIdx: B[:,i] = this.b[i]['value'] # Sort the matrix to find the global max I = np.argsort(-B, axis=None) B_sort = B.flatten()[I] # Find the unique indexes of the max-marginal (which have the same value on each node) _, I_unique = np.unique(B_sort, return_index=True) I_unique = I_unique[::-1] # The ranked value identifies a root node and a particle. We will traverse the tree from the selected root to find the # particles on each node if rankInd < len(I_unique): v = np.where(B == B.flatten()[I[I_unique[rankInd]]]) else: v = np.where(B == B.flatten()[I[I_unique[-1]]]) p = v[0][0] rootNode = v[1][0] # Depth-first traversal schedule = [None] schedule[0] = rootNode visited = np.zeros((this.nNodes), dtype=np.int) parent = np.zeros((this.nNodes), dtype=np.int) while len(schedule) > 0: # Pop root, add children i = schedule[0] schedule = schedule[1::] if i in this.nodeIdx: visited[i] = 1 nbrs = np.zeros((this.nNodes), dtype=np.int) nbrs[this.A[:,i]>=0] = 1 children = nbrs & ~visited N = np.where(children>0)[0] schedule.extend(N) parent[N] = i else: continue # Do backtracking if i == rootNode: logB = this.b[i]['value'][p] this.mapParticleInd[i] = p else: # Get the message i->parent that contains the argmax information t = this.Q[i, parent[i]] ind = np.where(this.m[t]['pDst'] == this.mapParticleInd[parent[i]])[0] p = this.m[t]['pSrc'][ind] this.mapParticleInd[i] = p if this.verbose > 0: print 'mapParticleInd:' print this.mapParticleInd # Set the object for i in this.nodeIdx: x = this.b[i]['x'][:, this.mapParticleInd[i]] ra, ta, za, zsa = particles.get_part_params(this.particleIdx[i], x) this.body.Zp[i] = za this.body.Zs[i] = zsa this.body.r_abs[i,:] = ra this.body.t[i,:] = ta return logB
def fun(this, rankInd): """ @params: rankInd = 0 for the MAP solution Computes the indexes of the particles for the ranked solution, and set the body model accordingly Returns the log posterior (or negative energy) Only works for the same number of particles on each node """ logB = None # Build a matrix (nParticles x nNodes) with the max-marginal values of all nodes B = -np.inf + np.zeros((len(this.b[this.torso]["value"]), this.nNodes)) for i in this.nodeIdx: B[:, i] = this.b[i]["value"] # Sort the matrix to find the global max I = np.argsort(-B, axis=None) B_sort = B.flatten()[I] # Find the unique indexes of the max-marginal (which have the same value on each node) _, I_unique = np.unique(B_sort, return_index=True) I_unique = I_unique[::-1] # The ranked value identifies a root node and a particle. We will traverse the tree from the selected root to find the # particles on each node if rankInd < len(I_unique): v = np.where(B == B.flatten()[I[I_unique[rankInd]]]) else: v = np.where(B == B.flatten()[I[I_unique[-1]]]) p = v[0][0] rootNode = v[1][0] # Depth-first traversal schedule = [None] schedule[0] = rootNode visited = np.zeros((this.nNodes), dtype=np.int) parent = np.zeros((this.nNodes), dtype=np.int) while len(schedule) > 0: # Pop root, add children i = schedule[0] schedule = schedule[1::] if i in this.nodeIdx: visited[i] = 1 nbrs = np.zeros((this.nNodes), dtype=np.int) nbrs[this.A[:, i] >= 0] = 1 children = nbrs & ~visited N = np.where(children > 0)[0] schedule.extend(N) parent[N] = i else: continue # Do backtracking if i == rootNode: logB = this.b[i]["value"][p] this.mapParticleInd[i] = p else: # Get the message i->parent that contains the argmax information t = this.Q[i, parent[i]] ind = np.where(this.m[t]["pDst"] == this.mapParticleInd[parent[i]])[0] p = this.m[t]["pSrc"][ind] this.mapParticleInd[i] = p if this.verbose > 0: print "mapParticleInd:" print this.mapParticleInd # Set the object for i in this.nodeIdx: x = this.b[i]["x"][:, this.mapParticleInd[i]] ra, ta, za, zsa = particles.get_part_params(this.particleIdx[i], x) this.body.Zp[i] = za this.body.Zs[i] = zsa this.body.r_abs[i, :] = ra this.body.t[i, :] = ta return logB
def compute_pairwise_stitch_potential(this, a, b, xSrc, xDst): """ Computes the pairwise stitch cost for two connected parts @params a: parent part @params b: child part @params xSrc: set of Na particles of the parent @params xDst: set of Nb particle of the child Return negative cost(Na x Nb) matrix """ Na = xSrc.shape[1] Nb = xDst.shape[1] # Computes the vertexes of the part a in global coordinates [ra, ta, za, zsa ] = particles.get_part_params(this.particleIdx[a], xSrc) Pa = ba.get_part_meshes(this.body, a, za, zsa) Ra = np.zeros((3,3,Na)) for i in xrange(Na): Ra[:,:,i], J = cv2.Rodrigues(ra[:,i]) Paw = ba.object_to_world(Pa, Ra, ta) # Computes the vertexes of the part b in global coordinates [rb, tb, zb, zsb ] = particles.get_part_params(this.particleIdx[b], xDst) Pb = ba.get_part_meshes(this.body, b, zb, zsb) Rb = np.zeros((3,3,Nb)) for i in xrange(Nb): Rb[:,:,i], J = cv2.Rodrigues(rb[:,i]) Pbw = ba.object_to_world(Pb, Rb, tb) cl = this.body.interfacePointsFromTo[b][a] clp = this.body.interfacePointsFromTo[a][b] clf = this.body.interfacePointsFlex[a][b] if len(Pbw.shape) == 3: p2 = Pbw[cl,:,:] p1 = Paw[clp,:,:] n1 = p1.shape[2] n2 = p2.shape[2] cost = np.zeros((n1,n2)) P1 = np.dstack([p1[:,0,:]] * n2) P2 = np.dstack([p2[:,0,:]] * n1) dx = np.transpose(P1, [0, 2, 1]) - P2 P1 = np.dstack([p1[:,1,:]] * n2) P2 = np.dstack([p2[:,1,:]] * n1) dy = np.transpose(P1, [0, 2, 1]) - P2 P1 = np.dstack([p1[:,2,:]] * n2) P2 = np.dstack([p2[:,2,:]] * n1) dz = np.transpose(P1, [0, 2, 1]) - P2 Ta = np.array([ta,]*n2).T Tb = np.array([tb,]*n1) dT = (Ta - Tb)**2 St = np.sqrt(np.sum(dT, axis=1)) else: p2 = Pbw[cl,:] p1 = Paw[clp,:] dx = p1[:,0] - p2[:,0] dy = p1[:,1] - p2[:,1] dz = p1[:,2] - p2[:,2] dT = (ta - tb)**2 St = np.sqrt(np.sum(dT, axis=0)) # Setting a penalty for part centers to be close penalizes interpenetration but only for parts that have similar length (arms, legs) S = np.array(St < 0.05, dtype=int) dsq = (dx**2) + (dy**2) + (dz**2) weights = np.ones((dsq.shape)) N = np.sum(weights) weights[clf] = 0.8 Nw = np.sum(weights) weights = weights * N / Nw dsq = dsq * weights cost = np.mean(dsq, axis=0) # Add a penalty for a total reflection of the parts, that would have a low cost, but # we should penalize for inter-penetration return -this.stitchAlpha[a,b] *( cost.T + S )
def sample_from_nbrs(this, i, x, p_i): """ Sample a new particle by looking at the neighbors. Differently from above, we look at the neighbors and sample the pose and shape deformations. We have to consider the model poseDefModelNeighbors, that has variables [z_parent, z_part, r_rel_part, r_rel_child], and we want to sample the part pose deformations z_part given the relative pose w.r.t parent and child @params this: a dpmp object @params i : the index of the node @params x : the particle array @params p_i : the index of the particle x in the node array b[i]['x'] """ pa = this.body.parent[i] ch = this.body.child[i] # Select the best neighbors particles or at random """ I_pa = np.argmax(this.b[pa]['value']) num_x = this.b[pa]['x'].shape[1] I_pa = np.random.randint(0,num_x,1)[0] x_pa = this.b[pa]['x'][:,I_pa] I_ch = np.argmax(this.b[ch]['value']) num_x = this.b[ch]['x'].shape[1] I_ch = np.random.randint(0,num_x,1)[0] x_ch = this.b[ch]['x'][:,I_ch] """ # Select the current best I_pa = np.argmax(this.b[pa]['value']) I_ch = np.argmax(this.b[ch]['value']) proposedParticle = x[:, p_i].copy() mu = this.body.poseDefModelNeighbors[i]['mu'] C = this.body.poseDefModelNeighbors[i]['C'] # Conditioning varibles # Compute the relative Rodrigues vector between the particle and the neightbors. Note that the relative Rodrigues vectors are # always defined between the part and its parent, so we need r_rel of i w.r.t pa and of ch w.r.t. i x_pa = this.b[pa]['x'][:, I_pa] rpa, tpa, zpa, zspa = particles.get_part_params(this.particleIdx[pa], x_pa) x_ch = this.b[ch]['x'][:, I_ch] rch, tch, zch, zsch = particles.get_part_params(this.particleIdx[ch], x_ch) r, t, z, zs = particles.get_part_params(this.particleIdx[i], proposedParticle) R0_parent, J = cv2.Rodrigues(rpa) R0_part, J = cv2.Rodrigues(r) A = np.matrix(R0_part) B = np.matrix(R0_parent) R = A * B.T * (B * B.T).I r_rel_pa, J = cv2.Rodrigues(R) R0_child, J = cv2.Rodrigues(rch) A = np.matrix(R0_child) B = np.matrix(R0_part) R = A * B.T * (B * B.T).I r_rel_ch, J = cv2.Rodrigues(R) X = np.concatenate((r_rel_pa, r_rel_ch)) # Indexes of the conditioning variables na = len(X) nmu = np.prod(mu.shape) cInd = xrange(nmu - na, nmu) # Indexes of the resulting variables nb = this.nB[i] rInd = xrange(this.body.nPoseBasis[pa], this.body.nPoseBasis[pa] + nb) mu_ab, C_ab = ba.compute_conditioned_gaussian(this.body, rInd, cInd, mu, C, X) proposedParticle[this.particleIdx[i]['zPoseIdx']] = mu_ab # For the shape parameters, we propagate the same shape from the parent proposedParticle[this.particleIdx[i]['zPoseIdx']] = x_pa[ this.particleIdx[pa]['zPoseIdx']] return proposedParticle