ここまでに紹介した事実を使って、理屈も(ある程度)説明しましょう。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枚目を考えます。写真上での座標(写真の左上が原点)を調べ、次の式を得ます。行列の部分は成分がまだ不明です。
この式から(sを消去して書き方を変えれば)
を得ます。空間点(X, Y, Z)に対して写真上の座標が(u, v)です。こういう点のペアが6組ありますから、これらをまとめれば左辺の行列は12×11に、右辺のベクトルは12個の成分からなるベクトルになります。これをAp=vと表しましょう。pを求めるにはAの一般逆行列をvに左からかければよいのでした。こうして1枚目の写真のカメラ行列が求まります(ベクトルpを3×4型に直すのです)。同様に2枚目の写真でもカメラ行列Bを求めます。ここまでで2枚の写真を撮った際のカメラ行列が明らかになりました。
ここから立体上に打った点の座標を求める作業に入ります。分かったカメラ行列を
としておきましょう。同じ空間点に対し、写真は2枚(同じ空間点が1枚目にも2枚目にも写っている)ありますから次の2式が成立します。
ここからs、s'を消去してまとめれば次が得られます。
これを Cx=v と表しておきましょう。xはvの左から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を求めるだけなので内部パラメータが変わっても問題ないのでした。