def testComputeFundamental(self):
E = sfm.compute_fundamental(self.x2[:, :8], self.x[:, :8])
self.assertEqual(self.expectedE.shape, E.shape)
self.assertTrue(numpy.allclose(self.expectedE, E))
execfile('load_vggdata.py')
import sfm
# 最初の2枚の画像の点のインデクス番号
ndx = (corr[:,0]>=0) & (corr[:,1]>=0)
# 座標値を取得し、どう座標系にする
x1 = points2D[0][:,corr[ndx,0]]
x1 = vstack( (x1,ones(x1.shape[1])) )
x2 = points2D[1][:,corr[ndx,1]]
x2 = vstack( (x2,ones(x2.shape[1])) )
# Fを計算する
F = sfm.compute_fundamental(x1,x2)
# エピ極を計算する
e = sfm.compute_epipole(F)
# 描画する
figure()
imshow(im1)
# 各行について適当に色を付つけて描画する。
for i in range(5):
sfm.plot_epipolar_line(im1,F,x2[:,i],e,False)
axis('off')
figure()
imshow(im2)
corr
# +
# %matplotlib inline
# 最初の２枚の画像の点のインデックス番号
ndx = (corr[:, 0] >= 0) & (corr[:, 1] >= 0)
# 座標値を取得し、同座標にする
x1 = points2D[0][:, corr[ndx, 0]]
x1 = homography.make_homog(x1)
x2 = points2D[1][:, corr[ndx, 1]]
x2 = homography.make_homog(x2)
#Fを計算する。
F = sfm.compute_fundamental(x1, x2)
#エピ極を計算する
e = sfm.compute_epipole(F)
#描写する
# plt.figure()
ip.show_img(im1)
#各行について適当に色を付けて描写する
for i in range(5):
sfm.plot_epipolar_line(im1, F, x2[:, i], epipole=e, show_epipole=False)
plt.axis('off')
#描写する
# plt.figure()
ip.show_img(im2)
def testComputeFundamental(self): E = sfm.compute_fundamental(self.x2[:, :8], self.x[:, :8]) self.assertEqual(self.expectedE.shape, E.shape) self.assertTrue(numpy.allclose(self.expectedE, E))
def draw3d_reconstracted_estP(self):
self.ax_rcn_est.cla()
# カメラ行列（内部・外部）
M1 = self.K.dot(self.P1)
M2 = self.K.dot(self.P2)
# 画像座標 ( sx, sy, s )
x1p = M1.dot(self.Xpt)
x2p = M2.dot(self.Xpt)
# sで正規化 (x, y, 1)
for i in range(3):
x1p[i] /= x1p[2]
x2p[i] /= x2p[2]
# pts1 = []
# pts2 = []
# for i in range(len(x1p[0])):
# pts1.append([x1p[0][i], x1p[1][i]])
# pts2.append([x2p[0][i], x2p[1][i]])
# pts1 = np.int32(pts1)
# pts2 = np.int32(pts2)
# 画像1, 画像2の特徴点を対応付ける行列Fを計算
# F, mask = cv2.findFundamentalMat(pts1, pts2, cv2.FM_RANSAC)
# mask = np.reshape(mask, (1, len(mask)))[0]
# idx_mask = np.arange(len(mask))
# idx_mask = idx_mask[mask == 1]
x1n = np.dot(inv(self.K), x1p)
x2n = np.dot(inv(self.K), x2p)
# RANSACでEを推定
# Method_EstE = "RAN"
Method_EstE = ""
E = ""
inliers = ""
if Method_EstE is "RAN":
model = sfm.RansacModel()
E, inliers = sfm.F_from_ransac(x1n, x2n, model)
else:
E = sfm.compute_fundamental(x1n, x2n)
inliers = [True for i in range(len(x1n[0, :]))]
print("E: ", E)
# # カメラ行列を計算する（P2は4つの解のリスト）
P1 = array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0]])
P2, S, R1, R2, U, V = sfm.compute_P_from_essential_mod(E)
print("S: ", S)
print("S fro: ", np.linalg.norm(S, "fro"))
print("R1: ", R1)
print("R2: ", R2)
print("R1 deg: ", calcRmatDeg(R1))
print("R2 deg: ", calcRmatDeg(R2))
P2_R = self.P2[:, 0:3]
print("P2_R: ", P2_R)
# print("U: ", U)
# print("V: ", V)
# print("S*R1: ", S.dot(R1))
# print("S*R2: ", S.dot(R2))
# 2つのカメラの前に点のある解を選ぶ
ind = 0
maxres = 0
for i in range(4):
# triangulate inliers and compute depth for each camera
# インライアを三角測量し各カメラからの奥行きを計算する
X = sfm.triangulate(x1n[:, inliers], x2n[:, inliers], P1, P2[i])
d1 = np.dot(P1, X)[2]
d2 = np.dot(P2[i], X)[2]
if sum(d1 > 0) + sum(d2 > 0) > maxres:
maxres = sum(d1 > 0) + sum(d2 > 0)
ind = i
infront = (d1 > 0) & (d2 > 0)
#全表示用
M1 = self.K.dot(P1)
M2_est = self.K.dot(P2[ind])
X = sfm.triangulate(x1p, x2p, M1, M2_est)
# print("P2 est:", P2[ind])
# print("P2: ", self.P2)
# for i in range(4):
# print("P2 est[ ", i, " ]: ", P2[i])
#
# M2 = self.K.dot(self.P2)
# print("M2: ", M2)
self.ax_rcn_est.set_xlabel("x_wld_rcn")
self.ax_rcn_est.set_ylabel("y_wld_rcn")
self.ax_rcn_est.set_zlabel("z_wld_rcn")
# self.ax_rcn_est.set_xlim([-5, 5])
# self.ax_rcn_est.set_ylim([-5, 5])
# self.ax_rcn_est.set_zlim([-5, 5])
self.ax_rcn_est.set_title("obj wld rcn est")
for l in self.ll:
x = np.array([X[0][l[0]], X[0][l[1]]])
y = np.array([X[1][l[0]], X[1][l[1]]])
z = np.array([X[2][l[0]], X[2][l[1]]])
self.ax_rcn_est.plot(x,
y,
z,
marker='.',
markersize=5,
color='red')
# # 対応が見つからなかった点
# x = X[0,[ not val for val in mask]]
# y = X[1,[ not val for val in mask]]
# z = X[2,[ not val for val in mask]]
# self.ax_rcn_est.plot(x, y, z, marker='.', markersize=5, color='black', linestyle = "")
# カメラより奥にあると推定された点
# X = sfm.triangulate(x1[:, idx_mask], x2[:, idx_mask], P1, P2[i])
x = X[0, [not val for val in infront]]
y = X[1, [not val for val in infront]]
z = X[2, [not val for val in infront]]
self.ax_rcn_est.plot(x,
y,
z,
marker='.',
markersize=5,
color='blue',
linestyle="")
def draw2d(self):
self.ax1.cla()
self.ax2.cla()
# カメラ行列
A1 = self.K.dot(self.P1)
A2 = self.K.dot(self.P2)
# 画像座標 ( sx, sy, s )
x1p = A1.dot(self.Xpt)
x2p = A2.dot(self.Xpt)
# sで正規化 (x, y, 1)
for i in range(3):
x1p[i] /= x1p[2]
x2p[i] /= x2p[2]
self.ax1.set_xlim([0, self.Imgwidth])
self.ax1.set_ylim([0, self.Imgheight])
self.ax1.set_xlabel("x_img")
self.ax1.set_ylabel("y_img")
self.ax1.set_title("obj cam1 img")
self.ax1.grid(which="major", axis="x")
self.ax1.grid(which="major", axis="y")
self.ax2.set_xlim([0, self.Imgwidth])
self.ax2.set_ylim([0, self.Imgheight])
self.ax2.set_xlabel("x_img")
self.ax2.set_ylabel("y_img")
self.ax2.set_title("obj cam2 img")
self.ax2.grid(which="major", axis="x")
self.ax2.grid(which="major", axis="y")
#self.ax1.axis('scaled')
for i, l in enumerate(self.ll):
x = np.array([x1p[0][l[0]], x1p[0][l[1]]])
y = np.array([x1p[1][l[0]], x1p[1][l[1]]])
# self.ax1.plot(x, y, marker='.', markersize=5, color='red')
self.ax1.plot(x,
y,
marker='.',
markersize=5,
color=self.llcolor[i])
x = np.array([x2p[0][l[0]], x2p[0][l[1]]])
y = np.array([x2p[1][l[0]], x2p[1][l[1]]])
#self.ax2.plot(x, y, marker='.', markersize=5, color='blue')
self.ax2.plot(x,
y,
marker='.',
markersize=5,
color=self.llcolor[i])
# draw epipolar line
## calcurate F: x1 * F * x2
F = sfm.compute_fundamental(x1p, x2p)
## calc epi poler
e1 = sfm.compute_epipole(F)
e2 = sfm.compute_epipole(F.T)
print("F:", F)
numLine = len(x1p[0, :])
for i in range(numLine):
# epiline on img1
line = dot(F, x2p[:, i])
t = linspace(0, self.Imgwidth, 100)
lt = array([(line[2] + line[0] * tt) / (-line[1]) for tt in t])
# ndx = (lt >= 0) & (lt < self.Imgheight)
# self.ax1.plot(t[ndx], lt[ndx], linewidth=1)
self.ax1.plot(t, lt, linewidth=1)
self.ax1.plot(e1[0] / e1[2], e1[1] / e1[2], 'r*')
# epiline on img1
line = dot(F.T, x1p[:, i])
t = linspace(0, self.Imgwidth, 100)
lt = array([(line[2] + line[0] * tt) / (-line[1]) for tt in t])
self.ax2.plot(t, lt, linewidth=1)
self.ax2.plot(e2[0] / e2[2], e2[1] / e2[2], 'r*')
