いぬおさんのおもしろ数学実験室

おいしい紅茶でも飲みながら数学、物理、工学、プログラミング、そして読書を楽しみましょう

写真から立体を再現(13)一応、フィナーレ!!

 ここまでに紹介した事実を使って、理屈も(ある程度)説明しましょう。Pythonのコードも示します。過去の記事で使うのは以下です。必要に応じて参照してください。
www.omoshiro-suugaku.com
www.omoshiro-suugaku.com
www.omoshiro-suugaku.com
www.omoshiro-suugaku.com
 次の2枚の写真から、ティッシュの箱の隣にある半透明の円柱を重ねた立体をPC内で再現してみます。立体そのものではなく、立体にマジックで打った点(17点あります)が空間内のどこにあるのか、ちゃんとPC内で立体的に配置するのが目的です。空間内の座標を求めるには基準になる(例えば)立方体が必要です。その立方体も一緒に写真に撮っています。ティッシュの箱の、赤マルで囲った点はおおよそ立方体のように並んでいますよね。6点だけですが、今回の目的にはそれで十分です。これらの点の空間内の座標を(0, 0, 1), (1, 0, 1), ……と仮定して、問題の立体の上に打った17点の座標を計算し、空間内に配置します。
f:id:Inuosann:20200424180930p:plain:w350
f:id:Inuosann:20200424181035p:plain:w350
赤マルの点が6点。写真の1枚目を考えます。写真上での座標(写真の左上が原点)を調べ、次の式を得ます。行列の部分は成分がまだ不明です。
f:id:Inuosann:20200424181115p:plain:w300
この式から(sを消去して書き方を変えれば)
f:id:Inuosann:20200424181236p:plain:w400
を得ます。空間点(X, Y, Z)に対して写真上の座標が(u, v)です。こういう点のペアが6組ありますから、これらをまとめれば左辺の行列は12×11に、右辺のベクトルは12個の成分からなるベクトルになります。これをAp=vと表しましょう。pを求めるにはAの一般逆行列をvに左からかければよいのでした。こうして1枚目の写真のカメラ行列が求まります(ベクトルpを3×4型に直すのです)。同様に2枚目の写真でもカメラ行列Bを求めます。ここまでで2枚の写真を撮った際のカメラ行列が明らかになりました。
 ここから立体上に打った点の座標を求める作業に入ります。分かったカメラ行列を
f:id:Inuosann:20200424181353p:plain:w400
 としておきましょう。同じ空間点に対し、写真は2枚(同じ空間点が1枚目にも2枚目にも写っている)ありますから次の2式が成立します。
f:id:Inuosann:20200424181426p:plain:w300
ここからs、s'を消去してまとめれば次が得られます。
f:id:Inuosann:20200424181531p:plain:w400
これを C と表しておきましょう。の左からCの一般逆行列をかけて得られるのでした。これで空間点が1点、求まりました。同じことを今回は17点分実行すればよいのです。
 ではPythonのコードを。参考に載せますが、ここまでで説明したことをコーディングすればよいだけです。Numpy、OpenCVがインポートされていないと動きません。また、3Dの散布図を描くのに関係するモジュールが必要です。インポートしてください。なお上の説明の行列A、Bはカメラ行列ですが、コードではA、Bは別の意味に使っています。過去の記事で特に使っているのは一般逆行列、マウスで画像上の点をポイントして座標を得ること、3Dの散布図の描き方あたりです。

