debug

main.py

@@ -27,15 +27,12 @@ def main(pathOdom_,pathImage_,pathWeight_):
     # parameter
     resultsId = 1
     iSamUpdateRate = 1 # times of updates in one iteration
-    startId = 0 # start image id
-    endId = 1e3 # end image id
-    numInterval = 1
-    iterations = int((endId-startId)/numInterval)
     timestamp = 0
 
     # read data
-    odom = odometry(pathOdom,startId,endId)
     images = readImage(pathImage)
+    startId = images.getStartId()
+    odom = odometry(pathOdom,startId)
     # TODO: read the ground truth for plot comparision
 
     # sensor info
@@ -51,11 +48,13 @@ def main(pathOdom_,pathImage_,pathWeight_):
     iSam.initialize(priorMu,priorCov) # adding prior
     img,imgTimestamp,currGT = images.getImage(startId)
     measurement = poseNet.test(images.getImage(startId))
-    startId += numInterval
+    startId += 1
     iSam.addObs(measurement,poseNetCov) # adding first measurement
     currEstimate = iSam.update(2) # update the graph
     records.append(currEstimate)
     groundTruth.append(currGT)
+
+    iterations = images.length()
 
     # localization begins here
     for i in range(iterations):
@@ -78,7 +77,7 @@ def main(pathOdom_,pathImage_,pathWeight_):
         records.append(currentEst)
 
         # increment the Id
-        startId += numInterval
+        startId += 1
 
         # plot tool for the calculated trajetory
         ax.plot(records[-2:,1],records[-2:,2],c='g') # plot line from x_t-1 to x_t
@@ -90,7 +89,9 @@ def main(pathOdom_,pathImage_,pathWeight_):
     pickleOut = open('dict.{0}_result'.format(resultsId),'wb')
     pickle.dump(records,pickleOut)
     pickleOut.close()
-
+
+    # save fig
+    fig.save("Trajectory{0}".format(resultsId))
 
 if __name__=='__main__':
 

readImage.py

@@ -24,8 +24,14 @@ def getImage(self,imageId,isPlot=False):
         if isPlot:
             cv2.imshow('image{0}'.format(imageId),image)
 
-        key = self.timestamp[self.currKey]
+        timestamp = self.timestamp[self.currKey]
         groundTruth = self.groundTruth[self.currKey,:]
         self.currKey += 1
 
-        return image, key, [groundTruth[0],groundTruth[1],groundTruth[5]]
+        return image, timestamp, [groundTruth[0],groundTruth[1],groundTruth[5]]
+
+    def getStartId(self):
+        return self.timestamp[0]
+
+    def length(self):
+        return len(self.timestamp)

readOdometry.py

@@ -6,7 +6,10 @@ class odometry(object):
     '''
     odometry is a class that read and distribute odometries
     '''
-    def __init__(self, filepath, startId=None, endId=None):
+    def __init__(self, filepath, startId=None):
+        ''' if startId is None read the whole dataset
+            if startId is not None find the closest timestamp within matchEps us
+        '''
 
         mu_path = os.path.join(filepath,'odometry_mu.csv')
         cov_path = os.path.join(filepath,'odometry_cov.csv')
@@ -16,9 +19,17 @@ def __init__(self, filepath, startId=None, endId=None):
         # divide by 10 is a compromise for image data which only has 15 digits
         self.timestamps = odom[:,0]/10
 
+        matchEps = 1e6
+
         if startId == None:
             startId = 0
             endId = len(odom)
+        else:
+            for i in range(len(self.timestamps)):
+                if(startId-self.timestamps[i]<=matchEps):
+                    startId = i
+                    self.timestamps = self.timestamps[i:]
+                    break
 
         # reading odometry mu (delta_x,delta_y,delta_theta)
         x = odom[startId:endId, 1]
@@ -39,9 +50,6 @@ def getOdometry(self,readingId = None):
         if readingId == None:
             readingId = self.readingId
 
-        ## accumulate the odometry if reading is not continuous
-        ## if the file is set to correspond with the image file then ommit this comment
-
         self.readingId += 1
 
         return list(self.odom[readingId,:]),list(self.odomCov[readingId,:]),self.timestamps[readingId]