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
