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 sample_from_nbr(this, i, x, p_i): """ Sample a new particle by looking at the neighbors. We first pick a neighbor, and then generate a particle from the model. """ #pa = this.body.parent[i] #ch = this.body.child[i] r_min = this.body.rmin r_max = this.body.rmax ks = np.where(this.A[:,i] >=0)[0] nNbrs = len(ks) assert nNbrs > 0 num_x = x.shape[1] x_per_nbr = np.max([1, int(num_x / nNbrs)]) A = xrange(x_per_nbr,num_x+1,x_per_nbr) try: I_nbr = np.min(np.where(p_i<=np.array(A))[0]) except: I_nbr = 0 k = ks[I_nbr] # Select the neighbor particle at random num_x = this.b[k]['x'].shape[1] I_k = np.random.randint(0,num_x,1)[0] x_k = this.b[k]['x'][:,I_k] a = k b = i proposedParticle = np.zeros(this.nodeDim[b]) za = particles.get_pose_def_params(this.particleIdx[a], x_k) na = len(za) mu = this.body.poseDefModelA2B[a][b]['mu'] C = this.body.poseDefModelA2B[a][b]['C'] # Indexes of the conditioning variables if npr.rand()>0.5 or k != this.body.parent[b]: cInd = xrange(0,na) X = za movedType = MT_NBR_Z_PARENT_COND else: l = np.prod(mu.shape) cInd = np.concatenate((xrange(0,na), xrange(l-3,l))) if k == this.body.parent[b]: alpha = npr.rand(3) r_rel = r_min[b,:] + alpha * (r_max[b,:] - r_min[b,:]) X = np.concatenate((za, r_rel)) movedType = MT_NBR_Z_PARENT_AND_ANGLE_COND nb = this.nB[b] # Indexes of the resulting variables rInd = xrange(this.body.nPoseBasis[a], this.body.nPoseBasis[a]+nb) mu_ab, C_ab = ba.compute_conditioned_gaussian(this.body, rInd, cInd, mu, C, np.expand_dims(X, axis=1)) proposedParticle = particles.set_pose_def_params(this.particleIdx[b], proposedParticle, mu_ab) # For the shape parameters, we propagate the same shape zs = particles.get_shape_params(this.particleIdx[a], x_k) proposedParticle = particles.set_shape_params(this.particleIdx[b], proposedParticle, zs) # Get the neighbor points in world frame Paw = particles.particle_to_points(this, x_k, a) # Get the points of the proposed particle Pb = ba.get_part_mesh(this.body, b, mu_ab, zs) # Compute the alignment R, T, cost = ba.align_to_parent(this.body, b, a, Pb, Paw, None) # Add some noise to the spring if this.springSigma != 0: T = npr.normal(T, this.springSigma) r, _ = cv2.Rodrigues(R) proposedParticle = particles.set_pose_params(this.particleIdx[b], proposedParticle, r, T) return proposedParticle, movedType
def get_sample(this, nB_torso, nB_pose, nB_shape, fixedShape=True, add_global_rotation=True, init_torso_rotation=np.zeros((3))): """ Returns the set of points of a model sample obtained with the specified number of basis components. The number of basis components can be a vector of dimension number of parts, or a single value for each part. Also returns the model parameters. @params: nB_torso, integer, the number of PCA components for the pose-dependent deformation model for the torso @params: nB, integer or array, the number of PCA components for the pose-dependent deformation model for the parts @params: fixedShape, boolean, if to use the mean intrinsic shape @params: add_global_rotation, boolean, if to sample a random rotation around the vertical axis for the torso @params: init_torso_rotation, 3-dim array, is the orientation of the sample """ torso = this.parts['torso'] Zp = [None] * this.nParts # If the number of PCA components for the pose-dependent model is not specified, we consider a fixed pose # (this is only used to visualize the model without deformations) if nB_torso is not None and nB_pose is not None: if np.size(nB_pose) == 1: nB_p = nB_pose * np.ones((this.nParts), dtype=np.int) else: nB_p = nB_pose nB_p[torso] = nB_torso fixedPose = False else: fixedPose = True if fixedPose: for b in this.partSet: Zp[b] = np.zeros((this.nPoseBasis[b])) else: for b in this.partSet: if b == this.parts['torso']: idx = np.arange(0, nB_p[torso]) Zp[b] = npr.normal(np.zeros((nB_p[b])), this.posePCA[torso]['sigma'][idx]) else: a = this.parent[b] x = Zp[a] cInd = np.arange(0, nB_p[a]) rInd = np.arange(this.nPoseBasis[a], this.nPoseBasis[a] + nB_p[b]) mu = this.poseDefModelA2B[a][b]['mu'] C = this.poseDefModelA2B[a][b]['C'] mu_ab, C_ab = ba.compute_conditioned_gaussian( this, rInd, cInd, mu, C, x) Zp[b] = mu_ab if fixedShape: zs = np.zeros((this.nShapeBasis[b])) else: zs = npr.normal(this.shapePCA[torso]['M'][0:nB_shape], this.shapePCA[torso]['sigma'][0:nB_shape]) # Generate the mesh Pw = [None] * this.nParts r_abs = np.zeros((this.nParts, 3)) t = np.zeros((this.nParts, 3)) for part in this.partSet: parent = this.parent[part] P = ba.get_part_mesh(this, part, Zp[part], zs) if parent >= 0: R = None R, T, cost = ba.align_to_parent(this, part, parent, P, Pw[parent], R) else: # Add a global rotation to the torso b = this.parts['torso'] if add_global_rotation: alpha = npr.rand(1) r_abs[b, 1] = -np.pi + alpha * 2.0 * np.pi else: r_abs[b, :] = 0.0 R1, _ = cv2.Rodrigues(r_abs[b, :]) R2, _ = cv2.Rodrigues(init_torso_rotation) R = np.matrix(R2) * np.matrix(R1) r, _ = cv2.Rodrigues(R) r_abs[b, :] = r[:, 0] T = np.zeros((3)) Pw[part] = ba.object_to_world(P, R, T) r, _ = cv2.Rodrigues(R) r_abs[part, :] = r.flatten() t[part, :] = T.flatten() return Pw, r_abs, t, Zp, zs
def get_sample(this, nB_torso, nB_pose, nB_shape, fixedShape=True, add_global_rotation=True, init_torso_rotation=np.zeros((3))): """ Returns the set of points of a model sample obtained with the specified number of basis components. The number of basis components can be a vector of dimension number of parts, or a single value for each part. Also returns the model parameters. @params: nB_torso, integer, the number of PCA components for the pose-dependent deformation model for the torso @params: nB, integer or array, the number of PCA components for the pose-dependent deformation model for the parts @params: fixedShape, boolean, if to use the mean intrinsic shape @params: add_global_rotation, boolean, if to sample a random rotation around the vertical axis for the torso @params: init_torso_rotation, 3-dim array, is the orientation of the sample """ torso = this.parts['torso'] Zp = [None] * this.nParts # If the number of PCA components for the pose-dependent model is not specified, we consider a fixed pose # (this is only used to visualize the model without deformations) if nB_torso is not None and nB_pose is not None: if np.size(nB_pose) == 1: nB_p = nB_pose*np.ones((this.nParts), dtype=np.int) else: nB_p = nB_pose nB_p[torso] = nB_torso fixedPose = False else: fixedPose = True if fixedPose: for b in this.partSet: Zp[b] = np.zeros((this.nPoseBasis[b])) else: for b in this.partSet: if b == this.parts['torso']: idx = np.arange(0, nB_p[torso]) Zp[b] = npr.normal(np.zeros((nB_p[b])), this.posePCA[torso]['sigma'][idx]) else: a = this.parent[b] x = Zp[a] cInd = np.arange(0,nB_p[a]) rInd = np.arange(this.nPoseBasis[a],this.nPoseBasis[a]+nB_p[b]) mu = this.poseDefModelA2B[a][b]['mu'] C = this.poseDefModelA2B[a][b]['C'] mu_ab, C_ab = ba.compute_conditioned_gaussian(this, rInd, cInd, mu, C, x) Zp[b] = mu_ab if fixedShape: zs = np.zeros((this.nShapeBasis[b])) else: zs = npr.normal(this.shapePCA[torso]['M'][0:nB_shape], this.shapePCA[torso]['sigma'][0:nB_shape]) # Generate the mesh Pw = [None]*this.nParts r_abs = np.zeros((this.nParts, 3)) t = np.zeros((this.nParts, 3)) for part in this.partSet: parent = this.parent[part] P = ba.get_part_mesh(this, part, Zp[part], zs) if parent >= 0: R = None R, T, cost = ba.align_to_parent(this, part, parent, P, Pw[parent], R) else: # Add a global rotation to the torso b = this.parts['torso'] if add_global_rotation: alpha = npr.rand(1) r_abs[b,1] = -np.pi + alpha * 2.0*np.pi else: r_abs[b,:] = 0.0 R1, _ = cv2.Rodrigues(r_abs[b,:]) R2, _ = cv2.Rodrigues(init_torso_rotation) R = np.matrix(R2)*np.matrix(R1) r, _ = cv2.Rodrigues(R) r_abs[b,:] = r[:,0] T = np.zeros((3)) Pw[part] = ba.object_to_world(P, R, T) r, _ = cv2.Rodrigues(R) r_abs[part, :] = r.flatten() t[part,:] = T.flatten() return Pw, r_abs, t, Zp, zs
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 sample_from_nbr(this, i, x, p_i): """ Sample a new particle by looking at the neighbors. We first pick a neighbor, and then generate a particle from the model. """ #pa = this.body.parent[i] #ch = this.body.child[i] r_min = this.body.rmin r_max = this.body.rmax ks = np.where(this.A[:, i] >= 0)[0] nNbrs = len(ks) assert nNbrs > 0 num_x = x.shape[1] x_per_nbr = np.max([1, int(num_x / nNbrs)]) A = xrange(x_per_nbr, num_x + 1, x_per_nbr) try: I_nbr = np.min(np.where(p_i <= np.array(A))[0]) except: I_nbr = 0 k = ks[I_nbr] # Select the neighbor particle at random num_x = this.b[k]['x'].shape[1] I_k = np.random.randint(0, num_x, 1)[0] x_k = this.b[k]['x'][:, I_k] a = k b = i proposedParticle = np.zeros(this.nodeDim[b]) za = particles.get_pose_def_params(this.particleIdx[a], x_k) na = len(za) mu = this.body.poseDefModelA2B[a][b]['mu'] C = this.body.poseDefModelA2B[a][b]['C'] # Indexes of the conditioning variables if npr.rand() > 0.5 or k != this.body.parent[b]: cInd = xrange(0, na) X = za movedType = MT_NBR_Z_PARENT_COND else: l = np.prod(mu.shape) cInd = np.concatenate((xrange(0, na), xrange(l - 3, l))) if k == this.body.parent[b]: alpha = npr.rand(3) r_rel = r_min[b, :] + alpha * (r_max[b, :] - r_min[b, :]) X = np.concatenate((za, r_rel)) movedType = MT_NBR_Z_PARENT_AND_ANGLE_COND nb = this.nB[b] # Indexes of the resulting variables rInd = xrange(this.body.nPoseBasis[a], this.body.nPoseBasis[a] + nb) mu_ab, C_ab = ba.compute_conditioned_gaussian(this.body, rInd, cInd, mu, C, np.expand_dims(X, axis=1)) proposedParticle = particles.set_pose_def_params(this.particleIdx[b], proposedParticle, mu_ab) # For the shape parameters, we propagate the same shape zs = particles.get_shape_params(this.particleIdx[a], x_k) proposedParticle = particles.set_shape_params(this.particleIdx[b], proposedParticle, zs) # Get the neighbor points in world frame Paw = particles.particle_to_points(this, x_k, a) # Get the points of the proposed particle Pb = ba.get_part_mesh(this.body, b, mu_ab, zs) # Compute the alignment R, T, cost = ba.align_to_parent(this.body, b, a, Pb, Paw, None) # Add some noise to the spring if this.springSigma != 0: T = npr.normal(T, this.springSigma) r, _ = cv2.Rodrigues(R) proposedParticle = particles.set_pose_params(this.particleIdx[b], proposedParticle, r, T) return proposedParticle, movedType