3,4,5焦点テンソルを用いて対応点を推定するプログラムをPythonで作りました。八木康史他著 コンピュータビジョン最先端ガイド 1 (アドコム・メディア社) を参照しました。
下記、インポートと正解データの設定
import cv2
from numpy.random import *
from numpy.linalg import *
from tensor import *
N = 400 #number of data
coeff = 10
true_x1 = array([10,20,30])
true_x2 = array([20,30,40])
true_x3 = array([30,40,50])
true_x4 = array([60,70,80])
true_x5 = array([90,100,110])
3焦点テンソルの計算
3焦点テンソルの拘束の式は下記の式で記述される。
x^{i}x^{'j}x^{''k}\epsilon_{rja}\epsilon_{skb}\mathcal{T}_{i}^{rs}=0_{ab}
以下、プログラム
def calc_trifocal_tensor(true_x1, true_x2, true_x3, N, coeff):
x1lst = []
x2lst = []
x3lst = []
#substitue random data of position vector to x1, x2 and x3
for i in range(N):
x1 = true_x1 + (random(3) - 0.5) * coeff
x2 = true_x2 + (random(3) - 0.5) * coeff
x3 = true_x3 + (random(3) - 0.5) * coeff
# x1, x2 and x3 is related by R = Rod(0,0,pi/8)
#R = cv2.Rodrigues((0,0,pi/8))[0]
#x2 = R.dot(x1)
#x3 = R.dot(x2)
x1lst += [x1]
x2lst += [x2]
x3lst += [x3]
x_i = tensor(x1lst, "ni", "uu")
x_j = tensor(x2lst, "nj", "uu")
x_k = tensor(x3lst, "nk", "uu")
ep_rja = tensor_ps(3, idx="rja", ud="ddd")
ep_skb = tensor_ps(3, idx="skb", ud="ddd")
# calculating M
M = x_i * (x_j * ep_rja) * (x_k * ep_skb)
# transposing M
M.transpose("abnrsi")
#
M_hat = M.arr.flatten().reshape(N*3*3, 3*3*3)
w,v = eigh(M_hat.T.dot(M_hat))
y = v[:,argmin(w)]
T = tensor(y.reshape(3,3,3), "rsi", "uud")
M = ((x_j * ep_rja) * (x_k * ep_skb)) * T
#M = x_i * ((T * ep_rja) * (x_k * ep_skb))
#M.transpose("nabj")
M.transpose("nabi")
M_hat = M.arr.flatten().reshape(N*3*3, 3)
w,v = eigh(M_hat.T.dot(M_hat))
y = v[:,argmin(w)]
if y[0] < 0: y *= -1
est_x1 = y * linalg.norm(true_x1)
print "Estimate value of x1:"
print est_x1
print "True value of x1:"
print true_x1
print
M = x_i * ((T * ep_rja) * (x_k * ep_skb))
M.transpose("nabj")
M_hat = M.arr.flatten().reshape(N*3*3, 3)
w,v = eigh(M_hat.T.dot(M_hat))
y = v[:,argmin(w)]
if y[0] < 0: y *= -1
est_x2 = y * linalg.norm(true_x2)
print "Estimate value of x2:"
print est_x2
print "True value of x2:"
print true_x2
print
M = x_i * ((x_j * ep_rja) * (T * ep_skb))
M.transpose("nabk")
M_hat = M.arr.flatten().reshape(N*3*3, 3)
w,v = eigh(M_hat.T.dot(M_hat))
y = v[:,argmin(w)]
y * linalg.norm(x3)
if y[0] < 0: y *= -1
est_x3 = y * linalg.norm(true_x3)
print "Estimate value of x3:"
print est_x3
print "True value of x3:"
print true_x3
RMS_of_tri = sum(sqrt((true_x1 - est_x1)**2 + (true_x2 - est_x2)**2 + (true_x3 - est_x3)**2))
print RMS_of_tri
return RMS_of_tri
RMS_of_tri_lst = []
for i in range(10):
RMS_of_tri_lst += [calc_trifocal_tensor(true_x1, true_x2,true_x3,N, coeff)]
RMS_of_tri_mean = array(RMS_of_tri_lst).mean()
4焦点テンソル
4焦点テンソルは以下の式で記述される。
x^{i}x'^{j}x''^{k}x'''^{l}\epsilon_{pja}\epsilon_{rkb}\epsilon_{rkc}\epsilon_{slc}Q_{i}^{qrs} = 0_{abc}
以下、プログラム
def calc_quadrifocal_tensor(true_x1, true_x2, true_x3, true_x4, N, coeff):
x1lst = []
x2lst = []
x3lst = []
x4lst = []
#substitue random data of position vector to x1, x2 and x3
for i in range(N):
x1 = true_x1 + (random(3) - 0.5) * coeff
x2 = true_x2 + (random(3) - 0.5) * coeff
x3 = true_x3 + (random(3) - 0.5) * coeff
x4 = true_x4 + (random(3) - 0.5) * coeff
# x1, x2 and x3 is related by R = Rod(0,0,pi/8)
#R = cv2.Rodrigues((0,0,pi/8))[0]
#x2 = R.dot(x1)
#x3 = R.dot(x2)
x1lst += [x1]
x2lst += [x2]
x3lst += [x3]
x4lst += [x4]
x_i = tensor(x1lst, "ni", "uu")
x_j = tensor(x2lst, "nj", "uu")
x_k = tensor(x3lst, "nk", "uu")
x_l = tensor(x4lst, "nl", "uu")
ep_qja = tensor_ps(3, idx="qja", ud="ddd")
ep_rkb = tensor_ps(3, idx="rkb", ud="ddd")
ep_slc = tensor_ps(3, idx="slc", ud="ddd")
# calculating M
M = x_i * (x_j * ep_qja) * (x_k * ep_rkb) * (x_l * ep_slc)
# transposing M
M.transpose("nabcqrsi")
#
M_hat = M.arr.flatten().reshape(N*3*3*3, 3*3*3*3)
w,v = eigh(M_hat.T.dot(M_hat))
y = v[:,argmin(w)]
Q = tensor(y.reshape(3,3,3,3), "qrsi", "uuud")
M = (x_j * ep_qja) * (x_k * ep_rkb) * (x_l * ep_slc) * Q
#M = ((x_j * ep_rja) * (x_k * ep_skb)) * T
#M = x_i * ((T * ep_rja) * (x_k * ep_skb))
#M.transpose("nabj")
M.transpose("nabci")
M_hat = M.arr.flatten().reshape(N*3*3*3, 3)
w,v = eigh(M_hat.T.dot(M_hat))
y = v[:,argmin(w)]
if y[0] < 0: y *= -1
est_x1 = y * linalg.norm(true_x1)
print "Estimate value of x1:"
print est_x1
print "True value of x1:"
print true_x1
print
M = x_i * ((Q * ep_qja) * (x_k * ep_rkb) * (x_l * ep_slc))
M.transpose("nabcj")
M_hat = M.arr.flatten().reshape(N*3*3*3, 3)
w,v = eigh(M_hat.T.dot(M_hat))
y = v[:,argmin(w)]
if y[0] < 0: y *= -1
est_x2 = y * linalg.norm(true_x2)
print "Estimate value of x2:"
print est_x2
print "True value of x2:"
print true_x2
print
M = x_i * ((x_j * ep_qja) * ((Q * ep_rkb) * (x_l * ep_slc)))
M.transpose("nabck")
M_hat = M.arr.flatten().reshape(N*3*3*3, 3)
w,v = eigh(M_hat.T.dot(M_hat))
y = v[:,argmin(w)]
if y[0] < 0: y *= -1
est_x3 = y * linalg.norm(true_x3)
print "Estimate value of x3:"
print est_x3
print "True value of x3:"
print true_x3
M = x_i * ((x_j * ep_qja) * (x_k * ep_rkb) * (Q * ep_slc))
M.transpose("nabcl")
M_hat = M.arr.flatten().reshape(N*3*3*3, 3)
w,v = eigh(M_hat.T.dot(M_hat))
y = v[:,argmin(w)]
est_x4 = y * linalg.norm(true_x4)
print "Estimate value of x4:"
print est_x4
print "True value of x4:"
print true_x4
RMS_of_quad = sum(sqrt((true_x1 - est_x1)**2 + (true_x2 - est_x2)**2 + (true_x3 - est_x3)**2))
print RMS_of_quad
return RMS_of_quad
RMS_of_quad_lst = []
for i in range(10):
RMS_of_quad_lst += [calc_quadrifocal_tensor(true_x1, true_x2,true_x3,true_x4,N, coeff)]
RMS_of_quad_mean = array(RMS_of_quad_lst).mean()
5焦点テンソル
5焦点テンソルの拘束の式は下記の式で記述される。
x^{i}x'^{j}x''^{k}x'''^{l}x''''^{m}\epsilon_{pia}\epsilon_{qjb}\epsilon_{rkc}\epsilon_{sld}\epsilon_{tme}\mathcal{R}^{pqrst}=0_{abcde}
以下、プログラム
def calc_pentarifocal_tensor(true_x1, true_x2, true_x3, true_x4, true_x5, N, coeff):
#substitue random data of position vector to x1, x2 and x3
x1lst = []
x2lst = []
x3lst = []
x4lst = []
x5lst = []
for i in range(N):
x1 = true_x1 + (random(3) - 0.5) * coeff
x2 = true_x2 + (random(3) - 0.5) * coeff
x3 = true_x3 + (random(3) - 0.5) * coeff
x4 = true_x4 + (random(3) - 0.5) * coeff
x5 = true_x5 + (random(3) - 0.5) * coeff
# x1, x2 and x3 is related by R = Rod(0,0,pi/8)
#R = cv2.Rodrigues((0,0,pi/8))[0]
#x2 = R.dot(x1)
#x3 = R.dot(x2)
x1lst += [x1]
x2lst += [x2]
x3lst += [x3]
x4lst += [x4]
x5lst += [x5]
x_i = tensor(x1lst, "ni", "uu")
x_j = tensor(x2lst, "nj", "uu")
x_k = tensor(x3lst, "nk", "uu")
x_l = tensor(x4lst, "nl", "uu")
x_m = tensor(x5lst, "nm", "uu")
ep_pia = tensor_ps(3, idx="pia", ud="ddd")
ep_qjb = tensor_ps(3, idx="qjb", ud="ddd")
ep_rkc = tensor_ps(3, idx="rkc", ud="ddd")
ep_sld = tensor_ps(3, idx="sld", ud="ddd")
ep_tme = tensor_ps(3, idx="tme", ud="ddd")
# calculating M
M = (x_i * ep_pia) * (x_j * ep_qjb ) * (x_k * ep_rkc) * (x_l * ep_sld) * (x_m * ep_tme)
# transposing M
M.transpose("nabcdepqrst")
#
M_hat = M.arr.flatten().reshape(N*3*3*3*3*3, 3*3*3*3*3)
w,v = eigh(M_hat.T.dot(M_hat))
y = v[:,argmin(w)]
P = tensor(y.reshape(3,3,3,3,3), "pqrst", "uuuuu")
M = (x_j * ep_qjb ) * (x_k * ep_rkc) * (x_l * ep_sld) * (x_m * ep_tme) * (P * ep_pia)
#M = ((x_j * ep_rja) * (x_k * ep_skb)) * T
#M = x_i * ((T * ep_rja) * (x_k * ep_skb))
#M.transpose("nabj")
M.transpose("nabcdei")
M_hat = M.arr.flatten().reshape(N*3*3*3*3*3, 3)
w,v = eigh(M_hat.T.dot(M_hat))
y = v[:,argmin(w)]
if y[0] < 0: y *= -1
est_x1 = y * linalg.norm(true_x1)
print "Estimate value of x1:"
print est_x1
print "True value of x1:"
print true_x1
print
M = (x_i * ep_pia) * (ep_qjb ) * (x_k * ep_rkc) * (x_l * ep_sld) * (x_m * ep_tme) * P
M.transpose("nabcdej")
M_hat = M.arr.flatten().reshape(N*3*3*3*3*3, 3)
w,v = eigh(M_hat.T.dot(M_hat))
y = v[:,argmin(w)]
if y[0] < 0: y *= -1
est_x2 = y * linalg.norm(true_x2)
print "Estimate value of x2:"
print est_x2
print "True value of x2:"
print true_x2
print
M = (x_i * ep_pia) * (x_j * ep_qjb ) * (ep_rkc) * (x_l * ep_sld) * (x_m * ep_tme) * P
M.transpose("nabcdek")
M_hat = M.arr.flatten().reshape(N*3*3*3*3*3, 3)
w,v = eigh(M_hat.T.dot(M_hat))
y = v[:,argmin(w)]
if y[0] < 0: y *= -1
est_x3 = y * linalg.norm(true_x3)
print "Estimate value of x3:"
print est_x3
print "True value of x3:"
print true_x3
M = (x_i * ep_pia) * (x_j * ep_qjb ) * (x_k * ep_rkc) * (ep_sld) * (x_m * ep_tme) * P
M.transpose("nabcdel")
M_hat = M.arr.flatten().reshape(N*3*3*3*3*3, 3)
w,v = eigh(M_hat.T.dot(M_hat))
y = v[:,argmin(w)]
if y[0] < 0: y *= -1
est_x4 = y * linalg.norm(true_x4)
print "Estimate value of x4:"
print est_x4
print "True value of x4:"
print true_x4
M = (x_i * ep_pia) * (x_j * ep_qjb ) * (x_k * ep_rkc) * (x_l * ep_sld) * (ep_tme) * P
M.transpose("nabcdem")
M_hat = M.arr.flatten().reshape(N*3*3*3*3*3, 3)
w,v = eigh(M_hat.T.dot(M_hat))
y = v[:,argmin(w)]
if y[0] < 0: y *= -1
est_x5 = y * linalg.norm(true_x5)
print "Estimate value of x5:"
print y * linalg.norm(true_x5)
print "True value of x5:"
print true_x5
RMS_of_penta = sum(sqrt((true_x1 - est_x1)**2 + (true_x2 - est_x2)**2 + (true_x3 - est_x3)**2))
print RMS_of_penta
return RMS_of_penta
RMS_of_penta_lst = []
for i in range(10):
RMS_of_penta_lst += [calc_pentarifocal_tensor(true_x1, true_x2,true_x3,true_x4,true_x5,N,coeff)]
RMS_of_penta_mean = array(RMS_of_penta_lst).mean()
上記の実行結果は長いため省略。5焦点テンソルは時間がかかりました。
それぞれ10回ずつ実行して誤差の平均をとっています。
以下、3,4,5焦点テンソルから推定した対応点と正解値のそれとの誤差を表示。
print RMS_of_tri_mean, RMS_of_quad_mean, RMS_of_penta_mean
結果、
[3.5723998185257329, 3.0135727733738618, 1.3922678397566748]
3よりも4、4よりも5のほうが誤差が少ないようです。