diff --git a/Camera.py b/Camera.py index 9314275..8358730 100644 --- a/Camera.py +++ b/Camera.py @@ -1,32 +1,53 @@ import numpy as np import cv2 +# from numba import njit -def featureMapping(image): - orb = cv2.ORB_create() - pts = cv2.goodFeaturesToTrack(np.mean(image, axis=2).astype(np.uint8), 1000, qualityLevel=0.01, minDistance=7) - key_pts = [cv2.KeyPoint(x=f[0][0], y=f[0][1], _size=20) for f in pts] - key_pts, descriptors = orb.compute(image, key_pts) - - # Return Key_points and ORB_descriptors - return np.array([(kp.pt[0], kp.pt[1]) for kp in key_pts]), descriptors +def featureMappingORB(frame): + orb = cv2.ORB_create() + pts = cv2.goodFeaturesToTrack(np.mean(frame, axis=2).astype(np.uint8), 1000, qualityLevel=0.01, minDistance=7) + key_pts = [cv2.KeyPoint(x=f[0][0], y=f[0][1], size=20) for f in pts] + key_pts, descriptors = orb.compute(frame, key_pts) + # Return Key_points and ORB_descriptors + return np.array([(kp.pt[0], kp.pt[1]) for kp in key_pts]), descriptors def normalize(count_inv, pts): - return np.dot(count_inv, np.concatenate([pts, np.ones((pts.shape[0], 1))], axis=1).T).T[:, 0:2] + return np.dot(count_inv, np.concatenate([pts, np.ones((pts.shape[0], 1))], axis=1).T).T[:, 0:2] def denormalize(count, pt): - ret = np.dot(count, np.array([pt[0], pt[1], 1.0])) - ret /= ret[2] - return int(round(ret[0])), int(round(ret[1])) + ret = np.dot(count, np.array([pt[0], pt[1], 1.0])) + ret /= ret[2] + return int(round(ret[0])), int(round(ret[1])) + +# @njit +def triangulate(pose1, pose2, pts1, pts2): + """ + change on cv.triangulatePoints + + Recreating bunch of points using Triangulation + Given the relative poses, calculating the 3d points + """ + ret = np.zeros((pts1.shape[0], 4)) + pose1 = np.linalg.inv(pose1) + pose2 = np.linalg.inv(pose2) + for i, p in enumerate(zip(pts1, pts2)): + A = np.zeros((4,4)) + A[0] = p[0][0] * pose1[2] - pose1[0] + A[1] = p[0][1] * pose1[2] - pose1[1] + A[2] = p[1][0] * pose2[2] - pose2[0] + A[3] = p[1][1] * pose2[2] - pose2[1] + _, _, vt = np.linalg.svd(A) + ret[i] = vt[3] + return ret / ret[:, 3:] Identity = np.eye(4) -class Camera(object): +class Camera: def __init__(self, desc_dict, image, count): - self.count = count - self.count_inv = np.linalg.inv(self.count) + self.count_inv = np.linalg.inv(count) self.pose = Identity - self.h, self.w = image.shape[0:2] - key_pts, self.descriptors = featureMapping(image) + self.h, self.w = image.shape[0:2] + key_pts, self.descriptors = featureMappingORB(image) self.key_pts = normalize(self.count_inv, key_pts) self.pts = [None]*len(self.key_pts) self.id = len(desc_dict.frames) desc_dict.frames.append(self) + diff --git a/README.md b/README.md index 01e4ff4..5b41250 100644 --- a/README.md +++ b/README.md @@ -171,3 +171,14 @@ Distributed under the MIT License. See `LICENSE` for more information. [linkedin-shield]: https://img.shields.io/badge/-LinkedIn-black.svg?style=flat-square&logo=linkedin&colorB=555 [linkedin-url]: https://www.linkedin.com/in/akshat-bajpai [product-screenshot]: https://user-images.githubusercontent.com/35187768/97795404-1207b600-1bc3-11eb-95e7-5dbcd1419310.gif + + +## New Feature +![Screenshot](img_slam_01.png) + +New Feature (color points) + +Road Trip - NCV | 0047 | Creative Commons | No Copyright Videos +NoCopyrightVideos is a copyright free / stream safe, providing free to use videos. +NCV Video is free to use for independent Creators and the Creative Commons Zero (CC0) +https://www.youtube.com/watch?v=K5eLoD9Kyso diff --git a/__pycache__/Camera.cpython-38.pyc b/__pycache__/Camera.cpython-38.pyc deleted file mode 100644 index 44e354f..0000000 Binary files a/__pycache__/Camera.cpython-38.pyc and /dev/null differ diff --git a/__pycache__/descriptor.cpython-38.pyc b/__pycache__/descriptor.cpython-38.pyc deleted file mode 100644 index 2198e8c..0000000 Binary files a/__pycache__/descriptor.cpython-38.pyc and /dev/null differ diff --git a/__pycache__/display.cpython-38.pyc b/__pycache__/display.cpython-38.pyc deleted file mode 100644 index d428e14..0000000 Binary files a/__pycache__/display.cpython-38.pyc and /dev/null differ diff --git a/__pycache__/match_frames.cpython-38.pyc b/__pycache__/match_frames.cpython-38.pyc deleted file mode 100644 index 4e822b5..0000000 Binary files a/__pycache__/match_frames.cpython-38.pyc and /dev/null differ diff --git a/__pycache__/triangulation.cpython-38.pyc b/__pycache__/triangulation.cpython-38.pyc deleted file mode 100644 index bfef030..0000000 Binary files a/__pycache__/triangulation.cpython-38.pyc and /dev/null differ diff --git a/cameraFrame.py b/cameraFrame.py deleted file mode 100644 index 17955bf..0000000 --- a/cameraFrame.py +++ /dev/null @@ -1,32 +0,0 @@ -import numpy as np -import cv2 - -def featureMapping(image): - orb = cv2.ORB_create() - pts = cv2.goodFeaturesToTrack(np.mean(image, axis=2).astype(np.uint8), 1000, qualityLevel=0.01, minDistance=7) - key_pts = [cv2.KeyPoint(x=f[0][0], y=f[0][1], _size=20) for f in pts] - key_pts, descriptors = orb.compute(image, key_pts) - - # Return Key_points and ORB_descriptors - return np.array([(kp.pt[0], kp.pt[1]) for kp in key_pts]), descriptors - -def normalize(count_inv, pts): - return np.dot(count_inv, add_ones(pts).T).T[:, 0:2] - -def denormalize(count, pt): - ret = np.dot(count, np.array([pt[0], pt[1], 1.0])) - ret /= ret[2] - return int(round(ret[0])), int(round(ret[1])) - -Identity = np.eye(4) -class Camera(object): - def __init__(self, desc_dict, image, count): - self.count = count - self.count_inv = np.linalg.inv(self.count) - self.pose = Identity - self.h, self.w = image.shape[0:2] - key_pts, self.descriptors = featureMapping(image) - self.key_pts = normalize(self.count_inv, key_pts) - self.pts = [None]*len(self.key_pts) - self.id = len(desc_dict.frames) - desc_dict.frames.append(self) diff --git a/descriptor.py b/descriptor.py index e5d9233..81de22b 100644 --- a/descriptor.py +++ b/descriptor.py @@ -2,120 +2,175 @@ import numpy as np import OpenGL.GL as gl import pangolin -import g2o - -class Point(object): - # A Point is a 3-D point in the world - # Each Point is observed in multiple Frames - - def __init__(self, mapp, loc): - self.pt = loc - self.frames = [] - self.idxs = [] - - self.id = len(mapp.points) - mapp.points.append(self) - - def add_observation(self, frame, idx): - frame.pts[idx] = self - self.frames.append(frame) - self.idxs.append(idx) - -class Descriptor(object): - def __init__(self): - self.frames = [] - self.points = [] - self.state = None - self.q = None - # G2O optimization: - def optimize(self): - err = optimize(self.frames, self.points, local_window, fix_points, verbose, rounds) - - # Key-Point Pruning: - culled_pt_count = 0 - for p in self.points: - # <= 4 match point that's old - old_point = len(p.frames) <= 4 and p.frames[-1].id+7 < self.max_frame - #handling the reprojection error - errs = [] - for f,idx in zip(p.frames, p.idxs): - uv = f.kps[idx] - proj = np.dot(f.pose[:3], p.homogeneous()) - proj = proj[0:2] / proj[2] - errs.append(np.linalg.norm(proj-uv)) - if old_point or np.mean(errs) > CULLING_ERR_THRES: - culled_pt_count += 1 - self.points.remove(p) - p.delete() - - return err - - def create_viewer(self): - self.q = Queue() - self.vp = Process(target=self.viewer_thread, args=(self.q,)) - self.vp.daemon = True - self.vp.start() - - def viewer_thread(self, q): - self.viewer_init(1024, 768) - while 1: - self.viewer_refresh(q) - - def viewer_init(self, w, h): - pangolin.CreateWindowAndBind('Main', w, h) - gl.glEnable(gl.GL_DEPTH_TEST) - - self.scam = pangolin.OpenGlRenderState( - pangolin.ProjectionMatrix(w, h, 420, 420, w//2, h//2, 0.2, 10000), - pangolin.ModelViewLookAt(0, -10, -8, - 0, 0, 0, - 0, -1, 0)) - self.handler = pangolin.Handler3D(self.scam) - - # Create Interactive View in window - self.dcam = pangolin.CreateDisplay() - self.dcam.SetBounds(0.0, 1.0, 0.0, 1.0, -w/h) - self.dcam.SetHandler(self.handler) - - def viewer_refresh(self, q): - if self.state is None or not q.empty(): - self.state = q.get() - - gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT) - gl.glClearColor(0, 0, 0, 0) - self.dcam.Activate(self.scam) - - # Draw Point Cloud - #points = np.random.random((10000, 3)) - #colors = np.zeros((len(points), 3)) - #colors[:, 1] = 1 -points[:, 0] - #colors[:, 2] = 1 - points[:, 1] - #colors[:, 0] = 1 - points[:, 2] - #points = points * 3 + 1 - #gl.glPointSize(10) - #pangolin.DrawPoints(self.state[1], colors) - - # draw keypoints - gl.glPointSize(2) - gl.glColor3f(0.184314, 0.309804, 0.184314) - pangolin.DrawPoints(self.state[1]+1) - gl.glPointSize(1) - gl.glColor3f(0.3099, 0.3099,0.184314) - pangolin.DrawPoints(self.state[1]) - - # draw poses - gl.glColor3f(0.0, 1.0, 1.0) - pangolin.DrawCameras(self.state[0]) - - pangolin.FinishFrame() - - def display(self): - if self.q is None: - return - poses, pts = [], [] - for f in self.frames: - poses.append(f.pose) - for p in self.points: - pts.append(p.pt) - self.q.put((np.array(poses), np.array(pts))) +# import g2o + +def draw_axis(size): + gl.glColor3f(1.0, 0.0, 0.0) + pangolin.DrawLine([[0.0, 0.0, 0.0], [size, 0.0, 0.0]]) + gl.glColor3f(0.0, 1.0, 0.0) + pangolin.DrawLine([[0.0, 0.0, 0.0], [0.0, -size, 0]]) + gl.glColor3f(0.0, 0.0, 1.0) + pangolin.DrawLine([[0.0, 0.0, 0.0], [0.0, 0.0, size]]) + +def draw_grid(col): + gl.glColor3f(0.7, 0.7, 0.7) + for line in np.arange(col+1): + pangolin.DrawLine([[line, 0.0, 0.0], [line, 0.0, col]]) + pangolin.DrawLine([[0.0, 0.0, line], [col, 0.0, line]]) + +def draw_keypoints(psize, points): + gl.glPointSize(psize) + gl.glColor3f(0.2, 0.6, 0.4) + pangolin.DrawPoints(points) + + +class Point: + # A Point is a 3-D point in the world + # Each Point is observed in multiple Frames + def __init__(self, mapp, loc): + self.pt = loc + self.frames = [] + self.idxs = [] + self.color = None + self.id = len(mapp.points) + mapp.points.append(self) + + def add_observation(self, frame, idx): + frame.pts[idx] = self + self.frames.append(frame) + self.idxs.append(idx) + + def add_color(self, color): + self.color = np.single(color) / 255. + +class Descriptor: + """Doc Descriptor""" + def __init__(self, width = 1280, height = 720, psize = 2): + self.width, self.height = width, height + self.frames = [] + self.points = [] + self.state = None + self.q3D = None # 3D data queue + self.psize = psize + self.tr = [] + self.mvla = (0, -20, -20, 0, 0, 0, 0, -1, 0) + self.pmx = (width, height, 420, 420, + width//2, height//2, 0.2, 10000) + + # G2O optimization: + def optimize(self): + """ This method does not work, in development """ + err = optimize(self.frames, self.points, local_window, fix_points, verbose, rounds) + # Key-Point Pruning: + culled_pt_count = 0 + for p in self.points: + # <= 4 match point that's old + old_point = len(p.frames) <= 4 and p.frames[-1].id+7 < self.max_frame + #handling the reprojection error + errs = [] + for f,idx in zip(p.frames, p.idxs): + uv = f.kps[idx] + proj = np.dot(f.pose[:3], p.homogeneous()) + proj = proj[0:2] / proj[2] + errs.append(np.linalg.norm(proj-uv)) + if old_point or np.mean(errs) > CULLING_ERR_THRES: + culled_pt_count += 1 + self.points.remove(p) + p.delete() + + return err + + def create_viewer(self): + self.q3D = Queue() + self.vp = Process(target=self.viewer_thread, args=(self.q3D,)) + self.vp.daemon = True + self.vp.start() + + def release(self): + # self.vp.kill() + return self.vp.terminate() + + def viewer_thread(self, q3d): + self.viewer_init() + while True: + self.viewer_refresh(q3d) + + def viewer_init(self): + pangolin.CreateWindowAndBind('Viewport', self.width, self.height) + gl.glEnable(gl.GL_DEPTH_TEST) + + self.scam = pangolin.OpenGlRenderState( + pangolin.ProjectionMatrix(*self.pmx), + pangolin.ModelViewLookAt(*self.mvla)) + self.handler = pangolin.Handler3D(self.scam) + + # Create Interactive View in window + self.dcam = pangolin.CreateDisplay() + self.dcam.SetBounds(0.0, 1.0, 0.0, 1.0, -self.width/self.height) + self.dcam.SetHandler(self.handler) + + # Create panel + self.panel = pangolin.CreatePanel('ui') + self.panel.SetBounds(0.0, 0.2, 0.0, 100/640.) + self.psize = pangolin.VarFloat('ui.Point_size', value=2, min=1, max=5) + self.gain = pangolin.VarFloat('ui.Gain', value=1.0, min=0, max=3) + self.background = pangolin.VarFloat('ui.Background', value=0.0, min=0, max=1) + self.alpha = pangolin.VarFloat('ui.Alpha', value=1.0, min=0, max=1) + self.screenshot = pangolin.VarBool('ui.Screenshot', value=False, toggle=False) + + def viewer_refresh(self, q3d): + if self.state is None or not q3d.empty(): + self.state = q3d.get() + + gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT) + gl.glClearColor(self.background, self.background, self.background, self.alpha) + self.dcam.Activate(self.scam) + + if pangolin.Pushed(self.screenshot): + pangolin.SaveWindowOnRender('screenshot_'+str(len(self.state[1]))) + + # draw Axis and Grid + draw_axis(3.0) + draw_grid(5.0) + + # draw keypoints + gl.glPointSize(self.psize) + # gl.glColor3f(self.R, self.G, self.B) + # mul = np.array([self.R, self.G, self.B]) + pangolin.DrawPoints(self.state[1], self.state[4]*np.single(self.gain)) + + # draw trajectory + gl.glLineWidth(1) + gl.glColor3f(1.0, 0.0, 0.0) + pangolin.DrawLine(self.state[2]) + + # draw all poses + gl.glColor3f(0.75, 0.75, 0.15) + pangolin.DrawCameras(self.state[0], 0.75, 0.75, 0.75) + + # draw current pose + gl.glColor3f(1.0, 0.0, 0.0) + pangolin.DrawCameras(self.state[3], 1.5, 0.75, 1.0) + + pangolin.FinishFrame() + + def put3D(self): + ''' put 3D data in Queue ''' + if self.q3D is None: + return + poses, pts, cam_pts, color = [], [], [], [] + # get last element of list + current_pose = [self.frames[-1].pose] + for f in self.frames: + x = f.pose.ravel()[3] + y = f.pose.ravel()[7] + z = f.pose.ravel()[11] + cam_pts.append([x, y, z]) + poses.append(f.pose) + for p in self.points: + pts.append(p.pt) + color.append(p.color) + self.q3D.put((np.array(poses[:-1]), np.array(pts), + np.array(cam_pts), np.array(current_pose), + np.array(color))) diff --git a/display.py b/display.py deleted file mode 100644 index 1a9459e..0000000 --- a/display.py +++ /dev/null @@ -1,14 +0,0 @@ -# PyGame > SDL2 > OpenCV -import pygame -from pygame.locals import DOUBLEBUF - -class Display(object): - def __init__(self, W, H): - pygame.init() - self.screen = pygame.display.set_mode((W, H), DOUBLEBUF) - self.surface = pygame.Surface(self.screen.get_size()).convert() - - def display2D(self, img): - pygame.surfarray.blit_array(self.surface, img.swapaxes(0,1)[:, :, [0,1,2]]) - self.screen.blit(self.surface, (0,0)) - pygame.display.flip() diff --git a/img_slam_01.png b/img_slam_01.png new file mode 100644 index 0000000..5b06607 Binary files /dev/null and b/img_slam_01.png differ diff --git a/match_frames.py b/match_frames.py index 43ec210..5ed1df4 100644 --- a/match_frames.py +++ b/match_frames.py @@ -6,71 +6,77 @@ from skimage.transform import FundamentalMatrixTransform from skimage.transform import EssentialMatrixTransform -# turn [[x,y]] -> [[x,y,1]] -def add_ones(x): - return np.concatenate([x, np.ones((x.shape[0], 1))], axis=1) - #Getting rid of the outliers: def extractRt(F): - W = np.mat([[0,-1,0],[1,0,0],[0,0,1]],dtype=float) - U,d,Vt = np.linalg.svd(F) - #assert np.linalg.det(U) > 0 - if np.linalg.det(Vt) < 0: - Vt *= -1.0 - R = np.dot(np.dot(U, W), Vt) - if np.sum(R.diagonal()) < 0: - R = np.dot(np.dot(U, W.T), Vt) - t = U[:, 2] - ret = np.eye(4) - ret[:3, :3] = R - ret[:3, 3] = t - #print(d) - return ret + W = np.mat([[0,-1,0],[1,0,0],[0,0,1]],dtype=float) + U, d, Vt = np.linalg.svd(F) + #assert np.linalg.det(U) > 0 + if np.linalg.det(Vt) < 0: + Vt *= -1.0 + R = np.dot(np.dot(U, W), Vt) + if np.sum(R.diagonal()) < 0: + R = np.dot(np.dot(U, W.T), Vt) + t = U[:, 2] + ret = np.eye(4) + ret[:3, :3] = R + ret[:3, 3] = t + #print(d) + return ret -#We make use of the Lowe's Ratio Test: -def generate_match(f1, f2): - bf = cv2.BFMatcher(cv2.NORM_HAMMING) - matches = bf.knnMatch(f1.descriptors, f2.descriptors, k=2) +def SLAMBFMatcher(des1, des2): + bf = cv2.BFMatcher(cv2.NORM_HAMMING) + return bf.knnMatch(des1.descriptors, des2.descriptors, k=2) - # Lowe's ratio test - ret = [] - x1,x2 = [], [] +def SLAMFlannBasedMatcher(des1, des2): + FLANN_INDEX_KDTREE = 1 + index_params = dict(algorithm = FLANN_INDEX_KDTREE, trees = 5) + search_params = dict(checks = 50) + flann = cv2.FlannBasedMatcher(index_params, search_params) + return flann.knnMatch(des1.descriptors, des2.descriptors, k=2) + +def generate_match(f1, f2): + ''' Rewrite the Function into an Object (class) ''' - for m,n in matches: - if m.distance < 0.75*n.distance: - pts1 = f1.key_pts[m.queryIdx] - pts2 = f2.key_pts[m.trainIdx] + matches = SLAMBFMatcher(f1, f2) + # bf = cv2.BFMatcher(cv2.NORM_HAMMING) + # matches = bf.knnMatch(f1.descriptors, f2.descriptors, k=2) + + ret = [] + x1, x2 = [], [] - # travel less than 10% of diagonal and be within orb distance 32 - if np.linalg.norm((pts1-pts2)) < 0.1*np.linalg.norm([f1.w, f1.h]) and m.distance < 32: - # keep around indices - # TODO: refactor this to not be O(N^2) - if m.queryIdx not in x1 and m.trainIdx not in x2: - x1.append(m.queryIdx) - x2.append(m.trainIdx) + for m,n in matches: + if m.distance < 0.75*n.distance: + pts1 = f1.key_pts[m.queryIdx] + pts2 = f2.key_pts[m.trainIdx] - ret.append((pts1, pts2)) + # travel less than 10% of diagonal and be within orb distance 32 + if np.linalg.norm((pts1-pts2)) < 0.1*np.linalg.norm([f1.w, f1.h]) and m.distance < 32: + # keep around indices + # TODO: refactor this to not be O(N^2) + if m.queryIdx not in x1 and m.trainIdx not in x2: + x1.append(m.queryIdx) + x2.append(m.trainIdx) + ret.append((pts1, pts2)) - # no duplicates - assert(len(set(x1)) == len(x1)) - assert(len(set(x2)) == len(x2)) + # no duplicates + assert(len(set(x1)) == len(x1)) + assert(len(set(x2)) == len(x2)) + + assert len(ret) >= 8 - assert len(ret) >= 8 - ret = np.array(ret) - x1 = np.array(x1) - x2 = np.array(x2) + ret = np.array(ret) + x1 = np.array(x1) + x2 = np.array(x2) - # fit matrix - model, f_pts = ransac((ret[:, 0], ret[:, 1]), - FundamentalMatrixTransform, - #EssentialMatrixTransform, - min_samples=8, - residual_threshold=0.001, - max_trials=100) - print("Matches: %d -> %d -> %d -> %d" % (len(f1.descriptors), len(matches), len(f_pts), sum(f_pts))) + # RANdom SAmple Consensus + model, f_pts = ransac((ret[:, 0], ret[:, 1]), + FundamentalMatrixTransform, + # EssentialMatrixTransform, + min_samples=8, + residual_threshold=0.001, + max_trials=1000) - # ignore outliers - Rt = extractRt(model.params) + # print("Matches: %d -> %d -> %d -> %d" % (len(f1.descriptors), len(matches), len(f_pts), sum(f_pts))) + Rt = extractRt(model.params) + return x1[f_pts], x2[f_pts], Rt - # return - return x1[f_pts], x2[f_pts], Rt diff --git a/road.mp4 b/road.mp4 new file mode 100644 index 0000000..58f3b15 Binary files /dev/null and b/road.mp4 differ diff --git a/slam.py b/slam.py index e955be8..7044ef5 100644 --- a/slam.py +++ b/slam.py @@ -1,90 +1,106 @@ +""" +Pythonic implementation of an ORB feature matching +based Monocular-vision SLAM. +""" + import numpy as np +np.seterr(divide='ignore', invalid='ignore') +np.finfo(np.dtype("float32")) +np.finfo(np.dtype("float64")) import cv2 -import os,sys,time,g2o -from triangulation import triangulate -from Camera import denormalize, normalize, Camera -from display import Display +import os, sys, time +from Camera import denormalize, normalize, triangulate, Camera from match_frames import generate_match from descriptor import Descriptor, Point +def show_attributes(frame, attribut): + cv2.rectangle(frame, (30, 0), (110, 45), (110,50,30), -1) + cv2.putText(frame, attribut, (45, 30), cv2.FONT_HERSHEY_SIMPLEX , 0.5, (255,255,255), 1) -F= int(os.getenv("F","500")) -W, H = 1920//2, 1080//2 -K = np.array([[F,0,W//2],[0,F,H//2],[0,0,1]]) -desc_dict = Descriptor() -# if os.getenv("D3D") is not None: -desc_dict.create_viewer() - -# disp = None -# if os.getenv("D2D") is not None: -disp = Display(W, H) +class SLAM: + def __init__(self, focal_length = 500, width = 1920, height = 1080, psize = 2): + self.F = focal_length + # self.W, self.H = width, height + self.W, self.H = width//2, height//2 + self.K = np.array([[self.F, 0, self.W//2], + [0, self.F, self.H//2], + [0, 0, 1]]) + self.desc_dict = Descriptor(psize=psize) + self.desc_dict.create_viewer() + self.image = None + print('Init SLAM', '\nFocal length:', self.F) + def calibrate(self, image): + # camera intrinsics...<================ Check this + return cv2.resize(image, (self.W, self.H)) -def calibrate(image): - # camera intrinsics...<================Check this - image = cv2.resize(image, (W,H)) - return image + def generate(self, image): + self.image = self.calibrate(image) + # self.image = image + frame = Camera(self.desc_dict, self.image, self.K) + if frame.id == 0: + return + frame1 = self.desc_dict.frames[-1] + frame2 = self.desc_dict.frames[-2] + x1, x2, Id = generate_match(frame1, frame2) + frame1.pose = np.dot(Id, frame2.pose) + for i, idx in enumerate(x2): + if frame2.pts[idx] is not None: + frame2.pts[idx].add_observation(frame1, x1[i]) + # homogeneous 3-D coords + pts4d = triangulate(frame1.pose, frame2.pose, frame1.key_pts[x1], frame2.key_pts[x2]) + # pts4d /= pts4d[:, 3:] + unmatched_points = np.array([frame1.pts[i] is None for i in x1]) + # print("Adding: %d points" % np.sum(unmatched_points)) + good_pts4d = (np.abs(pts4d[:, 3]) > 0.005) & (pts4d[:, 2] > 0) & unmatched_points -def generate_SLAM(image): - image = calibrate(image) - print("Thisis a test0") - frame = Camera(desc_dict, image, K) - if frame.id == 0: - return - frame1 = desc_dict.frames[-1] - frame2 = desc_dict.frames[-2] + for i, p in enumerate(pts4d): + if not good_pts4d[i]: + continue + pt = Point(self.desc_dict, p) + pt.add_observation(frame1, x1[i]) + pt.add_observation(frame2, x2[i]) + cx, cy = denormalize(self.K, frame1.key_pts[x1][i]) + pt.add_color(cv2.cvtColor(self.image, cv2.COLOR_BGR2RGB)[cy, cx]) - x1,x2,Id = generate_match(frame1,frame2) - frame1.pose =np.dot(Id,frame2.pose) - for i,idx in enumerate(x2): - if frame2.pts[idx] is not None: - frame2.pts[idx].add_observation(frame1,x1[i]) - # homogeneous 3-D coords - print("Thisis a test1") - pts4d = triangulate(frame1.pose, frame2.pose, frame1.key_pts[x1], frame2.key_pts[x2]) - pts4d /= pts4d[:, 3:] - unmatched_points = np.array([frame1.pts[i] is None for i in x1]) - print("Adding: %d points" % np.sum(unmatched_points)) - good_pts4d = (np.abs(pts4d[:, 3]) > 0.005) & (pts4d[:, 2] > 0) & unmatched_points + for pt1, pt2 in zip(frame1.key_pts[x1], frame2.key_pts[x2]): + u1, v1 = denormalize(self.K, pt1) + u2, v2 = denormalize(self.K, pt2) + cv2.drawMarker(self.image, (u1, v1), (0, 255, 0), 1, 15, 1, 8) + cv2.line(self.image, (u1, v1), (u2, v2), (0, 0, 255), 1) + show_attributes(self.image, 'ORB') - for i,p in enumerate(pts4d): - if not good_pts4d[i]: - continue - pt = Point(desc_dict, p) - pt.add_observation(frame1, x1[i]) - pt.add_observation(frame2, x2[i]) + # 3D display (put 3D data in Queue) + self.desc_dict.put3D() - for pt1, pt2 in zip(frame1.key_pts[x1], frame2.key_pts[x2]): - u1, v1 = denormalize(K, pt1) - u2, v2 = denormalize(K, pt2) - cv2.circle(image, (u1, v1), color=(0,255,0), radius=1) - cv2.line(image, (u1, v1), (u2, v2), color=(255, 255,0)) + def __del__(self): + print('Close SLAM') + return self.desc_dict.release() - # 2-D display - if disp is not None: - disp.display2D(image) - # 3-D display - desc_dict.display() if __name__ == "__main__": if len(sys.argv) < 2: print("%s takes in .mp4 as an arg" %sys.argv[0]) exit(-1) - print("Thisis a test-1") + cap = cv2.VideoCapture(sys.argv[1]) # Can try Realtime(highly unlikely though) - test= Display(W,H) - print("Thisis a test-2") + cap.set(cv2.CAP_PROP_POS_FRAMES, 10) + slam = SLAM(500, 1920, 1080, 2) while cap.isOpened(): ret, frame = cap.read() - print("Thisis a test-3") - frame1 = cv2.resize(frame, (720,400)) #Resizing the original window if ret == True: - print("Thisis a test") - cv2.imshow("Frame",frame1) - if cv2.waitKey(1) & 0xFF == ord('q'): #Quit Condition - break - generate_SLAM(frame) + slam.generate(frame) + if slam.image is not None: + frame_out = cv2.resize(slam.image, (int(slam.W*0.5), int(slam.H*0.5))) + cv2.imshow("SLAM", frame_out) + key = cv2.waitKey(1) + if key == ord('p'): + cv2.waitKey(-1) + elif key == ord('q') or key == 27: + break else: - break + break + cap.release() - cv2.destroyAllWindows() \ No newline at end of file + cv2.destroyAllWindows() + diff --git a/triangulation.py b/triangulation.py deleted file mode 100644 index 2927eb2..0000000 --- a/triangulation.py +++ /dev/null @@ -1,19 +0,0 @@ -import cv2 -import numpy as np - -#Recreating bunch of points using Triangulation -#Given the relative poses, calculating the 3d points -def triangulate(pose1, pose2, pts1, pts2): - - ret = np.zeros((pts1.shape[0], 4)) - pose1 = np.linalg.inv(pose1) - pose2 = np.linalg.inv(pose2) - for i, p in enumerate(zip(pts1, pts2)): - A = np.zeros((4,4)) - A[0] = p[0][0] * pose1[2] - pose1[0] - A[1] = p[0][1] * pose1[2] - pose1[1] - A[2] = p[1][0] * pose2[2] - pose2[0] - A[3] = p[1][1] * pose2[2] - pose2[1] - _, _, vt = np.linalg.svd(A) - ret[i] = vt[3] - return ret diff --git a/video_test.mp4 b/video_test.mp4 new file mode 100644 index 0000000..e516391 Binary files /dev/null and b/video_test.mp4 differ