Un algorithme d'estimation robuste qui utilise la minimisation pour estimer la matrice de rotation lorsque la caméra tourne, décrit par Kenichi Kanaya, «Mathématiques tridimensionnelles pour la compréhension de l'image». Je n'ai pas donné de perturbations particulières, mais j'aimerais évaluer leur résistance au bruit à l'avenir.
estimate_R_with_polar_decomp_or_SVD.py
r = array([0,0, pi/3])
R = cv2.Rodrigues(r)[0]
h = random.rand(3) * 100
h /= norm(h)
N = zeros((3,3))
for i in xrange(1000):
m = random.rand(3)
m = m / norm(m)
m2 = R.T.dot(m)
N += m[:, None] * m2
Rp, S = polar(N)
U, s, Vt = svd(N)
Rs = U.dot(Vt)
print "True value ="
print R
print ""
print "Estimate value with polar decomposition ="
print Rp
print ""
print "Estimate value with SVD ="
print Rs
Résultat de sortie:
True value =
[[ 5.00000000e-01 -8.66025404e-01 -1.16573418e-15]
[ 8.66025404e-01 5.00000000e-01 1.27675648e-15]
[ -2.41126563e-16 -1.60982339e-15 1.00000000e+00]]
Estimate value with polar decomposition =
[[ 5.00000000e-01 -8.66025404e-01 2.63677968e-16]
[ 8.66025404e-01 5.00000000e-01 5.55111512e-17]
[ 0.00000000e+00 2.22044605e-16 1.00000000e+00]]
Estimate value with SVD =
[[ 5.00000000e-01 -8.66025404e-01 2.63677968e-16]
[ 8.66025404e-01 5.00000000e-01 5.55111512e-17]
[ 0.00000000e+00 2.22044605e-16 1.00000000e+00]]
Un algorithme qui estime les paramètres de mouvement en utilisant la minimisation à partir d'une liste de paires de points correspondants également décrits dans le même livre.
G = array([cross(h, R[:,0]), cross(h, R[:, 1]), cross(h, R[:,2])])
G_tild = G.flatten()
G_tild = G_tild / norm(G_tild) * sqrt(2.)
n = 100
M_tild = zeros((9,9))
randlst = []
for i in range(n):
vec = random.rand(3)
vec = vec / norm(vec)
randlst += [vec]
for m2 in randlst:
m = R.dot(m2) + h
mi = tensor(m, "i", "d")
m2j = tensor(m2, "j", "d")
mk = tensor(m, "k", "d")
m2l = tensor(m2, "l", "d")
M_t = mi * m2j * mk * m2l
M_t.transpose("ijkl")
M_tild += M_t.arr.reshape((9,9))
w, v = eigh(M_tild)
G_tild2 = v[:, argmin(w)] / norm(v[:, argmin(w)]) * sqrt(2)
G_hat = G_tild2.reshape((3,3)).T
#print G_hat
w, v = eigh(G_hat.dot(G_hat.T))
h_hat = v[:, argmin(w)] / norm(v[:, argmin(w)])
#input(G_hat)
ep = tensor_ps(3, idx="ikl", ud="ddd")
G_hat_t = tensor(G_hat, "kj", "ud")
h_hat_t = tensor(h_hat, "l", "u")
K_hat = ep * G_hat_t * h_hat_t
K_hat.transpose("ij")
K_hat = K_hat.arr
R_hat, S = polar(K_hat)
print "G="
print G
print "G_hat="
print G_hat
print "K_hat"
print K_hat
print "R="
print R
print "R_hat="
print R_hat
print "h="
print h
print "h_hat="
prod = 0
for m2 in randlst:
m = R.dot(m2) + h
prod += cross(h, m).dot(G_hat.dot(m2))
h_hat = h_hat if prod > 0 else -h_hat
print h_hat
Résultat de sortie:
G=
[[-0.01710211 0.16271571 -0.67909514]
[-0.16271571 -0.01710211 0.71558431]
[ 0.75017391 -0.6406795 0. ]]
G_hat=
[[ 1.71021107e-02 1.62715715e-01 -7.50173912e-01]
[ -1.62715715e-01 1.71021107e-02 6.40679496e-01]
[ 6.79095138e-01 -7.15584312e-01 4.34179630e-14]]
K_hat
[[ 0.5360617 -0.53961079 -0.10482285]
[-0.43228422 0.48508244 -0.12273745]
[-0.11707818 -0.11110811 0.97323111]]
R=
[[ 0.9945219 -0.10452846 0. ]
[ 0.10452846 0.9945219 0. ]
[ 0. 0. 1. ]]
R_hat=
[[ 9.94521895e-01 -1.04528463e-01 3.75532938e-13]
[ 1.04528463e-01 9.94521895e-01 4.25284807e-13]
[ -4.18137747e-13 -3.83595933e-13 1.00000000e+00]]
h=
[ 0.6406795 0.75017391 0.163612 ]
h_hat=
[ 0.6406795 0.75017391 0.163612 ]
À condition qu'il n'y ait pas de perturbation, la valeur de consigne et la valeur estimée (_hat) sont presque identiques.