Ein robuster Schätzalgorithmus, der mithilfe der Minimierung die Rotationsmatrix schätzt, wenn sich die Kamera dreht, beschrieben von Kenichi Kanaya, "Dreidimensionale Mathematik für das Bildverständnis". Ich habe keine besondere Störung angegeben, möchte aber bewerten, wie robust es in Zukunft gegenüber Lärm ist.
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
Ausgabeergebnis:
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]]
Ein Algorithmus, der Bewegungsparameter mithilfe der Minimierung aus einer Liste von Paaren entsprechender Punkte schätzt, die ebenfalls im selben Buch beschrieben sind.
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
Ausgabeergebnis:
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 ]
Unter der Bedingung, dass keine Störung vorliegt, sind der eingestellte Wert und der geschätzte Wert (_hat) nahezu gleich.