def move_with_LM(this, i, x):
"""
Modify the particle x by running few iterations of the Levenberg-Marquardt algorithm
(see Robust Registration of 2D and 3D Point Sets, A. Fitzgibbon)
"""
proposedParticle = x
xk = x.copy()
r, t = particles.get_pose_params(this.particleIdx[i], xk)
z = particles.get_pose_def_params(this.particleIdx[i], xk)
q = np.concatenate([r, t, z])
qprev = q
k = 1
eprev = np.inf
eprev2 = np.inf
finito = False
I = np.eye(len(q))
Lambda = 0.1
B = this.body.posePCA[i]['B']
nB_p = len(z)
B = B[:,0:nB_p]
nP = B.shape[0]/3
Jz = [None]*nB_p
D0 = np.zeros((nP, 3, nB_p))
while (k<this.LMsteps) and not finito:
xk[this.particleIdx[i]['rIdx']] = q[0:3]
xk[this.particleIdx[i]['OIdx']] = q[3:6]
xk[this.particleIdx[i]['zPoseIdx']] = q[6:]
P = particles.particle_to_points(this, xk, i)
P = P[0::this.resolStep,:]
r = q[0:3]
R, Jr = cv2.Rodrigues(r)
out = this.kdtree.query(P)
M = this.kdpoints[out[1],:]
# Residuals
e = P - M
ex = e[:,0]
ey = e[:,1]
ez = e[:,2]
Px = P[:,0]
Py = P[:,1]
Pz = P[:,2]
# Jacobian
J4 = 2*ex
J5 = 2*ey
J6 = 2*ez
J1 = J4*(Jr[0,0]*Px + Jr[0,1]*Py + Jr[0,2]*Pz) + J5*(Jr[0,3]*Px + Jr[0,4]*Py + Jr[0,5]*Pz) + J6*(Jr[0,6]*Px + Jr[0,7]*Py + Jr[0,8]*Pz)
J2 = J4*(Jr[1,0]*Px + Jr[1,1]*Py + Jr[1,2]*Pz) + J5*(Jr[1,3]*Px + Jr[1,4]*Py + Jr[1,5]*Pz) + J6*(Jr[1,6]*Px + Jr[1,7]*Py + Jr[1,8]*Pz)
J3 = J4*(Jr[2,0]*Px + Jr[2,1]*Py + Jr[2,2]*Pz) + J5*(Jr[2,3]*Px + Jr[2,4]*Py + Jr[2,5]*Pz) + J6*(Jr[2,6]*Px + Jr[2,7]*Py + Jr[2,8]*Pz)
# Partial derivatives of the residuals with respect to the PCA coefficients
A = B*z
D0[:,0,:] = A[0::3,:]
D0[:,1,:] = A[1::3,:]
D0[:,2,:] = A[2::3,:]
D = D0[0::this.resolStep,:,:]
for l in xrange(nB_p):
D[:,:,l] = np.squeeze(D[:,:,l])*np.matrix(R)
Jz[l] = J4*D[:,0,l]+J5*D[:,1,l]+J6*D[:,2,l];
J = np.vstack([J1, J2, J3, J4, J5, J6, Jz])
e = np.sum(e**2, axis=1)
J = np.matrix(J)
dq = - (J*J.T+Lambda*I).I*J*np.matrix(e).T
q = np.array((q+dq.flatten()))[0]
e = np.mean(e)
if e > eprev:
Lambda = Lambda*10
q = qprev.copy()
else:
Lambda = Lambda/10
eprev2 = eprev
eprev = e
qprev = q.copy()
k = k+1
finito = abs(eprev - eprev2) < 1e-6
proposedParticle = particles.set_pose_params(this.particleIdx[i], proposedParticle, q[0:3], q[3:6])
return proposedParticle
def move_with_LM(this, i, x):
"""
Modify the particle x by running few iterations of the Levenberg-Marquardt algorithm
(see Robust Registration of 2D and 3D Point Sets, A. Fitzgibbon)
"""
proposedParticle = x
xk = x.copy()
r, t = particles.get_pose_params(this.particleIdx[i], xk)
z = particles.get_pose_def_params(this.particleIdx[i], xk)
q = np.concatenate([r, t, z])
qprev = q
k = 1
eprev = np.inf
eprev2 = np.inf
finito = False
I = np.eye(len(q))
Lambda = 0.1
B = this.body.posePCA[i]['B']
nB_p = len(z)
B = B[:, 0:nB_p]
nP = B.shape[0] / 3
Jz = [None] * nB_p
D0 = np.zeros((nP, 3, nB_p))
while (k < this.LMsteps) and not finito:
xk[this.particleIdx[i]['rIdx']] = q[0:3]
xk[this.particleIdx[i]['OIdx']] = q[3:6]
xk[this.particleIdx[i]['zPoseIdx']] = q[6:]
P = particles.particle_to_points(this, xk, i)
P = P[0::this.resolStep, :]
r = q[0:3]
R, Jr = cv2.Rodrigues(r)
out = this.kdtree.query(P)
M = this.kdpoints[out[1], :]
# Residuals
e = P - M
ex = e[:, 0]
ey = e[:, 1]
ez = e[:, 2]
Px = P[:, 0]
Py = P[:, 1]
Pz = P[:, 2]
# Jacobian
J4 = 2 * ex
J5 = 2 * ey
J6 = 2 * ez
J1 = J4 * (Jr[0, 0] * Px + Jr[0, 1] * Py + Jr[0, 2] * Pz) + J5 * (
Jr[0, 3] * Px + Jr[0, 4] * Py + Jr[0, 5] * Pz) + J6 * (
Jr[0, 6] * Px + Jr[0, 7] * Py + Jr[0, 8] * Pz)
J2 = J4 * (Jr[1, 0] * Px + Jr[1, 1] * Py + Jr[1, 2] * Pz) + J5 * (
Jr[1, 3] * Px + Jr[1, 4] * Py + Jr[1, 5] * Pz) + J6 * (
Jr[1, 6] * Px + Jr[1, 7] * Py + Jr[1, 8] * Pz)
J3 = J4 * (Jr[2, 0] * Px + Jr[2, 1] * Py + Jr[2, 2] * Pz) + J5 * (
Jr[2, 3] * Px + Jr[2, 4] * Py + Jr[2, 5] * Pz) + J6 * (
Jr[2, 6] * Px + Jr[2, 7] * Py + Jr[2, 8] * Pz)
# Partial derivatives of the residuals with respect to the PCA coefficients
A = B * z
D0[:, 0, :] = A[0::3, :]
D0[:, 1, :] = A[1::3, :]
D0[:, 2, :] = A[2::3, :]
D = D0[0::this.resolStep, :, :]
for l in xrange(nB_p):
D[:, :, l] = np.squeeze(D[:, :, l]) * np.matrix(R)
Jz[l] = J4 * D[:, 0, l] + J5 * D[:, 1, l] + J6 * D[:, 2, l]
J = np.vstack([J1, J2, J3, J4, J5, J6, Jz])
e = np.sum(e**2, axis=1)
J = np.matrix(J)
dq = -(J * J.T + Lambda * I).I * J * np.matrix(e).T
q = np.array((q + dq.flatten()))[0]
e = np.mean(e)
if e > eprev:
Lambda = Lambda * 10
q = qprev.copy()
else:
Lambda = Lambda / 10
eprev2 = eprev
eprev = e
qprev = q.copy()
k = k + 1
finito = abs(eprev - eprev2) < 1e-6
proposedParticle = particles.set_pose_params(this.particleIdx[i],
proposedParticle, q[0:3],
q[3:6])
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 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