ここまでに紹介した事実を使って、理屈も（ある程度）説明しましょう。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点の座標を計算し、空間内に配置します。
赤マルの点が6点。写真の1枚目を考えます。写真上での座標（写真の左上が原点）を調べ、次の式を得ます。行列の部分は成分がまだ不明です。
この式から（ｓを消去して書き方を変えれば）
を得ます。空間点(X, Y, Z)に対して写真上の座標が(u, v)です。こういう点のペアが6組ありますから、これらをまとめれば左辺の行列は12×11に、右辺のベクトルは12個の成分からなるベクトルになります。これをAｐ＝ｖと表しましょう。ｐを求めるにはAの一般逆行列をｖに左からかければよいのでした。こうして1枚目の写真のカメラ行列が求まります（ベクトルｐを3×4型に直すのです）。同様に2枚目の写真でもカメラ行列Bを求めます。ここまでで2枚の写真を撮った際のカメラ行列が明らかになりました。
ここから立体上に打った点の座標を求める作業に入ります。分かったカメラ行列を
としておきましょう。同じ空間点に対し、写真は2枚（同じ空間点が1枚目にも2枚目にも写っている）ありますから次の2式が成立します。
ここからｓ、ｓ'を消去してまとめれば次が得られます。
これを 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()実行結果は以下。
しっかり、上側の円柱側面の5点、下側の円柱側面の3点×4列が空間内の点として座標が得られていることが分かります。最初の写真を再び載せておきましょう。
マウスで写真上の点を拾うときは、写真の1枚目で立方体の頂点で下の絵の順にクリック、2枚目で同じことをします。続いて立体は、1枚目のどこかの点、2枚目のその点の対応点、1枚目のどこかの点、2枚目のその点の対応点、……というようにクリックするようにコーディングしてあります（この辺は各自で判断して好きなようにすればよいかと思います）。立方体で6×2点、立体で17×2点マウスで座標を拾ったら、ESCキーを押します。写真上の点から空間内の座標を計算して3Dの散布図を描くようになっています。
2020年4月25日(土) 追記：
この方法ではカメラの内部パラメータが固定されている必要はないと気づきました。写真は2枚撮りますが、ピントを変えようが何をしようが、2枚の写真で独立に上の説明のカメラ行列A、Bを求めるだけなので内部パラメータが変わっても問題ないのでした。
