対応点からの運動パラメータ推定

##ロバストな回転行列の推定テスト
「画像理解3次元の数理」金谷健一著 記載の、カメラが回転した時の回転行列を、最小化を用いてロバストに推定アルゴリズム。特に擾乱を与えたりはしていませんが、今後ノイズにどれだけロバストなのか評価してみたいと考えています。

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

出力結果:

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

##ロバストな基本行列分解テスト
同じく同著記載の対応点のペアのリストから最小化を用いて運動パラメータを推定するアルゴリズム。

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

出力結果:

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  ]

擾乱が無い条件では、設定値と推定値(_hat)がほぼ一致しています。