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 move_with_LM_only_pose(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)
q = np.concatenate([r, t])
qprev = q
k = 1
eprev = np.inf
eprev2 = np.inf
finito = False
I = np.eye(len(q))
Lambda = 0.1
while (k<this.LMsteps) and not finito:
xk[this.particleIdx[i]['rIdx']] = q[0:3]
xk[this.particleIdx[i]['OIdx']] = q[3: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
J1 = 2*ex*(Jr[0,0]*Px + Jr[0,1]*Py + Jr[0,2]*Pz) + 2*ey*(Jr[0,3]*Px + Jr[0,4]*Py + Jr[0,5]*Pz) + 2*ez*(Jr[0,6]*Px + Jr[0,7]*Py + Jr[0,8]*Pz)
J2 = 2*ex*(Jr[1,0]*Px + Jr[1,1]*Py + Jr[1,2]*Pz) + 2*ey*(Jr[1,3]*Px + Jr[1,4]*Py + Jr[1,5]*Pz) + 2*ez*(Jr[1,6]*Px + Jr[1,7]*Py + Jr[1,8]*Pz)
J3 = 2*ex*(Jr[2,0]*Px + Jr[2,1]*Py + Jr[2,2]*Pz) + 2*ey*(Jr[2,3]*Px + Jr[2,4]*Py + Jr[2,5]*Pz) + 2*ez*(Jr[2,6]*Px + Jr[2,7]*Py + Jr[2,8]*Pz)
J4 = 2*ex
J5 = 2*ey
J6 = 2*ez
J = np.vstack([J1, J2, J3, J4, J5, J6])
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_only_pose(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)
q = np.concatenate([r, t])
qprev = q
k = 1
eprev = np.inf
eprev2 = np.inf
finito = False
I = np.eye(len(q))
Lambda = 0.1
while (k < this.LMsteps) and not finito:
xk[this.particleIdx[i]['rIdx']] = q[0:3]
xk[this.particleIdx[i]['OIdx']] = q[3: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
J1 = 2 * ex * (
Jr[0, 0] * Px + Jr[0, 1] * Py + Jr[0, 2] * Pz) + 2 * ey * (
Jr[0, 3] * Px + Jr[0, 4] * Py + Jr[0, 5] * Pz) + 2 * ez * (
Jr[0, 6] * Px + Jr[0, 7] * Py + Jr[0, 8] * Pz)
J2 = 2 * ex * (
Jr[1, 0] * Px + Jr[1, 1] * Py + Jr[1, 2] * Pz) + 2 * ey * (
Jr[1, 3] * Px + Jr[1, 4] * Py + Jr[1, 5] * Pz) + 2 * ez * (
Jr[1, 6] * Px + Jr[1, 7] * Py + Jr[1, 8] * Pz)
J3 = 2 * ex * (
Jr[2, 0] * Px + Jr[2, 1] * Py + Jr[2, 2] * Pz) + 2 * ey * (
Jr[2, 3] * Px + Jr[2, 4] * Py + Jr[2, 5] * Pz) + 2 * ez * (
Jr[2, 6] * Px + Jr[2, 7] * Py + Jr[2, 8] * Pz)
J4 = 2 * ex
J5 = 2 * ey
J6 = 2 * ez
J = np.vstack([J1, J2, J3, J4, J5, J6])
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
