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]
