Estimation des paramètres moteurs à partir des points correspondants

Test d'estimation de matrice de rotation robuste

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]]

Test de décomposition de matrice de base robuste

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.

Recommended Posts

Estimation des paramètres moteurs à partir des points correspondants
Extrait des paramètres de l'histogramme matplotlib
Estimation des paramètres avec le filtre de Kalman