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