import sys
import numpy as np
import cv2
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
#-----------------------------------
#初期値
#
verPairNum = 17 #2枚の写真で、対応させるペアの数
i = 0
#-----------------------------------
list6A = np.zeros([6, 5]) #XYZuv 6組分
list6B = np.zeros([6, 5])
listM = np.zeros([verPairNum * 2, 2]) #uvのペアをverPairNum個
P = np.zeros([3, 3, 4]) #3*4行列を3個分
P2 = np.zeros([3, 3, 4]) #3*4行列を3個分
P3 = np.zeros([3, 3, 4]) #3*4行列を3個分
X = np.zeros([64, 3]) #3次元ベクトルを6個分
#-----------------------------------
#list6A, list6B に空間点を6点をセット
#
def set3p(p0, p1, p2, p3, p4, p5):
    list6A[0, 0:3] = p0
    list6A[1, 0:3] = p1
    list6A[2, 0:3] = p2
    list6A[3, 0:3] = p3
    list6A[4, 0:3] = p4
    list6A[5, 0:3] = p5
    list6B[0, 0:3] = p0
    list6B[1, 0:3] = p1
    list6B[2, 0:3] = p2
    list6B[3, 0:3] = p3
    list6B[4, 0:3] = p4
    list6B[5, 0:3] = p5
#-----------------------------------
# カメラ行列 P = 3*4 と写真の点 m = 2*1 から行列 M1 = 2*3 を作る
#
def getM1(P, m):
    M1 = np.zeros([2, 3])
    p11 = P[0][0]
    p12 = P[0][1]
    p13 = P[0][2]
    p21 = P[1][0]
    p22 = P[1][1]
    p23 = P[1][2]
    p31 = P[2][0]
    p32 = P[2][1]
    p33 = P[2][2]
    u = m[0]
    v = m[1]
    M1 = [[p31*u - p11, p32*u - p12, p33*u - p13],\
        [p31*v - p21, p32*v - p22, p33*v - p23]]
    return M1
#-----------------------------------
# カメラ行列 P1, P2 = 3*4 と
# 写真の点 m1, m2 = 2*1 から行列 M = 4*3 を作る。
#
def getM(P1, P2, m1, m2):
    M = np.zeros([4, 3])
    M1 = getM1(P1, m1)
    M2 = getM1(P2, m2)
    M[0:2, 0:3] = M1
    M[2:4, 0:3] = M2
    return M 
#-----------------------------------
#
def getv1(P, m):
    w = np.zeros([2, 1])
    p14 = P[0][3]
    p24 = P[1][3]
    p34 = P[2][3]
    u = m[0]
    v = m[1]
    w[0] = p14 - p34*u
    w[1] = p24 - p34*v
    return w    
#-----------------------------------
# 空間点を求める式の右辺を求める
#
def getv(P2, P3, m2, m3):
    w = np.zeros([4, 1])
    w1 = getv1(P2, m2)
    w2 = getv1(P3, m3)
    w[0:2] = w1
    w[2:4] = w2
    return w   
#-----------------------------------
#カメラ行列がP2, P3、写真の対応点がm2, m3
#空間点を一般逆行列で求める。
#
def getX(P2, P3, m2, m3):
    M = getM(P2, P3, m2, m3)
    v = getv(P2, P3, m2, m3)
    ginv = np.linalg.pinv(M) #Aの一般逆行列を求める
    print('ginv =\n', ginv)
    X = np.dot(ginv, v) #一般逆行列を左からかける
    return X
#-----------------------------------
#コールバック関数
#マウスイベントが起こるとここへ来る
def getClickPt(event, x, y, flags, param):
    global i
    if event == cv2.EVENT_LBUTTONDOWN:
        if i < 6:
            list6A[i, 3:] = [x, y]
        elif 6 <= i < 12:
            list6B[i-6, 3:] = [x, y]
        else: #ここまでで、立方体による校正用の点は入力済み。
            if (i-12)%2 == 0: #1枚目の写真
                listM[i-12] = [x, y]
            else: #2枚目の写真
                listM[i-12] = [x, y]
        i = i + 1
        print([x, y])
    return
#-----------------------------------
#A[i], A[i+1] に点をセット
#
def setPair(A, i, list6):
    X = list6[0]
    Y = list6[1]
    Z = list6[2]
    u = list6[3]
    v = list6[4]
    A[i] = [X, Y, Z, 1, 0, 0, 0, 0, -u*X, -u*Y, -u*Z]
    A[i+1] = [0, 0, 0, 0, X, Y, Z, 1, -v*X, -v*Y, -v*Z]
    return
