Merge pull request #11 from Samleo8/plot-map-points

Plot map points

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()

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)