A robust estimation algorithm that uses minimization to estimate the rotation matrix when the camera rotates, as described by Kenichi Kanatani in "Image Understanding 3D Mathematics". I haven't caused any disturbance, but I would like to evaluate how robust it is to noise in the future.
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
Output result:
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]]
An algorithm that estimates motion parameters using minimization from a list of pairs of corresponding points also described in the same book.
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
Output result:
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 ]
Under the condition that there is no disturbance, the set value and the estimated value (_hat) are almost the same.