def nllNormalConditional(d_x_t):
x_t = util.make_rigid_transform(R_x_t, d_x_t)
t1 = m.algi(m.logm(m.inv(x[t - 1]).dot(x_t)))
nll = 0.5 * t1.dot(Qi).dot(t1)
return nll
def test_decomp(s):
T, K, N, grp = (3, 1, 0, 'se2')
o, x, theta, E, S, Q, y, z = tu.GenerateRandomDataset(T, K, N, grp)
m = getattr(lie, o.lie)
# infer d_x_t in forward direction
t = 1
R_x_tminus1, d_x_tminus1 = m.Rt(x[t - 1])
R_x_tplus1, d_x_tplus1 = m.Rt(x[t + 1])
R_x_t, _ = m.Rt(x[t])
V_x_tminus1_x_t_inv = m.getVi(m.inv(x[t - 1]).dot(x[t]))
_, d_x_tminus1_x_t = m.Rt(m.inv(x[t - 1]).dot(x[t]))
u_x_t = m.algi(m.logm(m.inv(x[t - 1]).dot(x[t])))[:-1]
norm = np.linalg.norm(u_x_t - V_x_tminus1_x_t_inv.dot(d_x_tminus1_x_t))
assert norm < 1e-8, 'bad norm'
V_x_t_x_tplus1_inv = m.getVi(m.inv(x[t]).dot(x[t + 1]))
_, d_x_t_x_tplus1 = m.Rt(m.inv(x[t]).dot(x[t + 1]))
u_x_tplus1 = m.algi(m.logm(m.inv(x[t]).dot(x[t + 1])))[:-1]
norm = np.linalg.norm(u_x_tplus1 -
V_x_t_x_tplus1_inv.dot(d_x_t_x_tplus1))
assert norm < 1e-8, 'bad norm'
x_t = util.make_rigid_transform(R_x_t, d_x_tminus1)
V_x_tminus1_x_t_inv = m.getVi(m.inv(x[t - 1]).dot(x_t))
_, d_x_tminus1_x_t = m.Rt(m.inv(x[t - 1]).dot(x_t))
u_x_t = m.algi(m.logm(m.inv(x[t - 1]).dot(x_t)))[:-1]
norm = np.linalg.norm(u_x_t - V_x_tminus1_x_t_inv.dot(d_x_tminus1_x_t))
assert norm < 1e-8, 'bad norm'
x_t = util.make_rigid_transform(R_x_t, d_x_tminus1)
V_x_t_x_tplus1_inv = m.getVi(m.inv(x_t).dot(x[t + 1]))
_, d_x_t_x_tplus1 = m.Rt(m.inv(x_t).dot(x[t + 1]))
u_x_tplus1 = m.algi(m.logm(m.inv(x_t).dot(x[t + 1])))[:-1]
norm = np.linalg.norm(u_x_tplus1 -
V_x_t_x_tplus1_inv.dot(d_x_t_x_tplus1))
assert norm < 1e-8, 'bad norm'