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のほうが誤差が少ないようです。