Merge branch 'master' into debugging

README.md

@@ -7,6 +7,7 @@ Application of PoseNet and dynamic structural data generation for real-time loca
 3. OpenCV 3
 4. TensorFlow
 5. MatPlotlib/ NumPy/ urllib2
+6. [GTSAM 4.0](https://bitbucket.org/gtborg/gtsam/) with Python 2.7 enabled
 
 ## Procedure
 1. Install IP WebCam on your phone

gtsamSolver.py

@@ -4,8 +4,8 @@
 
 
 # a hash-like function
-X = lambda i: int(gtsam.symbol(ord('x'), i))
 
+X = lambda i: int(gtsam.symbol(ord('x'), i))
 
 class PoseNetiSam(object):
     """A solver for posenet specified SLAM
@@ -22,6 +22,7 @@ def __init__(self, relinearizeThreshold=0.01, relinearizeSkip=1):
         parameters = gtsam.ISAM2Params()
         parameters.setRelinearizeThreshold(relinearizeThreshold)
         parameters.setRelinearizeSkip(relinearizeSkip)
+
         self.isam = gtsam.ISAM2(parameters)
 
         # init container for initial values
@@ -32,51 +33,61 @@ def __init__(self, relinearizeThreshold=0.01, relinearizeSkip=1):
 
         # current estimate
         self.currentEst = False
+
         self.currentPose = [0,0,0]
 
         return
 
     def _motion_model(self, odometry):
+
         currPos = self.currentPose
+
         predPos = [currPos[0]+odometry[0], currPos[1]+odometry[1], currPos[2]+odometry[2]]
         return predPos
 
     def initialize(self, priorMean=[0,0,0], priorCov=[0,0,0]):
         # init the prior
         priorMean = gtsam.Pose2(priorMean[0], priorMean[1], priorMean[2])
+
         priorCov = gtsam.noiseModel_Diagonal.Sigmas(np.array(priorCov))
+
         self.graph.add(gtsam.PriorFactorPose2(X(self.currentKey), priorMean, priorCov))
         self.initialValues.insert(X(self.currentKey), priorMean)
 
         return
 
     def step(self, odometry, odometryNoise):
         odometryGT = gtsam.Pose2(odometry[0],odometry[1],odometry[2])
+
         odometryNoise = gtsam.noiseModel_Diagonal.Variances(np.array(odometryNoise))
+
         self.graph.add(gtsam.BetweenFactorPose2(X(self.currentKey), X(self.currentKey+1),
                                                 odometryGT, odometryNoise))
 
         # adding the initialValues
-        # TODO: when step function is called keep track of the current value instead          of calling the current position in motion model.
+
         predMean = self._motion_model(odometry)
         initialVal = gtsam.Pose2(predMean[0],predMean[1],predMean[2])
         self.initialValues.insert(X(self.currentKey+1), initialVal)
 
         # increment the key
         self.currentKey += 1
+
         # update current pose if adding odometry
         self.currentPose = predMean
-
         return
 
     def addObs(self, measurement, measurementNoise):
+
         measurement = gtsam.Pose2(float(measurement[0]),float(measurement[1]),float(measurement[2]))
         measurementNoise = gtsam.noiseModel_Diagonal.Variances(np.array(measurementNoise))
+
         self.graph.add(gtsam.PriorFactorPose2(X(self.currentKey), measurement, measurementNoise))
 
         return
 
     def update(self, updateNum = 1):
+
         self.isam.update(self.graph, self.initialValues)
         updateNum -= 1
 
@@ -87,6 +98,7 @@ def update(self, updateNum = 1):
         # clear graph and initial values
         self.graph.resize(0)
         self.initialValues.clear()
+
         self.currentEst = self.isam.calculateEstimate()
         # print(self.currentKey)
         # self.currentEst.print("\nEstimate:\n")
@@ -124,3 +136,4 @@ def printResult(self, output = "\nEstimation for vertices:\n"):
         '''
         print(self.currentEst)
         return
+

gtsamTest.py

@@ -0,0 +1,38 @@
+from __future__ import print_function
+import numpy as np
+import matplotlib.pyplot as plt
+import gtsamSolver
+
+
+def main():
+    # define some prior
+    delta = 0.1
+    priorMean = [delta, -delta, delta/10]
+    priorCov = [delta/10, -delta/10, delta]
+
+    # define odometry
+    odometrys = [[5,0,0],[5,0,0],[5,0,-np.pi/2],[5,0,-np.pi/2],[5,0,-np.pi/2]]
+    odoNoise = [0.5,0.5,1]
+
+    # define measurements
+    measurements = [[0,0,0],[5+2*delta,0+delta,0+delta/10],[10-4*delta,0-delta,-np.pi/2+delta/5],
+                    [10+3*delta,-5+2*delta, -np.pi+delta],[5-delta,-5+delta,np.pi-delta/5],
+                    [5-2*delta,0+3*delta,0-delta]]
+    measureNoise = [0.1,0.1,0.1]
+
+    solver = gtsamSolver.PoseNetiSam()
+    solver.initialize(priorMean,priorCov)
+    solver.addObs(measurements[0],measureNoise)
+    solver.update()
+    iterations = 5
+
+    # Slam starts
+    for i in range(iterations):
+        solver.step(odometrys[i],odoNoise)
+        solver.addObs(measurements[i+1], measureNoise)
+        solver.update()
+
+    solver.printResult()
+
+if __name__ == '__main__':
+    main()

readOdomTest.py

@@ -0,0 +1,11 @@
+from readOdometry import odometry
+
+# change this to the odometry folder
+filepath = 'path/to/odometry'
+
+# load odometry and print the first item
+odom = odometry(filepath)
+odom.printOdometry()
+
+# get the next odometry and increment the timestamp
+print odom.getOdometry()

readOdometry.py

@@ -6,11 +6,11 @@ class odometry(object):
     '''
     odometry is a class that read and distribute odometries
     '''
+
     def __init__(self, filepath, startId=None, endId=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')
 
@@ -43,6 +43,7 @@ def __init__(self, filepath, startId=None, endId=None):
         # reading the covariance matrix
         odomCov = np.loadtxt(cov_path, delimiter=",")
 
+
         self.odomCov = np.transpose(np.array([odomCov[startId:endId,1], odomCov[startId:endId,7], odomCov[startId:endId,21]]))
 
         # current reading index
@@ -61,4 +62,6 @@ def printOdometry(self,printingId = None):
         if printingId == None:
             printingId = self.readingId+1
 
+
         print("The next odometry is:",list(self.odom[printingId,:]),"\nThe Covariance is",list(self.odomCov[printingId,:]))
+

test.py

@@ -0,0 +1,38 @@
+from __future__ import print_function
+import numpy as np
+import matplotlib.pyplot as plt
+import gtsamSolver
+
+
+def main():
+    # define some prior
+    delta = 0.1
+    priorMean = [delta, -delta, delta/10]
+    priorCov = [delta/10, -delta/10, delta]
+
+    # define odometry
+    odometrys = [[5,0,0],[5,0,0],[5,0,-np.pi/2],[5,0,-np.pi/2],[5,0,-np.pi/2]]
+    odoNoise = [0.5,0.5,1]
+
+    # define measurements
+    measurements = [[0,0,0],[5+2*delta,0+delta,0+delta/10],[10-4*delta,0-delta,-np.pi/2+delta/5],
+                    [10+3*delta,-5+2*delta, -np.pi+delta],[5-delta,-5+delta,np.pi-delta/5],
+                    [5-2*delta,0+3*delta,0-delta]]
+    measureNoise = [0.1,0.1,0.1]
+
+    solver = gtsamSolver.PoseNetiSam()
+    solver.initialize(priorMean,priorCov)
+    solver.addObs(measurements[0],measureNoise)
+    solver.update()
+    iterations = 5
+
+    # Slam starts
+    for i in range(iterations):
+        solver.step(odometrys[i],odoNoise)
+        solver.addObs(measurements[i+1], measureNoise)
+        solver.update()
+
+    solver.printResult()
+
+if __name__ == '__main__':
+    main()