Update README and docs

Author: mtszkw

README.md

@@ -1,12 +1,17 @@
 ## Monocular Visual Odometry
 
-### Pipeline
-To be described
+**DatasetReaderKITTI** is responsible for loading frames from [KITTI Visual Odometry Dataset](http://www.cvlibs.net/datasets/kitti/eval_odometry.php) (optionally scaling them to reduce processing time) and ground truth (camera matrix, camera position and scale).
 
-#### Dataset
-- [KITTI Visual Odometry Dataset](http://www.cvlibs.net/datasets/kitti/eval_odometry.php)
+In a processing loop I convert images to greyscale, run keypoint detection using GFTT and then track these keypoints with **FeatureTracker** that uses OpenCV optical flow. After finding feature tracks, I remove outliers (outside of image).
+
+In the next step I estimate the essential matrix *E* to find relative pose *R*, *t* between consecutive frames. Calculating *E* matrix also allows to remove few outliers (found by RANSAC). Having that rotation and translation, I calculate absolute position and orientation of the camera and I use them to draw a trajectory.
+
+To get better results, I rely on absolute scale provided as KITTI groundtruth when computing abs. pose.
 
 #### Screenshots
+
 <p align="center"> <img src="doc/tracking_1.png" alt="Tracking 1"/> </p>
+
 <p align="center"> <img src="doc/tracking_2.png" alt="Tracking 2"/> </p>
+
 <p align="center"> <img src="doc/trajectory.png" alt="Trajectory"/> </p>

slam/FeatureTracker.py renamed to source/feature_tracking.py

@@ -1,12 +1,18 @@
 import cv2
 import numpy as np
 
-class FeatureTracker:        
-    # Finds indices of bad features (with status=0 or position outside the frame).
-    # @param features Detected (or tracked) features.
-    # @param frame Image matrix.
-    # @param status Vector of status flags returned by optical flow.
-    # @returns Vector containing indices that should be filtered out.
+class FeatureTracker:
+    """
+    Finds indices of bad features (with status=0 or position outside the frame).
+    
+    Args:
+        features: Detected (or tracked) features.
+        frame: Image matrix.
+        status: Vector of status flags returned by optical flow.
+    
+    Returns:
+        Vector containing indices that should be filtered out.
+    """        
     def calcWrongFeatureIndices(self, features, frame, status):
         status_ = status.copy()
         for idx, pt in enumerate(features):
@@ -15,12 +21,18 @@ def calcWrongFeatureIndices(self, features, frame, status):
         wrongIndices = np.where(status_ == 0)[0]
         return wrongIndices
 
-    # Tracks features using Lucas-Kanade optical flow and filters out bad features.
-    # @param prevFrame Previous image.
-    # @param currFrame Current (next) image.
-    # @param prevPts Features detected on previous frame.
-    # @param removeOutliers Set to true if you want to remove bad features after tracking.
-    # @returns Features from previous and current frame (tracked), both filtered.
+    """
+    Tracks features using Lucas-Kanade optical flow and filters out bad features.
+    
+    Args:
+        prevFrame: Previous image.
+        currFrame: Current (next) image.
+        prevPts: Features detected on previous frame.
+        removeOutliers: Set to true if you want to remove bad features after tracking.
+    
+    Returns:
+        Features from previous and current frame (tracked), both filtered.
+    """
     def trackFeatures(self, prevFrame, currFrame, prevPts, removeOutliers=False):
         # Feature tracking on the 2nd frame
         currPts, status, _ = cv2.calcOpticalFlowPyrLK(prevFrame, currFrame, prevPts, None)

slam/DatasetReaderKITTI.py renamed to source/kitti_reader.py

@@ -36,7 +36,6 @@ def readCameraMatrix(self):
             K[1, 1] = focal
             K[1, 2] = cy
             K[2, 2] = 1
-            # print("Constructed camera matrix {}:\n{}".format(K.shape, K))
             return K
 
     def readGroundtuthPosition(self, frameId):

slam/slam.py renamed to source/run.py

@@ -3,18 +3,9 @@
 import numpy as np
 import matplotlib.pyplot as plt
 
-from DatasetReaderKITTI import DatasetReaderKITTI
-from FeatureTracker import FeatureTracker
-from utils import drawFrameFeatures, updateTrajectoryDrawing
-
-def savePly(points, colors, output_file):
-    vertexes = [ (p[0], p[1], p[2], c[0], c[1], c[2]) for p, c in zip(points, colors)]
-    vertexes = [ v for v in vertexes if v[2] >= 0 ] # Discard negative z
-    dtypes = [('x', 'f8'), ('y', 'f8'), ('z', 'f8'), ('red', 'u1'), ('green', 'u1'), ('blue', 'u1')]
-    array = np.array(vertexes, dtype=dtypes)
-    element = plyfile.PlyElement.describe(array, "vertex")
-    plyfile.PlyData([element]).write(output_file)
-
+from kitti_reader import DatasetReaderKITTI
+from feature_tracking import FeatureTracker
+from utils import drawFrameFeatures, updateTrajectoryDrawing, savePly
 
 if __name__ == "__main__":
     tracker = FeatureTracker()

slam/utils.py renamed to source/utils.py

@@ -24,4 +24,12 @@ def updateTrajectoryDrawing(trackedPoints, groundtruthPoints):
     plt.title("Trajectory")
     plt.legend()
     plt.draw()
-    plt.pause(0.01)
+    plt.pause(0.01)
+
+def savePly(points, colors, output_file):
+    vertexes = [ (p[0], p[1], p[2], c[0], c[1], c[2]) for p, c in zip(points, colors)]
+    vertexes = [ v for v in vertexes if v[2] >= 0 ] # Discard negative z
+    dtypes = [('x', 'f8'), ('y', 'f8'), ('z', 'f8'), ('red', 'u1'), ('green', 'u1'), ('blue', 'u1')]
+    array = np.array(vertexes, dtype=dtypes)
+    element = plyfile.PlyElement.describe(array, "vertex")
+    plyfile.PlyData([element]).write(output_file)