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_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