def particle_to_points(dpmp, x, i):
idx = dpmp.particleIdx[i]
Zp = x[[idx['zPoseIdx']]]
Zs = x[[idx['zShapeIdx']]]
T = x[[idx['OIdx']]]
P = dpmp.body.get_part_mesh(i, Zp, Zs)
r_abs = x[[idx['rIdx']]]
R, J = cv2.Rodrigues(r_abs)
Pw = ba.object_to_world(P, R, T)
return Pw
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 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 )
