def sample_from_randwalk(this, i, x, p_i):
"""
@params: this, a Dpmp object
@params: i, integer, the index of the node or part
@params: x, (D,N) array of particles, where D is the particle dimension and N the number of particles
@params: p_i, integer, the index of the particle we take the random walk from
"""
if this.use_map_particle_for_rnd_walk:
p = this.mapParticleInd[i]
else:
p = p_i
p_value = 0.5
if npr.rand() > p_value:
movedType = MT_RND_ALL
sigma = particles.get_noise_std(this, i, x[:, p])
proposedParticle = npr.normal(x[:, p], sigma)
# Generate a random Rodrigues vector
r = npr.normal(
np.zeros(3),
np.array([
this.particle_rSigma, this.particle_rSigma,
this.particle_rSigma
]))
R, _ = cv2.Rodrigues(r)
Rx, _ = cv2.Rodrigues(x[0:3, p])
Rt = np.matrix(R) * np.matrix(Rx)
rt, _ = cv2.Rodrigues(Rt)
proposedParticle[0:3] = rt[:, 0]
else:
movedType = MT_RND_Z_POSE
# Instead of a random walk from the current value for all parameters, resample only the pose-dependent deformations coefficients
# from the whole distribution
proposedParticle = x[:, p].copy()
nB = this.nB[i]
zp = npr.normal(np.zeros(nB), this.body.posePCA[i]['sigma'][0:nB])
proposedParticle = particles.set_pose_def_params(
this.particleIdx[i], proposedParticle, zp)
return proposedParticle, movedType
def sample_from_randwalk(this, i, x, p_i):
"""
@params: this, a Dpmp object
@params: i, integer, the index of the node or part
@params: x, (D,N) array of particles, where D is the particle dimension and N the number of particles
@params: p_i, integer, the index of the particle we take the random walk from
"""
if this.use_map_particle_for_rnd_walk:
p = this.mapParticleInd[i]
else:
p = p_i
p_value = 0.5
if npr.rand() > p_value:
movedType = MT_RND_ALL
sigma = particles.get_noise_std(this, i, x[:,p])
proposedParticle = npr.normal(x[:,p], sigma)
# Generate a random Rodrigues vector
r = npr.normal(np.zeros(3), np.array([this.particle_rSigma, this.particle_rSigma, this.particle_rSigma]))
R, _ = cv2.Rodrigues(r)
Rx, _ = cv2.Rodrigues(x[0:3,p])
Rt = np.matrix(R)*np.matrix(Rx)
rt, _ = cv2.Rodrigues(Rt)
proposedParticle[0:3] = rt[:,0]
else:
movedType = MT_RND_Z_POSE
# Instead of a random walk from the current value for all parameters, resample only the pose-dependent deformations coefficients
# from the whole distribution
proposedParticle = x[:, p].copy()
nB = this.nB[i]
zp = npr.normal(np.zeros(nB), this.body.posePCA[i]['sigma'][0:nB])
proposedParticle = particles.set_pose_def_params(this.particleIdx[i], proposedParticle, zp)
return proposedParticle, movedType
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 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
