def main(): img_path = '../Data/stereo/centre/' imgs = sorted(os.listdir(img_path)) fx, fy, cx, cy, G_camera_image, LUT = ReadCameraModel('../Data/model') i = 100 img1 = cv2.imread(img_path + imgs[i], -1) img1 = cv2.cvtColor(img1, cv2.COLOR_BAYER_GR2BGR) img1 = UndistortImage(img1, LUT) img2 = cv2.imread(img_path + imgs[i + 1], -1) img2 = cv2.cvtColor(img2, cv2.COLOR_BAYER_GR2BGR) img2 = UndistortImage(img2, LUT) x1, x2 = utils.getMatchingFeaturePoints(img1, img2) x1 = np.hstack([x1, np.ones((x1.shape[0], 1))]) x2 = np.hstack([x2, np.ones((x2.shape[0], 1))]) print(x1.shape) print(x2.shape) F = EstimateFundamentalMatrix(x1, x2) print("Calculated") print(F) # print(F_n) print("CV2") F, _ = cv2.findFundamentalMat(x1, x2) print(F)
def main(): img_path = '../Data/stereo/centre/' imgs = sorted(os.listdir(img_path)) fx, fy, cx, cy, G_camera_image, LUT = ReadCameraModel('../Data/model') i = 100 img1 = cv2.imread(img_path + imgs[i], -1) img1 = cv2.cvtColor(img1, cv2.COLOR_BAYER_GR2BGR) img1 = UndistortImage(img1, LUT) img2 = cv2.imread(img_path + imgs[i + 1], -1) img2 = cv2.cvtColor(img2, cv2.COLOR_BAYER_GR2BGR) img2 = UndistortImage(img2, LUT) x1, x2 = utils.getMatchingFeaturePoints(img1, img2) x1 = np.hstack([x1, np.ones((x1.shape[0], 1))]) x2 = np.hstack([x2, np.ones((x2.shape[0], 1))]) # print(x1.shape) # print(x2.shape) features = np.hstack([x1, x2]) # getInliersRANSAC(features,threshold=(0.07),size=8,num_inliers=0.6*features.shape[0],num_iters=1000) x1_in, x2_in = RANSAC.getInliersRANSAC(features, threshold=(0.005), size=8, num_inliers=0.6 * features.shape[0], num_iters=200) fund_mtx = fundamental.EstimateFundamentalMatrix(x1_in, x2_in) K = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]]) ess_mtx = essential.EssentialMatrixFromFundamentalMatrix(fund_mtx, K) print("Essential Matrix") print(ess_mtx) C, R = ExtractCameraPose(ess_mtx) print("Pose Orientation :") print(R) E_act = cv2.findEssentialMat(x1_in[:, :2], x2_in[:, :2], K) # _,R,T,_ = cv2.recoverPose(E_act[0],x1_in[:,:2],x2_in[:,:2]) # print("Pose Position :") # print(T.T) # print("Pose Orientation :") # print(R) R1, R2, T = cv2.decomposeEssentialMat(E_act[0]) print("OpenCV R1") print(R1) print("OpenCV R2") print(R2) print("Calculated Pose Position :") print(C) print("Opencv T") print(T.T)
def main(): img_path = '../Data/stereo/centre/' imgs = sorted(os.listdir(img_path)) fx, fy, cx, cy, G_camera_image, LUT = ReadCameraModel('../Data/model') i = 100 img1 = cv2.imread(img_path + imgs[i], -1) img1 = cv2.cvtColor(img1, cv2.COLOR_BAYER_GR2BGR) img1 = UndistortImage(img1, LUT) img2 = cv2.imread(img_path + imgs[i + 1], -1) img2 = cv2.cvtColor(img2, cv2.COLOR_BAYER_GR2BGR) img2 = UndistortImage(img2, LUT) x1, x2 = utils.getMatchingFeaturePoints(img1, img2) x1 = np.hstack([x1, np.ones((x1.shape[0], 1))]) x2 = np.hstack([x2, np.ones((x2.shape[0], 1))]) features = np.hstack([x1, x2]) x1_in, x2_in = RANSAC.getInliersRANSAC(features, threshold=(0.005), size=8, num_inliers=0.6 * features.shape[0], num_iters=200) fund_mtx = fundamental.EstimateFundamentalMatrix(x1_in, x2_in) K = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]]) ess_mtx = essential.EssentialMatrixFromFundamentalMatrix(fund_mtx, K) C, R = cameraPose.ExtractCameraPose(ess_mtx) X_tri = [] for j in range(4): X_tri.append( linearTriangulation(K, np.zeros(3), C[j], np.eye(3), R[j], x1_in, x2_in)) X_tri = np.array(X_tri) print(X_tri.shape) sys.exit(0)
def main(): img_path = '../Data/stereo/centre/' imgs = sorted(os.listdir(img_path)) fx, fy, cx, cy, G_camera_image, LUT = ReadCameraModel('../Data/model') i = 100 img1 = cv2.imread(img_path + imgs[i], -1) img1 = cv2.cvtColor(img1, cv2.COLOR_BAYER_GR2BGR) img1 = UndistortImage(img1, LUT) img2 = cv2.imread(img_path + imgs[i + 1], -1) img2 = cv2.cvtColor(img2, cv2.COLOR_BAYER_GR2BGR) img2 = UndistortImage(img2, LUT) x1, x2 = utils.getMatchingFeaturePoints(img1, img2) x1 = np.hstack([x1, np.ones((x1.shape[0], 1))]) x2 = np.hstack([x2, np.ones((x2.shape[0], 1))]) # print(x1.shape) # print(x2.shape) features = np.hstack([x1, x2]) # getInliersRANSAC(features,threshold=(0.07),size=8,num_inliers=0.6*features.shape[0],num_iters=1000) x1_in, x2_in = RANSAC.getInliersRANSAC(features, threshold=(0.005), size=8, num_inliers=0.6 * features.shape[0], num_iters=1000) fund_mtx = fundamental.EstimateFundamentalMatrix(x1_in, x2_in) K = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]]) E = EssentialMatrixFromFundamentalMatrix(fund_mtx, K) print("Essential Matrix") print(E) E_act = cv2.findEssentialMat(x1_in[:, :2], x2_in[:, :2], K) print("E actual") print(E_act[0])
def main(): img_path = '../Data/stereo/centre/' imgs = sorted(os.listdir(img_path)) fx, fy, cx, cy, G_camera_image, LUT = ReadCameraModel('../Data/model') i = 150 img1 = cv2.imread(img_path + imgs[i], -1) img1 = cv2.cvtColor(img1, cv2.COLOR_BAYER_GR2BGR) img1 = UndistortImage(img1, LUT) img2 = cv2.imread(img_path + imgs[i + 1], -1) img2 = cv2.cvtColor(img2, cv2.COLOR_BAYER_GR2BGR) img2 = UndistortImage(img2, LUT) print(img1.shape) x1, x2 = utils.getMatchingFeaturePoints(img1, img2) x1 = np.hstack([x1, np.ones((x1.shape[0], 1))]) x2 = np.hstack([x2, np.ones((x2.shape[0], 1))]) # print(x1.shape) # print(x2.shape) features = np.hstack([x1, x2]) # getInliersRANSAC(features,threshold=(0.07),size=8,num_inliers=0.6*features.shape[0],num_iters=1000) x1_in, x2_in = getInliersRANSAC(features, threshold=(0.005), size=8, num_inliers=0.6 * features.shape[0], num_iters=500) print(x1_in.shape) # drawFeatures(img1,img2,x1,x2,'r',0.3) # drawFeatures(img1,img2,x1_in,x2_in,'g',1) # plt.show() # sys.exit(0) fund_mtx = fundamental.EstimateFundamentalMatrix(x1_in, x2_in) # x1 = x1_in # x2 = x2_in # print("CV2 Fundamental Matrix :",correct_fund_mtx[0]) print("Fundamental Matrix RANSAC:") print(fund_mtx) # img1 =cv2.imread('../Data/1.jpg') # img2 =cv2.imread('../Data/2.jpg') pts1 = np.int32(x1[:, :2]) pts2 = np.int32(x2[:, :2]) F, _ = cv2.findFundamentalMat(x1, x2, cv2.RANSAC) drawFeatures(img1, img2, x1, x2, 'r', 0.1) drawFeatures(img1, img2, x1_in, x2_in, 'g', 0.5) plt.show() print("F") print(F) # Find epilines corresponding to points in right image (second image) and # drawing its lines on left image lines1 = cv2.computeCorrespondEpilines(pts2.reshape(-1, 1, 2), 2, F) lines1 = lines1.reshape(-1, 3) # lines1 = [lines1[15]] # img5,img6 = drawlines(img1,img2,lines1,pts1,pts2) img5, img6 = drawlines(img1, img2, lines1[25:30], pts1[25:30], pts2[25:30]) # Find epilines corresponding to points in left image (first image) and # drawing its lines on right image lines2 = cv2.computeCorrespondEpilines(pts1.reshape(-1, 1, 2), 1, F) lines2 = lines2.reshape(-1, 3) # lines2 = [lines2[25]] # img3,img4 = drawlines(img2,img1,lines2,pts2,pts1) img3, img4 = drawlines(img2, img1, lines2[25:30], pts2[25:30], pts1[25:30]) plt.subplot(221), plt.imshow(img5) plt.subplot(222), plt.imshow(img3) # plt.show() # Find epilines corresponding to points in right image (second image) and # drawing its lines on left image lines1 = cv2.computeCorrespondEpilines(pts2.reshape(-1, 1, 2), 2, fund_mtx) lines1 = lines1.reshape(-1, 3) # lines1 = [lines1[25]] # img5,img6 = drawlines(img1,img2,lines1,pts1,pts2) img5, img6 = drawlines(img1, img2, lines1[25:30], pts1[25:30], pts2[25:30]) # Find epilines corresponding to points in left image (first image) and # drawing its lines on right image lines2 = cv2.computeCorrespondEpilines(pts1.reshape(-1, 1, 2), 1, fund_mtx) lines2 = lines2.reshape(-1, 3) # lines2 = [lines2[25]] # img3,img4 = drawlines(img2,img1,lines2,pts2,pts1) img3, img4 = drawlines(img2, img1, lines2[25:30], pts2[25:30], pts1[25:30]) plt.subplot(223), plt.imshow(img5) plt.subplot(224), plt.imshow(img3) plt.show()
def main(): img_path = '../Data/stereo/centre/' imgs = sorted(os.listdir(img_path)) fx, fy, cx, cy, G_camera_image, LUT = ReadCameraModel('../Data/model') i = 100 pt_old = np.zeros((3, 1)) pt_old_cv = np.zeros((3, 1)) i = 15 while i < len(imgs): # for i in range(15,len(imgs)): # for i in range(500,600): i = i + 3 print(i) img1 = cv2.imread(img_path + imgs[i]) img1 = cv2.cvtColor(img1, cv2.COLOR_BAYER_GR2BGR) img1 = UndistortImage(img1, LUT) img2 = cv2.imread(img_path + imgs[i + 1]) img2 = cv2.cvtColor(img2, cv2.COLOR_BAYER_GR2BGR) img2 = UndistortImage(img2, LUT) x1, x2 = utils.getMatchingFeaturePoints(img1, img2) x1 = np.hstack([x1, np.ones((x1.shape[0], 1))]) x2 = np.hstack([x2, np.ones((x2.shape[0], 1))]) features = np.hstack([x1, x2]) x1_in, x2_in = RANSAC.getInliersRANSAC(features, threshold=(0.002), size=8, num_inliers=0.6 * features.shape[0], num_iters=200) # print("Inliers :",x1_in.shape[0]) fund_mtx = fundamental.EstimateFundamentalMatrix(x1_in, x2_in) K = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]]) ess_mtx = essential.EssentialMatrixFromFundamentalMatrix(fund_mtx, K) C, R = cameraPose.ExtractCameraPose(ess_mtx) X_tri = [] for j in range(4): X_tri.append( triangulation.linearTriangulation(K, np.zeros(3), C[j], np.eye(3), R[j], x1_in, x2_in)) X_tri = np.array(X_tri) # print(X_tri.shape) C_c, R_c = disambiguateCameraPose.disambiguateCameraPose(X_tri, C, R) C_c = np.reshape(C_c, (3, 1)) if (C_c[2] < 0): C_c *= -1 # print("Pose Position :") # print(C_c.T) # print("Pose Orientation :") # print(R_c) pt_new = np.matmul(R_c, pt_old) pt_new += C_c if abs(pt_new[0] - pt_old[0]) > 2: pt_new[0] = copy.copy(pt_old[0]) if abs(pt_new[1] - pt_old[1]) > 2: pt_new[1] = copy.copy(pt_old[1]) if abs(pt_new[2] - pt_old[2]) > 2: pt_new[2] = copy.copy(pt_old[2]) print("Old point :", pt_old.T) print("New point :", pt_new.T) plt.figure("Calculated") plt.plot([pt_old[0], pt_new[0]], [pt_old[2], pt_new[2]]) pt_old = copy.copy(pt_new) # E_act = cv2.findEssentialMat(x1[:,:2],x2[:,:2],K) # _,R,T,_ = cv2.recoverPose(E_act[0],x1[:,:2],x2[:,:2]) # pt_new_cv = np.matmul(R,pt_old_cv) # pt_new_cv += T # if abs(pt_new_cv[0]-pt_old_cv[0]) >0.5: # pt_new_cv[0] = copy.copy(pt_old_cv[0]) # if abs(pt_new_cv[1]-pt_old_cv[1]) >0.5: # pt_new_cv[1] = copy.copy(pt_old_cv[1]) # print("Act :",plot_act) # print("Calc :",plot_calc) # print("---") # print("CV2 Pose Position :") # print(T.T) # print("CV2 Pose Orientation :") # print(R) # # print("Pose Position cv2:") # print(T.T) # print("Pose Orientation cv2 :") # print(R) # print("Old point :",pt_old_cv.T) # print("New point :",pt_new_cv.T) # plt.figure("OpenCV") # plt.plot([pt_old_cv[0],pt_new_cv[0]],[pt_old_cv[2],pt_new_cv[2]]) # pt_old_cv = copy.copy(pt_new_cv) plt.show() plt.pause(0.00001) print("!-----------------!")
def main(): img_path = '../Data/stereo/centre/' imgs = sorted(os.listdir(img_path)) fx,fy,cx,cy,G_camera_image,LUT = ReadCameraModel('../Data/model') i = 100 img1 = cv2.imread(img_path+imgs[i],-1) img1 = cv2.cvtColor(img1,cv2.COLOR_BAYER_GR2BGR) img1 = UndistortImage(img1,LUT) img2 = cv2.imread(img_path+imgs[i+1],-1) img2 = cv2.cvtColor(img2,cv2.COLOR_BAYER_GR2BGR) img2 = UndistortImage(img2,LUT) x1,x2 = utils.getMatchingFeaturePoints(img1,img2) x1 = np.hstack([x1,np.ones((x1.shape[0],1))]) x2 = np.hstack([x2,np.ones((x2.shape[0],1))]) features = np.hstack([x1,x2]) x1_in,x2_in = RANSAC.getInliersRANSAC(features,threshold=(0.005),size=8,num_inliers=0.6*features.shape[0],num_iters=1000) print(x1_in.shape) fund_mtx = fundamental.EstimateFundamentalMatrix(x1_in,x2_in) K = np.array([[fx,0,cx],[0,fy,cy],[0,0,1]]) ess_mtx = essential.EssentialMatrixFromFundamentalMatrix(fund_mtx,K) C,R = cameraPose.ExtractCameraPose(ess_mtx) # print("Available C :") # print(C) print("Available R") print(R) X_tri = [] for j in range(4): X_tri.append(triangulation.linearTriangulation(K,np.zeros(3),C[j],np.eye(3),R[j],x1_in,x2_in)) X_tri = np.array(X_tri) # print(X_tri.shape) C_c,R_c = disambiguateCameraPose(X_tri,C,R) print("Pose Position :") print(C_c) print("Pose Orientation :") print(R_c) # angle = utils.rotationMatrixToEulerAngles(R_c) # print("Angle :") # print(angle) E_act = cv2.findEssentialMat(x1_in[:,:2],x2_in[:,:2],K) _,R,T,_ = cv2.recoverPose(E_act[0],x1_in[:,:2],x2_in[:,:2]) print("---") print("CV2 Pose Position :") print(T.T) print("CV2 Pose Orientation :") print(R) # angle = utils.rotationMatrixToEulerAngles(R) # print("CV2 Ang;e:") # print(angle) print("<----------------->")