from CVImage import CVImage from Matcher import Matcher import cv2 import numpy as np from VisualOdometry import VisualOdometry import matplotlib as mplt import matplotlib.pyplot as plt match = Matcher() img = CVImage('/home/cesar/Documentos/Computer_Vision/01/image_0') img.read_image() img.copy_image() img.acquire() cv2.namedWindow('Invariant', cv2.WINDOW_NORMAL) cv2.imshow('Invariant', img.invariant_image) cv2.waitKey(0) cv2.destroyAllWindows() h = img.new_image.shape[0] w = img.new_image.shape[1] n = 2 # Number of roi's size = np.array([[w / n], [h / n]], np.int32) start = np.array([[0], [0]], np.int32) # Create roi roi = img.crop_image(start, size, img.new_image) roi_prev = img.crop_image(start, size, img.prev_image) match.match(roi, roi_prev)
from CVImage import CVImage from Matcher import Matcher import cv2 import numpy as np from VisualOdometry import VisualOdometry import matplotlib as mplt import matplotlib.pyplot as plt match = Matcher() img = CVImage('/home/cesar/Documentos/Computer_Vision/01/image_0') img.read_image() img.copy_image() img.acquire() h = img.new_image.shape[0] w = img.new_image.shape[1] n = 2 # Number of roi's size = np.array([[w / n], [h / n]], np.int32) start = np.array([[0], [0]], np.int32) # Create roi roi = img.crop_image(start, size, img.new_image) roi_prev = img.crop_image(start, size, img.prev_image) match.match(roi, roi_prev) print "good_matches bf", len(match.good_matches) print "good_kp1", len(match.good_kp1) print "good_kp2", len(match.good_kp2) match.draw_matches(roi, match.good_matches) cv2.namedWindow('roi', cv2.WINDOW_NORMAL)
def run(): match = Matcher() img = CVImage('/home/cesar/Documentos/Computer_Vision/01/image_0') # img = CVImage('/home/cesar/Documentos/Computer_Vision/images_test') # Load images img.read_image() img.copy_image() img.acquire() # t = threading.Thread(target=plot_image, args=(img.new_image, )) # t.start() # Correlate p1, p2 = correlate_image(match, img, 2, 7) print ("Total number of keypoints in second image: \ {}".format(len(match.global_kpts1))) print ("Total number of keypoints in first image: \ {}".format(len(match.global_kpts2))) if not match.is_minmatches: print "There aren't matches after filtering. Iterate to next image..." return # Plot keypoints plot_matches(match, img) # t.__stop() # Now, estimate F vo = VisualOdometry() match.global_kpts1, match.global_kpts2 = \ vo.EstimateF_multiprocessing(match.global_kpts2, match.global_kpts1) # Get structure of the scene, up to a projectivity scene = get_structure(match, img, vo) # Optimize F # param_opt, param_cov = vo.optimize_F(match.global_kpts1, match.global_kpts2) # vo.cam2.set_P(param_opt[:9].reshape((3, 3))) # scene = vo.recover_structure(param_opt) # Plot it plot_scene(scene) # Get the Essential matrix vo.E_from_F() print vo.F print vo.E # Recover pose R, t = vo.get_pose(match.global_kpts1, match.global_kpts2, vo.cam1.focal, vo.cam1.pp) print R print t # Compute camera matrix 2 print "CAM2", vo.cam2.P vo.cam2.compute_P(R, t) print "CAM2", vo.cam2.P # Get the scene scene = get_structure_normalized(match, img, vo) plot_scene(scene) # What have we stored? print ("Permanent Keypoints in the first image stored: \ {}".format(type(match.curr_kp[0]))) print ("Permanent descriptors in the first image stored: \ {}".format(len(match.curr_dsc))) print ("Format of global keypoints: \ {}".format(type(match.global_kpts1))) print ("Format of global keypoints: \ {}".format(type(match.global_kpts1[0]))) print ("Shape of global kpts1: {}".format(np.shape(match.global_kpts1))) # print ("global keypoint: \ # {}".format(match.global_kpts1[0])) # Acquire image img.copy_image() img.acquire() d, prev_points, points_tracked = match.lktracker(img.prev_image, \ img.new_image, match.global_kpts2) print ("Points tracked: \ {}".format(len(points_tracked))) plot_two_points(np.reshape(match.global_kpts2, (len(match.global_kpts2), 2)), prev_points, img) test = [] for (x, y), good_flag in zip(match.global_kpts2, d): if not good_flag: continue test.append((x, y)) # plot_two_points(np.reshape(match.global_kpts2, (len(match.global_kpts2), 2)), # np.asarray(points_tracked), img) plot_two_points(np.asarray(test), np.asarray(points_tracked), img) # points, st, err = cv2.calcOpticalFlowPyrLK(img.prev_grey, img.new_image, # match.global_kpts2, None, # **lk_params) # print len(points) print "Shape of p1: {}".format(np.shape(p1)) plane = vo.opt_triangulation(p1, p2, vo.cam1.P, vo.cam2.P) plot_scene(plane) print "Shpe of plane: {}".format(np.shape(plane)) print "Type of plane: {}".format(type(plane)) print np.transpose(plane[:, :3]) plane = np.transpose(plane) print "shape plane: {}".format(np.shape(plane)) plane_inhomogeneous = np.delete(plane, 3, 1) print "shape plane: {}".format(np.shape(plane_inhomogeneous)) print plane_inhomogeneous[:3, :] # Use ransac to fit a plane debug = False plane_model = RansacModel(debug) ransac_fit, ransac_data = ransac.ransac(plane_inhomogeneous, plane_model, 4, 1000, 1e-4, 50, debug=debug, return_all=True) print "Ransac fit: {}".format(ransac_fit) # PLot the plane X, Y = np.meshgrid(np.arange(-0.3, 0.7, 0.1), np.arange(0, 0.5, 0.1)) Z = -(ransac_fit[0] * X - ransac_fit[1] * Y - ransac_fit[3]) / ransac_fit[2] plot_plane(X, Y, Z, plane_inhomogeneous[ransac_data['inliers']])
def run(): match = Matcher() img = CVImage('/home/cesar/Documentos/Computer_Vision/01/image_0') img.read_image() img.copy_image() img.acquire() h = img.new_image.shape[0] w = img.new_image.shape[1] n = 2 # Number of roi's size = np.array([[w / n], [h / n]], np.int32) start = np.array([[0], [0]], np.int32) # First roi correlate_roi(match, img, size, start) # Second roi start = np.array([[w / n], [0]]) correlate_roi(match, img, size, start) # Third roi start = np.array([[0], [h / n]]) correlate_roi(match, img, size, start) # Last roi start = np.array([[w / n], [h / n]]) correlate_roi(match, img, size, start) # We have stored two times every original keypoint (curr_kp, prev_kp) match.curr_kp = match.curr_kp[::2] match.prev_kp = match.prev_kp[::2] # The same applies for the descriptors match.curr_dsc = match.curr_dsc[::2] match.prev_dsc = match.prev_dsc[::2] print match.curr_kp[0].pt print match.global_kpts1[0] # Print the total number of keypoints encountered print("Total number of keypoints encountered: \ {}".format(get_number_keypoints(match))) # Test the plot_same_figure function # plot_same_figure(match, img) # plot_one(match, img) # plot_save(match, img) # Get Fundamental Matrix vo = VisualOdometry() print "Type of match.global_kpts1: ", type(match.global_kpts1) match.global_kpts1, match.global_kpts2 = \ vo.EstimateF_multiprocessing(match.global_kpts2, match.global_kpts1) # plot_one(match, img) # plot_one_np(vo.outlier_points_new, img) # plot_together_np(match.global_kpts1, vo.outlier_points_new, img) print("Total number of keypoints encountered: \ {}".format(get_number_keypoints(match))) # Triangulate. To get the actual movement of the camera we are "swapping" # the scene. The first camera is cam1.P, the first keypoints are # global_kpts1. On the other hand, the second camera is cam2.P and the # second keypoints are global_kpts2 scene = get_structure(match, img, vo) print "ESCENA", scene[:, :20] print "PROYECCION EN SEGUNDA", vo.cam1.project(scene[:, :20]) print "SEGUNDA", match.global_kpts1[:20] print "CORREGIDOS SEGUNDA", vo.correctedkpts1[:, :20] print "PROYECCION EN PRIMERA", vo.cam2.project(scene[:, :20]) print "PRIMERA", match.global_kpts2[:20] print "CORREGIDOS EN PRIMERA", vo.correctedkpts2[:, :20] fig = plt.figure() ax = fig.gca(projection='3d') ax.plot(scene[0], scene[1], scene[2], 'ko') plt.axis('equal') plt.show()