Merge pull request #923 from soumya997/master

added monocular slam blog code

Author: vikasgupta-github

Monocular SLAM for Robotics implementation in python/README.md

@@ -0,0 +1,62 @@
+### Implementation of Monocular Visual SLAM in Python:
+
+
+## Setup pangolin for python:
+
+#### Install pangolin python:
+The [original library](https://github.com/stevenlovegrove/Pangolin) is written in c++, but there is [python binding](https://github.com/uoip/pangolin) available. 
+
+- **Install dependency:** For Ubuntu/Debian execute the below commands to install library dependencies,   
+
+```
+sudo apt-get install libglew-dev
+sudo apt-get install cmake
+sudo apt-get install ffmpeg libavcodec-dev libavutil-dev libavformat-dev libswscale-dev
+sudo apt-get install libdc1394-22-dev libraw1394-dev
+sudo apt-get install libjpeg-dev libpng-dev libtiff5-dev libopenexr-dev
+```
+
+- Don't need to follow the [Very Optional Dependencies](https://github.com/uoip/pangolin?tab=readme-ov-file#very-optional-dependencies) from the repository.
+
+- **Install the Library:** Execute the below commands to install *pangolin*,
+```
+git clone https://github.com/uoip/pangolin.git
+cd pangolin
+mkdir build
+cd build
+cmake ..
+make -j8
+cd ..
+python setup.py install
+```
+
+In the `make -j8` you might get some error, just follow the comment mentioned in this [github issue](https://github.com/uoip/pangolin/issues/33#issuecomment-717655495). Running the `python setup.py install` might throw an silly error, use this [comment](https://github.com/uoip/pangolin/issues/20#issuecomment-498211997) from the exact issue to solve this. 
+
+- Other dependencies are pip installable.
+
+ 
+## How to run?
+
+```bash
+python main.py
+```
+
+## Code structure:
+```bash
+├── display.py
+├── extractor.py
+├── pointmap.py
+├── main.py
+├── notebooks
+│   ├── bundle_adjustment.ipynb
+│   ├── mapping.ipynb
+│   └── SLAM_pipeline_step_by_step.ipynb
+
+```
+
+In the notebook section we have shown how to run all the components of a monocular slam,
+- `SLAM_pipeline_step_by_step.ipynb` Describes the entire pipeline
+- `mapping.ipynb` is another resource for mapping [source](https://github.com/SiddhantNadkarni/Parallel_SFM)
+-  `bundle_adjustment.ipynb` another great resource to understand g2o and bundle adjustment. [source](https://github.com/maxcrous/multiview_notebooks)
+
+1st notebook uses the kitti dataset (grayscale, 22 GB), [download it from here](https://www.cvlibs.net/datasets/kitti/eval_odometry.php).

Monocular SLAM for Robotics implementation in python/__pycache__/display.cpython-310.pyc

@@ -0,0 +1,27 @@
+import sdl2
+import sdl2.ext
+import cv2
+
+class Display(object):
+    def __init__(self, W, H):
+        sdl2.ext.init()
+        self.window = sdl2.ext.Window("Tim Slam", size=(W, H))
+        self.window.show()
+        self.W, self.H = W, H
+
+    def paint(self, img):
+        img = cv2.resize(img, (self.W, self.H))
+        # Retrieves a list of SDL2 events.
+        events = sdl2.ext.get_events()
+        for event in events:
+            # Checks if the event type is SDL_QUIT (window close event).
+            if event.type == sdl2.SDL_QUIT:
+                exit(0)
+        # Retrieves a 3D numpy array that represents the pixel data of the window's surface.
+        surf = sdl2.ext.pixels3d(self.window.get_surface())
+        # Updates the pixel data of the window's surface with the resized image. 
+        # img.swapaxes(0, 1) swaps the axes of the image array to match the expected format of the SDL surface.
+        surf[:, :, 0:3] = img.swapaxes(0, 1)
+        # Refreshes the window to display the updated surface.
+        self.window.refresh()
+

Monocular SLAM for Robotics implementation in python/__pycache__/extractor.cpython-310.pyc

@@ -0,0 +1,126 @@
+import cv2
+import numpy as np
+from skimage.measure import ransac
+from skimage.transform import FundamentalMatrixTransform
+import g2o
+
+def add_ones(x):
+    # concatenates the original array x with the column of ones along the second axis (columns). 
+    # This converts the N×2 array to an N×3 array where each point is represented 
+    # in homogeneous coordinates as [x,y,1].
+    return np.concatenate([x, np.ones((x.shape[0], 1))], axis=1)
+
+
+IRt = np.eye(4)
+
+def extractPose(F):
+    W = np.mat([[0,-1,0],[1,0,0],[0,0,1]])
+    U,d,Vt = np.linalg.svd(F)
+    assert np.linalg.det(U) > 0
+    if np.linalg.det(Vt) < 0:
+        Vt *= -1
+    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
+
+def extract(img):
+    orb = cv2.ORB_create()
+    
+    # Convert to grayscale
+    gray_img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
+
+    # Detection
+    pts = cv2.goodFeaturesToTrack(gray_img, 8000, qualityLevel=0.01, minDistance=10)
+
+    if pts is None:
+        return np.array([]), None
+
+    # Extraction
+    kps = [cv2.KeyPoint(f[0][0], f[0][1], 20) for f in pts]
+    kps, des = orb.compute(gray_img, kps)
+
+    return np.array([(kp.pt[0], kp.pt[1]) for kp in kps]), des
+
+def normalize(Kinv, pts):
+    # The inverse camera intrinsic matrix 𝐾 − 1 transforms 2D homogeneous points 
+    # from pixel coordinates to normalized image coordinates. This transformation centers 
+    # the points based on the principal point (𝑐𝑥 , 𝑐𝑦) and scales them 
+    # according to the focal lengths 𝑓𝑥 and 𝑓𝑦, effectively mapping the points 
+    # to a normalized coordinate system where the principal point becomes the origin and 
+    # the distances are scaled by the focal lengths.
+    return np.dot(Kinv, add_ones(pts).T).T[:, 0:2]
+    # `[:, 0:2]` selects the first two columns of the resulting array, which are the normalized x and y coordinates.
+    # `.T` transposes the result back to N x 3.
+
+
+def denormalize(K, pt):
+    ret = np.dot(K, [pt[0], pt[1], 1.0])
+    ret /= ret[2]
+    return int(round(ret[0])), int(round(ret[1]))
+
+
+class Matcher(object):
+    def __init__(self):
+        self.last = None
+
+
+def match_frames(f1, f2):
+    bf = cv2.BFMatcher(cv2.NORM_HAMMING)
+    matches = bf.knnMatch(f1.des, f2.des, k=2)
+
+    # Lowe's ratio test
+    ret = []
+    idx1, idx2 = [], []
+    for m, n in matches:
+        if m.distance < 0.75*n.distance:
+            p1 = f1.pts[m.queryIdx]
+            p2 = f2.pts[m.trainIdx]
+            
+            # Distance test
+            # dditional distance test, ensuring that the 
+            # Euclidean distance between p1 and p2 is less than 0.1
+            if np.linalg.norm((p1-p2)) < 0.1:
+                # Keep idxs
+                idx1.append(m.queryIdx)
+                idx2.append(m.trainIdx)
+                ret.append((p1, p2))
+                pass
+
+
+    assert len(ret) >= 8
+    ret = np.array(ret)
+    idx1 = np.array(idx1)
+    idx2 = np.array(idx2)
+
+    # Fit matrix
+    model, inliers = ransac((ret[:, 0], 
+                            ret[:, 1]), FundamentalMatrixTransform, 
+                            min_samples=8, residual_threshold=0.005, 
+                            max_trials=200)
+    
+    # Ignore outliers
+    ret = ret[inliers]
+    Rt = extractPose(model.params)
+
+    return idx1[inliers], idx2[inliers], Rt
+
+
+class Frame(object):
+    def __init__(self, mapp, img, K):
+        self.K = K
+        self.Kinv = np.linalg.inv(self.K)
+        self.pose = IRt
+
+        self.id = len(mapp.frames)
+        mapp.frames.append(self)
+
+        pts, self.des = extract(img)
+        
+        if self.des.any()!=None:
+            self.pts = normalize(self.Kinv, pts)