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 image_process(H_init, p_0, all_images): data_points = [] for index in range(20, len(all_images) - 1): print("image_number = ", index) img1 = cv2.imread("stereo/centre/" + str(all_images[index]), 0) #loading first image in greyscale img2 = cv2.imread("stereo/centre/" + str(all_images[index + 1]), 0) #loading second image in greyscale colorimage1 = cv2.cvtColor( img1, cv2.COLOR_BayerGR2BGR) #converting first image from BayerGR to BGR colorimage2 = cv2.cvtColor( img2, cv2.COLOR_BayerGR2BGR ) #converting second image from BayerGR to BGR undistortedimage1 = UndistortImage( colorimage1, LUT) #undistorting first image using LUT undistortedimage2 = UndistortImage( colorimage2, LUT) #undistorting second image using LUT gray1 = cv2.cvtColor(undistortedimage1, cv2.COLOR_BGR2GRAY) #converting back to grayscale gray2 = cv2.cvtColor(undistortedimage2, cv2.COLOR_BGR2GRAY) #converting back to grayscale # Finding proper matches between two frames features1, features2 = SIFT_matches(gray1, gray2) #using inbuilt function to directly compute F_matrix after RANSAC FinalFundamentalMatrix, m = cv2.findFundamentalMat( features1, features2, cv2.FM_RANSAC) features1 = features1[m.ravel() == 1] features2 = features2[m.ravel() == 1] # Calculating the Essential matrix E_matrix = K.T @ FinalFundamentalMatrix @ K # Obtaining final Rotational and Translation pose for cameras retval, rot_mat, T, mask = cv2.recoverPose(E_matrix, features1, features2, K) H_init = H_init @ H_matrix( rot_mat, T ) #matmul between H_init(Ident 4x4) and H_matrix that was calculated using new Rot and Trans matrix p_projection = H_init @ p_0 #new projection point calculated data_points.append([p_projection[0][0], -p_projection[2][0]]) plt.scatter(p_projection[0][0], -p_projection[2][0], color='g') return data_points
def data_prep(data_img): img_1 = cv2.cvtColor(data_img[x], cv2.COLOR_BAYER_GR2BGR) img_2 = cv2.cvtColor(data_img[x + 1], cv2.COLOR_BAYER_GR2BGR) fx, fy, cx, cy, G_C_1, LUT1 = ReadCameraModel('Oxford_dataset/model') undimg_1 = UndistortImage(img_1, LUT1) undimg_1 = cv2.cvtColor(undimg_1, cv2.COLOR_BGR2GRAY) imgeqi_1 = cv2.equalizeHist(undimg_1) fx, fy, cx, cy, G_C_2, LUT2 = ReadCameraModel('Oxford_dataset/model') undimg_2 = UndistortImage(img_2, LUT2) undimg_2 = cv2.cvtColor(undimg_2, cv2.COLOR_BGR2GRAY) imgeqi_2 = cv2.equalizeHist(undimg_2) return fx, fy, cx, cy, imgeqi_1, imgeqi_2
def preprocess(self, image, nextimage): rgb1 = cv2.cvtColor(image, cv2.COLOR_BAYER_GR2BGR) rgb2 = cv2.cvtColor(nextimage, cv2.COLOR_BAYER_GR2BGR) undimg_1 = UndistortImage(rgb1, self.LUT1) undimg_1 = cv2.cvtColor(undimg_1, cv2.COLOR_BGR2GRAY) undimg_2 = UndistortImage(rgb2, self.LUT2) undimg_2 = cv2.cvtColor(undimg_2, cv2.COLOR_BGR2GRAY) imgeqi_1 = cv2.equalizeHist(undimg_1) # imgeqi_1 = (imgeqi_1[0:800, 0:1280].copy()) imgeqi_2 = cv2.equalizeHist(undimg_2) # imgeqi_2 = (imgeqi_2[0:800, 0:1280].copy()) return imgeqi_1, imgeqi_2
def preprocess_data(img_path, name, LUT): im = cv2.imread(os.path.join(img_path, name), 0) BGR_im = cv2.cvtColor(im, cv2.COLOR_BayerGR2BGR) un_im = UndistortImage(BGR_im, LUT) un_im = cv2.cvtColor(un_im, cv2.COLOR_BGR2GRAY) un_im = un_im[200:650, :] return un_im
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 preprocess_data(img_path, name, LUT): im = cv2.imread(os.path.join(img_path, name), 0) BGR_im = cv2.cvtColor(im, cv2.COLOR_BayerGR2BGR) un_im = UndistortImage(BGR_im, LUT) # un_im = un_im[200:650, :] # cv2.imshow("image", un_im) # if cv2.waitKey(0) & 0xff == 27: # cv2.destroyAllWindows() # h, w = im.shape # dim = (int(0.6*w), int(0.6*h)) # un_im = cv2.resize(un_im, dim) return un_im
def undistortImage(path_to_model, path_to_images): ''' Here, we undistort the dataset images :param path_to_model: :param path_to_images: :return: K - calibration matrix ''' # Read camera parameters fx, fy, cx, cy, G_camera_image, LUT = ReadCameraModel(path_to_model) # iterate through each image, convert to BGR, undistort(function takes all channels in input) images = glob.glob(path_to_images + "/*.png") images.sort() for cnt, image in enumerate(images): frame = cv2.imread(image, -1) frame_RGB = cv2.cvtColor(frame, cv2.COLOR_BayerGR2BGR) undistorted_image = UndistortImage(frame_RGB, LUT) cv2.imwrite("./undistort/frame" + str(cnt) + ".png", undistorted_image)
def raw2Undistorted(img_path, LUT): img = cv2.imread(img_path, cv2.IMREAD_UNCHANGED) img = cv2.cvtColor(img, cv2.COLOR_BayerGR2BGR) img = UndistortImage(img, LUT) return img
print(t_f[1,0]) fig = plt.figure() ax = fig.add_subplot(111, projection='3d') for i in range(len(filenames) - 1): if i<=40: continue f = filenames[i] f1 = filenames[i + 1] img1 = cv2.imread(f, 0) img2 = cv2.imread(f1, 0) img1 = cv2.cvtColor(img1, cv2.COLOR_BAYER_GR2RGB) img1 = UndistortImage(img1, LUT) img2 = cv2.cvtColor(img2, cv2.COLOR_BAYER_GR2RGB) img2 = UndistortImage(img2, LUT) img1GrayScale = cv2.cvtColor(img1, cv2.COLOR_RGB2GRAY) img2GrayScale = cv2.cvtColor(img2, cv2.COLOR_RGB2GRAY) pts1, pts2 = FindCorrespondence(img1GrayScale, img2GrayScale) corr_list = np.concatenate((pts1, pts2), 1) F, mI = RANSACforF(pts1, pts2, corr_list) E = EstimateEssentialMatrix(cameraMatrix, F) P1, P2, P3, P4, (C1, R1), (C2, R2), (C3, R3), (C4, R4) = EstimateCameraPose(E, cameraMatrix)
h = np.eye(4) current_pos = np.zeros((3, 1)) current_rot = np.eye(3) while a < (len(car_images) - 1): # for i in range(len(car_images)): #imageRaw1 = cv2.imread(images[i], cv2.IMREAD_GRAYSCALE | cv2.IMREAD_ANYDEPTH) #rgb1 = cv2.cvtColor(imageRaw1, cv2.COLOR_BAYER_GR2BGR) rgb1 = cv2.cvtColor(car_images[a], cv2.COLOR_BAYER_GR2BGR) #imageRaw2 = cv2.imread(frame2, cv2.IMREAD_GRAYSCALE | cv2.IMREAD_ANYDEPTH) #rgb2 = cv2.cvtColor(imageRaw2, cv2.COLOR_BAYER_GR2BGR) rgb2 = cv2.cvtColor(car_images[a + 1], cv2.COLOR_BAYER_GR2BGR) fx, fy, cx, cy, G_camera_image1, LUT1 = ReadCameraModel( 'Oxford_dataset/model') undistorted_image1 = UndistortImage(rgb1, LUT1) undistorted_image1 = cv2.cvtColor(undistorted_image1, cv2.COLOR_BGR2GRAY) eqimage1 = cv2.equalizeHist(undistorted_image1) fx, fy, cx, cy, G_camera_image2, LUT2 = ReadCameraModel( 'Oxford_dataset/model') undistorted_image2 = UndistortImage(rgb2, LUT2) undistorted_image2 = cv2.cvtColor(undistorted_image2, cv2.COLOR_BGR2GRAY) eqimage2 = cv2.equalizeHist(undistorted_image2) K = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]]) # Initiate STAR detector orb = cv2.ORB_create()
def image_process(img): color_image = cv2.cvtColor(img, cv2.COLOR_BayerGR2BGR) color_image_undistorted = UndistortImage(color_image, LUT) gray_image = cv2.cvtColor(color_image_undistorted, cv2.COLOR_BGR2GRAY) return gray_image
for i in range(0, N): img_current_frame = cv2.imread(centre_path[i], 0) img_next_frame = cv2.imread(centre_path[i + 1], 0) img_current_frame = cv2.rectangle( img_current_frame, (np.float32(50), np.float32(np.shape(img_current_frame)[0])), (np.float32(1250), np.float32(800)), (0, 0, 0), -1) img_next_frame = cv2.rectangle( img_next_frame, (np.float32(50), np.float32(np.shape(img_next_frame)[0])), (np.float32(1250), np.float32(800)), (0, 0, 0), -1) color_img = cv2.cvtColor(img_current_frame, cv2.COLOR_BayerGR2RGB) color_img_next = cv2.cvtColor(img_next_frame, cv2.COLOR_BayerGR2RGB) undist_img = UndistortImage(color_img, LUT) undist_img_next = UndistortImage(color_img_next, LUT) MIN_MATCH_COUNT = 10 # Initiate SIFT detector sift = cv2.xfeatures2d.SIFT_create() # find the keypoints and descriptors with SIFT kp1, des1 = sift.detectAndCompute(undist_img, None) kp2, des2 = sift.detectAndCompute(undist_img_next, None) FLANN_INDEX_KDTREE = 0 index_params = dict(algorithm=FLANN_INDEX_KDTREE, trees=5) search_params = dict(checks=50) flann = cv2.FlannBasedMatcher(index_params, search_params)
images = [] # Read and save camera intrinsic parameters fx, fy, cx, cy, G_camera_image, LUT = ReadCameraModel(modelDirectory) ''' K = np.array([[fx , 0 , cx],[0 , fy , cy],[0 , 0 , 1]]) f = open('Camera Intrinsics.txt','a') f.write('K: ' + '\n' + str(K) + '\n\n') f.write('G_camera_image: ' + '\n' + str(G_camera_image)) f.close() ''' # Process all images in the image directory for imageName in os.listdir(imageDirectory): rawImage = cv2.imread(imageDirectory + imageName) # Convert raw image to grayscale and demosaic into BGR image gray = cv2.cvtColor(rawImage, cv2.COLOR_BGR2GRAY) imageBGR = cv2.cvtColor(gray, cv2.COLOR_BayerGR2BGR) # Undistort BGR image undistoredImage = UndistortImage(imageBGR, LUT) # Save to dataset folder cv2.imwrite('dataset/' + imageName, undistoredImage)
import os from UndistortImage import UndistortImage from ReadCameraModel import ReadCameraModel img = glob.glob('./stereo/centre/*.png') img.sort() print(img) data = [] final = [] fx, fy, cx, cy, G_camera_image, LUT = ReadCameraModel("./model") for i in range(len(img)): temp = cv2.imread(img[i], 0) data.append(temp) count = 0 EXTN = ".png" for j in range(len(data)): count = count + 1 # if(count>50): # break pic = data[j] print(count) tempo = cv2.cvtColor(pic, cv2.COLOR_BayerGR2BGR) temp_img = UndistortImage(tempo, LUT) filename = str(count).zfill(4) + EXTN cv2.imwrite("./undistorted_input_images/frame%s" % filename, temp_img) # print(temp_img) final.append(temp_img) cv2.imshow("image", final[0]) cv2.waitKey(0) cv2.destroyAllWindows()
def undistortImageToGray(img, LUT): colorimage = cv2.cvtColor(img, cv2.COLOR_BayerGR2BGR) undistortedimage = UndistortImage(colorimage, LUT) gray = cv2.cvtColor(undistortedimage, cv2.COLOR_BGR2GRAY) # equ = cv2.equalizeHist(gray1) return gray
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("!-----------------!")
if Translation_final[2] > 0: Translation_final = -Translation_final return Rotation_final, Translation_final H_Start = np.identity(4) p_0 = np.array([[0, 0, 0, 1]]).T flag = 0 data_points = [] for index in range(19, len(frames) - 1): print(frames[index], index) img1 = cv2.imread("stereo/centre/" + str(frames[index]), 0) colorimage1 = cv2.cvtColor(img1, cv2.COLOR_BayerGR2BGR) undistortedimage1 = UndistortImage(colorimage1, LUT) gray1 = cv2.cvtColor(undistortedimage1, cv2.COLOR_BGR2GRAY) img2 = cv2.imread("stereo/centre/" + str(frames[index + 1]), 0) colorimage2 = cv2.cvtColor(img2, cv2.COLOR_BayerGR2BGR) undistortedimage2 = UndistortImage(colorimage2, LUT) gray2 = cv2.cvtColor(undistortedimage2, cv2.COLOR_BGR2GRAY) grayImage1 = gray1[200:650, 0:1280] grayImage2 = gray2[200:650, 0:1280] sift = cv2.xfeatures2d.SIFT_create() kp1, des1 = sift.detectAndCompute(grayImage1, None) kp2, des2 = sift.detectAndCompute(grayImage2, None)
def main(): # Preprocess Images start_t = time.time() H_init = np.identity(4) p0 = np.array([[0, 0, 0, 1]]).T data = [] for i in range(18, len(f) - 1): print(i) img1 = cv2.imread(path + str(f[i]), 0) # cv2.imshow("",img1) # cv2.waitKey(0) # Frame 1 bgr_img1 = cv2.cvtColor(img1, cv2.COLOR_BayerGR2BGR) undist_img1 = UndistortImage(bgr_img1, LUT) gray_img1 = cv2.cvtColor(undist_img1, cv2.COLOR_BGR2GRAY) # Frame 2 img2 = cv2.imread(path + str(f[i + 1]), 0) bgr_img2 = cv2.cvtColor(img2, cv2.COLOR_BayerGR2BGR) undist_img2 = UndistortImage(bgr_img2, LUT) gray_img2 = cv2.cvtColor(undist_img2, cv2.COLOR_BGR2GRAY) gray_img1 = gray_img1[250:600, 0:1280] gray_img2 = gray_img2[250:600, 0:1280] # Find Points of Correspondence f1, f2 = findpts(gray_img1, gray_img2) for l in range(0, len(f1)): a = (int(f1[l][0]), int(f1[l][1])) sift_img = cv2.circle(gray_img2, a, 1, (255, 0, 0), 3) cv2.imwrite(os.path.join(wrframe, str(i) + '.jpg'), bgr_img1) #sift_img = plt.imread(sift_img) # plt.imshow("",sift_img) # cv2.waitKey(0) # #print(f2) # Determine F F, i1, i2 = findF(f1, f2) # Determine E E = findE(K, F) #print(E) # Find T and R R_list, T_list = decomp(E) # Find Best estimates for R and T R, T = cheirality_check(R_list, T_list, i1, i2) # Find Homogeneous Matrix and Camera Centre for Each frame H = get_homogeneous(R, T) H_init = H_init @ H p = H_init @ p0 print('X = ', p[0]) print('Y = ', p[2]) # Write the Co-ordinates in a text file #data.append([p[0][0], -p[2][0]]) file1.write(str(p[0][0]) + ",") file1.write(str(-p[2][0]) + "\n") # Plot the Trajectory # plt.yticks(np.arange(-200,800,100)) # plt.xticks(np.arange(0,1000,100)) plt.scatter(p[0][0], -p[2][0], color='b') plt.savefig(os.path.join(pathplot, str(i) + '.png')) file1.close() end_t = time.time() delta = (end_t - start_t) / 60 print("Time taken: ", delta) plt.show()
images.append(image) images.sort() fx, fy, cx, cy, G_camera_image, LUT = ReadCameraModel('model/') K = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]]) init = np.identity(4) origin = np.array([[0, 0, 0, 1]]).T trajectory_points = [] for ind in range(19, len(images) - 1): print(ind) img1 = cv2.imread(path + str(images[ind]), 0) first_image = cv2.cvtColor(img1, cv2.COLOR_BayerGR2BGR) first_image_undistorted = UndistortImage(first_image, LUT) first_image_gray = cv2.cvtColor(first_image_undistorted, cv2.COLOR_BGR2GRAY) # Reading next image img2 = cv2.imread(path + str(images[ind + 1]), 0) next_image = cv2.cvtColor(img2, cv2.COLOR_BayerGR2BGR) next_image_undistorted = UndistortImage(next_image, LUT) next_image_gray = cv2.cvtColor(next_image_undistorted, cv2.COLOR_BGR2GRAY) # Cropping the area of interest from the current image first_image_final = first_image_gray[200:650, 0:1280] # Cropping the area of interest from the next frame next_image_final = next_image_gray[200:650, 0:1280] sift = cv2.xfeatures2d.SIFT_create()
import cv2 import glob import numpy as np from ReadCameraModel import ReadCameraModel from UndistortImage import UndistortImage model_dir = 'Oxford_dataset/model/' fx, fy, cx, cy, G_camera_image, LUT = ReadCameraModel(model_dir) count = 0 for img in glob.glob('Oxford_dataset/stereo/centre/*.png'): image = cv2.imread(img,cv2.IMREAD_GRAYSCALE | cv2.IMREAD_ANYDEPTH) color_image = cv2.cvtColor(image,cv2.COLOR_BayerGR2BGR) dist = UndistortImage(color_image,LUT) cv2.imwrite('data/'+'%d.png'%count,dist) count = count+1 K = np.matrix([[fx,0,cx],[0,fy,cy],[0,0,1]])
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()
it += 1 continue # elif it > 5 and it % 2 != 0: # it += 1 # continue elif it > 700: it += 1 break print('Iteration: ', it) # load image img = cv2.imread(filepath, 0) img = cv2.cvtColor(img, cv2.COLOR_BayerGR2BGR) img_orig = UndistortImage(img, LUT) img = cv2.cvtColor(img_orig, cv2.COLOR_BGR2GRAY) if (it == 1): img_orig1 = img_orig img1 = img it = 2 orb_kp1, orb_des1 = orb.detectAndCompute(img1, None) continue it += 1 img2 = img img_orig2 = img_orig # continue # find orb features
lastH = np.identity( 4) # Initial camera Pose is considered as an identity matrix origin = np.array([[0, 0, 0, 1]]).T l = [] # Variable for storing all the trajectory points for index in range(19, len(images) - 1): # Looping over all the images img1 = cv2.imread("stereo/centre/" + str(images[index]), 0) # Read current image colorimage1 = cv2.cvtColor( img1, cv2.COLOR_BayerGR2BGR) # Convert image from bayer format to BGR format undistortedimage1 = UndistortImage(colorimage1, LUT) # Undistort the current image gray1 = cv2.cvtColor( undistortedimage1, cv2.COLOR_BGR2GRAY) # Convert the undistorted image in grayscale img2 = cv2.imread("stereo/centre/" + str(images[index + 1]), 0) # Read next image colorimage2 = cv2.cvtColor( img2, cv2.COLOR_BayerGR2BGR) # Convert image from bayer format to BGR format undistortedimage2 = UndistortImage(colorimage2, LUT) # Undistort the next image gray2 = cv2.cvtColor( undistortedimage2, cv2.COLOR_BGR2GRAY) # Convert the undistorted image in grayscale
# Main # Preprocess Images start_t = time.time() H_init = np.identity(4) p0 = np.array([[0, 0, 0, 1]]).T data = [] for i in range(19, 25): print(i) img1 = cv2.imread(path + str(f[i]), 0) # cv2.imshow("",img1) # cv2.waitKey(0) bgr_img1 = cv2.cvtColor(img1, cv2.COLOR_BayerGR2BGR) undist_img1 = UndistortImage(bgr_img1, LUT) gray_img1 = cv2.cvtColor(undist_img1, cv2.COLOR_BGR2GRAY) img2 = cv2.imread(path + str(f[i + 1]), 0) bgr_img2 = cv2.cvtColor(img2, cv2.COLOR_BayerGR2BGR) undist_img2 = UndistortImage(bgr_img2, LUT) gray_img2 = cv2.cvtColor(undist_img2, cv2.COLOR_BGR2GRAY) gray_img1 = gray_img1[200:650, 0:1280] gray_img2 = gray_img2[200:650, 0:1280] # Find Points of Correspondence f1, f2 = findpts(gray_img1, gray_img2) F, m = cv2.findFundamentalMat
if Translation_final[2] > 0: Translation_final = -Translation_final return Rotation_final, Translation_final H_Start = np.identity(4) p_0 = np.array([[0, 0, 0, 1]]).T flag = 0 data_points = [] for index in range(18, len(frames) - 1): print(frames[index], index) img1 = cv2.imread(pathimage + str(frames[index]), 0) distorted_image1 = cv2.cvtColor(img1, cv2.COLOR_BayerGR2BGR) undistorted_image1 = UndistortImage(distorted_image1, LUT) grayscale_1 = cv2.cvtColor(undistorted_image1, cv2.COLOR_BGR2GRAY) img2 = cv2.imread(pathimage + str(frames[index + 1]), 0) distorted_image2 = cv2.cvtColor(img2, cv2.COLOR_BayerGR2BGR) undistorted_image2 = UndistortImage(distorted_image2, LUT) grayscale_2 = cv2.cvtColor(undistorted_image2, cv2.COLOR_BGR2GRAY) frame_image1 = grayscale_1[200:650, 0:1280] frame_image2 = grayscale_2[200:650, 0:1280] sift = cv2.xfeatures2d.SIFT_create() keypoints_1, descriptor_1 = sift.detectAndCompute(frame_image1, None) keypoints_2, descriptor_2 = sift.detectAndCompute(frame_image2, None)
def undistortImg(img): colorimage = cv2.cvtColor(img, cv2.COLOR_BayerGR2BGR) undistortedimage = UndistortImage(colorimage, LUT) gray = cv2.cvtColor(undistortedimage, cv2.COLOR_BGR2GRAY) return gray
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("<----------------->")