#----------------------------------------------
# Ap = b を満たす p(を3*4に直した行列)を求めるルーチン
# A:空間点1点で2行分。6点分(12行)の行列。(………… -uX, -uY, -uZ)
# b:写真2枚の6点分の座標
#
def getP(A, b):
    ginv = np.linalg.pinv(A) #Aの一般逆行列を求める
    p = np.dot(ginv, b) #一般逆行列を左からかける
    P2 = np.zeros(12)
    P2[:11] = p
    P2[11] = 1
    return P2.reshape(3, 4)
#----------------------------------------------
#A = 12*11
#list6A = [X,Y,Z,u,v]*6
#
def setMatA(A, list6A):
    for i in range(6):
        setPair(A, 2*i, list6A[i])
    return A
#-----------------------------------
# main routine
#-----------------------------------
img = cv2.imread('gazou1.png')
img2 = cv2.imread('gazou2.png')
#画像のウインドウに名前をつけ、コールバック関数をセット
cv2.imshow('image',img)
cv2.namedWindow('image')
cv2.setMouseCallback('image',getClickPt)
cv2.moveWindow('image', 500, 50)
cv2.imshow('image2',img2)
cv2.namedWindow('image2')
cv2.setMouseCallback('image2',getClickPt)
cv2.moveWindow('image2', 800, 50)
while(1):
    #ESCキーでブレーク
    if cv2.waitKey(20) & 0xFF == 27:
        break
set3p([0,0,1],[1,0,1],[1,1,1],[0,1,1],[1,0,0],[1,1,0])
A = np.zeros([12, 11])
B = np.zeros([12, 11])
A = setMatA(A, list6A)
B = setMatA(B, list6B)
b = np.zeros(12)
c = np.zeros(12)
for i in range(6):
    b[i*2] = list6A[i, 3]
    b[i*2+1] = list6A[i, 4]
for i in range(6):
    c[i*2] = list6B[i, 3]
    c[i*2+1] = list6B[i, 4]
P2 = getP(A, b)
P3 = getP(B, c)
m2 = np.zeros([2])
m3 = np.zeros([2])
for i in range(0, verPairNum * 2, 2):
    m2 = listM[i]
    m3= listM[i+1]
    X[int(i/2)] = getX(P2, P3, m2, m3).flatten()
XT = X[0:verPairNum].T
fig = plt.figure()
ax = fig.add_subplot(1,1,1, projection='3d')
ax.scatter(XT[0], XT[1], XT[2])
plt.show()
sys.exit()

実行結果は以下。
f:id:Inuosann:20200424182710p:plain:w450
 しっかり、上側の円柱側面の5点、下側の円柱側面の3点×4列が空間内の点として座標が得られていることが分かります。最初の写真を再び載せておきましょう。
f:id:Inuosann:20200424180930p:plain:w350
 マウスで写真上の点を拾うときは、写真の1枚目で立方体の頂点で下の絵の順にクリック、2枚目で同じことをします。続いて立体は、1枚目のどこかの点、2枚目のその点の対応点、1枚目のどこかの点、2枚目のその点の対応点、……というようにクリックするようにコーディングしてあります(この辺は各自で判断して好きなようにすればよいかと思います)。立方体で6×2点、立体で17×2点マウスで座標を拾ったら、ESCキーを押します。写真上の点から空間内の座標を計算して3Dの散布図を描くようになっています。
f:id:Inuosann:20200424184626p:plain:w250

2020年4月25日(土) 追記:
この方法ではカメラの内部パラメータが固定されている必要はないと気づきました。写真は2枚撮りますが、ピントを変えようが何をしようが、2枚の写真で独立に上の説明のカメラ行列A、Bを求めるだけなので内部パラメータが変わっても問題ないのでした。