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 # 最初の2枚の画像の点のインデックス番号 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 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*')