update real-time plot

main.py

@@ -2,6 +2,7 @@
 from readImage import readImage
 from gtsamSolver import PoseNetiSam
 from train import trainer
+import matplotlib.pyplot as plt
 import pickle
 import sys
 
@@ -19,6 +20,10 @@ def main(pathOdom_,pathImage_,pathWeight_):
     # initilize posenet
     poseNet = trainer(pathWeight,pathImage)
 
+    # initialize the plot tool
+    plt.ion()
+    fig,ax = plt.subplots()
+
     # parameter
     resultsId = 1
     iSamUpdateRate = 2 # times of updates in one iteration
@@ -30,7 +35,7 @@ def main(pathOdom_,pathImage_,pathWeight_):
     # read data
     odom = odometry(pathOdom,startId,endId)
     images = readImage(pathImage)
-    #TODO: read the ground truth for plot comparision
+    # TODO: read the ground truth for plot comparision
 
     # sensor info
     poseNetCov = [0.5,0.5,0.5]
@@ -46,7 +51,7 @@ def main(pathOdom_,pathImage_,pathWeight_):
     startId += numInterval
     iSam.addObs(measurement,poseNetCov) # adding first measurement
     iSam.update(2) # update the graph
-    
+
     # localization begins here
     for i in range(iterations):
 
@@ -66,8 +71,10 @@ def main(pathOdom_,pathImage_,pathWeight_):
         # increment the Id
         startId += numInterval
 
-        # TODO: plot tool for the calculated trajetory
-
+        # plot tool for the calculated trajetory
+        ax.plot(records[-2:,1],records[-2:,2]) # plot line from x_t-1 to x_t
+        plt.pause(0.01) # whether need to pause?
+        plt.draw()
 
     # store data
     pickleOut = open('dict.{0}_result'.format(resultsId),'wb')