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)