def _test_normal_equations(self):
R, t, xs0, xs1 = self.R, self.t, self.xs0, self.xs1
xs0 = unpr(xs0)
xs1 = unpr(xs1)
c = lambda v: cost(K, dot(R, SO3.exp(v[:3])), t + v[3:], xs0, xs1)
JTJ, JTr = compute_normal_equations(K, R, t, xs0, xs1, residual,
Jresidual)
H = finite_differences.numeric_hessian(c, zeros(6))
print 'numeric hessian (least squares):'
print H
print '2*JTJ (least squares):'
print 2. * JTJ
self.assertJacobian(c, 2. * JTr, zeros(6))
def test_cauchy_hessian(self):
J = np.arange(6).reshape((3, 2)).astype(float)
y = arange(3)[::-1].astype(float)
f = lambda x: dot(J, x) - y
E = lambda x: cauchy_cost(f(x))
x0 = arange(2) + 2.5
r0 = f(x0)
self.assertJacobian(f, J, x0)
self.assertJacobian(f, J, x0)
H_analytic = cauchy_hessian_firstorder(r0, J)
print H_analytic
H_numeric = finite_differences.numeric_hessian(E, x0)[0]
print H_numeric
self.assertArrayEqual(H_analytic, H_numeric)
