diff --git a/Mapping.py b/Mapping.py
index 42b6c16..26ca72e 100644
--- a/Mapping.py
+++ b/Mapping.py
@@ -93,7 +93,7 @@ def convertFeaturesLocalToGlobal(
         featurePointsGlobal = (R @ (featurePointsGlobal.T) + t).T
 
         return featurePointsGlobal
-    
+
     def getPrunedFeaturesGlobalPosition(self):
         x, y, th = self.pose
 
@@ -221,11 +221,17 @@ def plot(self, fig: plt.figure, show: bool = False):
         @param[in] fig plt figure to plot on @todo do this
         '''
         # TODO: For now, plot all the keyframe feature points
+        points_global = np.empty((0, 2))
         for kf in self.keyframes:
-            points_local = kf.featurePointsLocal
-            points_global = kf.convertFeaturesLocalToGlobal(points_local)
-            
-            plt.scatter(points_global[:, 0], points_global[:, 1], marker='+', color='red',label='Map Points')
+            points_global = np.vstack((points_global,kf.getPrunedFeaturesGlobalPosition()))
+
+        subsampleFactor = 5
+        plt.scatter(points_global[::subsampleFactor, 0],
+                    points_global[::subsampleFactor, 1],
+                    marker='+',
+                    color='g',
+                    alpha=.8,
+                    label='Map Points')
 
         if show:
             plt.show()
diff --git a/RawROAMSystem.py b/RawROAMSystem.py
index 44c3097..ebba43c 100644
--- a/RawROAMSystem.py
+++ b/RawROAMSystem.py
@@ -250,7 +250,7 @@ def run(self, startSeqInd: int = 0, endSeqInd: int = -1) -> None:
                 print("\nAdding keyframe...\n")
 
                 #old_kf.copyFromOtherKeyframe(possible_kf)
-                #self.map.addKeyframe(possible_kf)
+                self.map.addKeyframe(possible_kf)
 
                 # TODO: Aliasing above? old_kf is never assigned the object possible_kf,
                 # map ends up with a list of N pointers to the same keyframe
@@ -271,8 +271,9 @@ def run(self, startSeqInd: int = 0, endSeqInd: int = -1) -> None:
                 self.plot(prevImgCart, currImgCart, good_old, good_new, R, h,
                         seqInd, save = True, show = False)
             else:
-                self.plot(prevImgCart, currImgCart, good_old, good_new, R, h,
-                        seqInd, save = False, show = False)
+                if seqInd % 3 == 0:
+                    self.plot(prevImgCart, currImgCart, good_old, good_new, R, h,
+                            seqInd, save = False, show = False)
             # Update incremental variables
             blobCoord = good_new.copy()
             prevImgCart = currImgCart
@@ -319,7 +320,7 @@ def plot(self,
 
         ax2 = self.fig.add_subplot(1, 2, 2)
         # TODO: Plotting for map points
-        #self.map.plot(self.fig, show=False)
+        self.map.plot(self.fig, show=False)
 
         self.plotTraj(seqInd, R, h, save=False, show=False)
 
diff --git a/__init__.py b/__init__.py
deleted file mode 100644
index e69de29..0000000
diff --git a/img/roam_mapping/tiny_traj/0001.jpg b/img/roam_mapping/tiny_traj/0001.jpg
index b2a5ce8..5a9255a 100644
Binary files a/img/roam_mapping/tiny_traj/0001.jpg and b/img/roam_mapping/tiny_traj/0001.jpg differ
diff --git a/img/roam_mapping/tiny_traj/0002.jpg b/img/roam_mapping/tiny_traj/0002.jpg
index 37676ec..cc61d1b 100644
Binary files a/img/roam_mapping/tiny_traj/0002.jpg and b/img/roam_mapping/tiny_traj/0002.jpg differ
diff --git a/img/roam_mapping/tiny_traj/0003.jpg b/img/roam_mapping/tiny_traj/0003.jpg
index 2237332..42fb687 100644
Binary files a/img/roam_mapping/tiny_traj/0003.jpg and b/img/roam_mapping/tiny_traj/0003.jpg differ
diff --git a/img/roam_mapping/tiny_traj/0004.jpg b/img/roam_mapping/tiny_traj/0004.jpg
index dbb22df..090e4f2 100644
Binary files a/img/roam_mapping/tiny_traj/0004.jpg and b/img/roam_mapping/tiny_traj/0004.jpg differ
diff --git a/img/roam_mapping/tiny_traj/0005.jpg b/img/roam_mapping/tiny_traj/0005.jpg
index ffeffc0..b04ef42 100644
Binary files a/img/roam_mapping/tiny_traj/0005.jpg and b/img/roam_mapping/tiny_traj/0005.jpg differ
diff --git a/img/roam_mapping/tiny_traj/0006.jpg b/img/roam_mapping/tiny_traj/0006.jpg
index 3a758e6..dee9f5a 100644
Binary files a/img/roam_mapping/tiny_traj/0006.jpg and b/img/roam_mapping/tiny_traj/0006.jpg differ
diff --git a/img/roam_mapping/tiny_traj/0007.jpg b/img/roam_mapping/tiny_traj/0007.jpg
index 47b0af0..a217445 100644
Binary files a/img/roam_mapping/tiny_traj/0007.jpg and b/img/roam_mapping/tiny_traj/0007.jpg differ
diff --git a/img/roam_mapping/tiny_traj/0008.jpg b/img/roam_mapping/tiny_traj/0008.jpg
index 99e54b8..20dd429 100644
Binary files a/img/roam_mapping/tiny_traj/0008.jpg and b/img/roam_mapping/tiny_traj/0008.jpg differ
diff --git a/img/roam_mapping/tiny_traj/0009.jpg b/img/roam_mapping/tiny_traj/0009.jpg
index ff59f5f..cf7c6b7 100644
Binary files a/img/roam_mapping/tiny_traj/0009.jpg and b/img/roam_mapping/tiny_traj/0009.jpg differ
diff --git a/img/roam_mapping/tiny_traj/0010.jpg b/img/roam_mapping/tiny_traj/0010.jpg
index 1974303..ef09dea 100644
Binary files a/img/roam_mapping/tiny_traj/0010.jpg and b/img/roam_mapping/tiny_traj/0010.jpg differ
diff --git a/RadarSLAMPaper.pdf b/ref/RadarSLAMPaper.pdf
similarity index 100%
rename from RadarSLAMPaper.pdf
rename to ref/RadarSLAMPaper.pdf
diff --git a/RadarSLAMPaper2021.pdf b/ref/RadarSLAMPaper2021.pdf
similarity index 100%
rename from RadarSLAMPaper2021.pdf
rename to ref/RadarSLAMPaper2021.pdf