def multiple_gradient_test(Ntests): scene = Scene() l1 = zeros((Ntests, 4)) l2 = zeros((Ntests, 4)) l3 = zeros((Ntests, 4)) l4 = zeros((Ntests, 4)) lang = zeros(Ntests) for k in xrange(Ntests): qori = random_quaternion() corisco = corisco_module.Corisco(scene.project(qori)) qrand = random_quaternion() # Test gradient at solution. Should be pretty close to 0. l1[k] = corisco.target_function_gradient(qori.q) l2[k] = corisco.target_function_gradient_numeric(qori.q, dx=1e-6) # Test gradient at random point l3[k] = corisco.target_function_gradient(qrand.q) l4[k] = corisco.target_function_gradient_numeric(qrand.q, dx=1e-6) lang[k] = dot(qrand.q, l3[k]) print "Gradient projection on quaternion should be pretty low", mean(lang), std(lang) print "Mean and covariances from gradients at the solution, arithmetic and " "numeric. Should all be low." print mean(l1, 0) print cov(l1.T) print mean(l2, 0) print cov(l2.T) print "Covariances from gradients at generic points, and the difference " "between the arithmetic and numeric." print cov(l3.T) print cov(l4.T) print cov((l3 - l4).T)
def single_gradient_test(Ntest_iters): scene = Scene() dq = 2e-2 qq = [ Quat(1.0, 0, 0, 0), Quat(1.0, dq, 0, 0), Quat(1.0, -dq, 0, 0), Quat(1.0, 0, dq, 0), Quat(1.0, 0, -dq, 0), Quat(1.0, 0, 0, dq), Quat(1.0, 0, 0, -dq), ] qori = random_quaternion().canonical() # qori = Quat(1.0,0,0,0).normalize() # qori = Quat(0.0,1,0,0).normalize() # qori = Quat(1.0,0.1,0.1,0).normalize() print qori print for qdelta in qq: g1, g2 = corisco_gradient_test(scene, qori, Ntest_iters, qdelta) print g1, dot((qori * qdelta).q, g1) print g2, dot((qori * qdelta).q, g2) print plot_results(scene, qori, qori * qdelta)
def corisco_ransac_test(scene, Niters): qori = random_quaternion().canonical() edgels = scene.project(qori) ## Estimate orientation using RANSAC. corisco = corisco_module.Corisco(edgels) random_search_result = corisco.ransac_search(Niters)[0] qest = random_search_result.canonical() return qori, qest
def random_pose(): psi = random_quaternion() t = dot(psi.rot(), array([6*rand()-3,6*rand()-3,8.0 + 4 * rand()])) return t, psi.q
def main(): cc = Camera() answer = -pi/12 #ori_correct = Quat(sqrt(1.0 - answer ** 2), answer, 0.0, 0.0) ori_correct = Quat(cos(answer), sin(answer), 0.0, 0.0) edgels = cc.project(ori_correct) co = corisco.Corisco(edgels) fps = array([ [1.0,3.0,0], [2.0,3.0,0], [3.0,1.0,0.15], ]) rc("mathtext",fontset='stixsans') Nang = 2000 angles = .25*pi * mgrid[0.0:Nang + 1] / Nang - pi/4 ccc=['#ea6949', '#51c373', '#a370ff', '#444444'] ck=0 qini = ori_correct * random_quaternion(0.2) co = corisco.Corisco(edgels) qopt = co.estimate_orientation(qini=qini) print qini, co.target_function_value(qini.q) print qopt, co.target_function_value(qopt.q) print ori_correct, co.target_function_value(ori_correct.q) ion() figure() ax = subplot(1,1,1) title('rectrckr simulation') axis('equal') co.plot_vps(ax, ori_correct) co.plot_edgels(ax) axis([0,640,480,0]) figure() ax = subplot(1,1,1) title('Estimation initial state') axis('equal') co.plot_vps(ax, qini) co.plot_edgels(ax) axis([0,640,480,0]) figure() ax = subplot(1,1,1) title('Estimation result') axis('equal') co.plot_vps(ax, qopt) co.plot_edgels(ax) axis([0,640,480,0]) code.interact(local=locals())
def dynamical_model(st): covariance = diag([0.1, 0.1, 0.1]) new_pos = multivariate_normal(st[:3], covariance) new_ori = Quat(st[3:7]) * random_quaternion(0.01) return r_[new_pos, new_ori.q]