diff --git a/cython/gtsam_utils/__init__.py b/cython/gtsam_utils/__init__.py new file mode 100644 index 000000000..3e9ea71a6 --- /dev/null +++ b/cython/gtsam_utils/__init__.py @@ -0,0 +1 @@ +from .np_utils import * \ No newline at end of file diff --git a/cython/gtsam_utils/circlePose3.py b/cython/gtsam_utils/circlePose3.py new file mode 100644 index 000000000..3b1a8828b --- /dev/null +++ b/cython/gtsam_utils/circlePose3.py @@ -0,0 +1,34 @@ +from gtsam import * +from math import * +from np_utils import * + +def circlePose3(numPoses = 8, radius = 1.0, symbolChar = 0): + """ + circlePose3 generates a set of poses in a circle. This function + returns those poses inside a gtsam.Values object, with sequential + keys starting from 0. An optional character may be provided, which + will be stored in the msb of each key (i.e. gtsam.Symbol). + + We use aerospace/navlab convention, X forward, Y right, Z down + First pose will be at (R,0,0) + ^y ^ X + | | + z-->xZ--> Y (z pointing towards viewer, Z pointing away from viewer) + Vehicle at p0 is looking towards y axis (X-axis points towards world y) + """ + + # Force symbolChar to be a single character + if type(symbolChar) is str: symbolChar = ord(symbolChar[0]) + + values = gtsam.Values() + theta = 0.0 + dtheta = 2*pi/numPoses + gRo = gtsam.Rot3(R = Matrix([[0., 1., 0.], [1., 0., 0.], [0., 0., -1.]])) + for i in range(numPoses): + key = gtsam.symbol(symbolChar, i) + gti = gtsam.Point3(radius*cos(theta), radius*sin(theta), 0) + oRi = gtsam.Rot3.Yaw(-theta) # negative yaw goes counterclockwise, with Z down ! + gTi = gtsam.Pose3(gRo.compose(oRi), gti) + values.insertPose3(key, gTi) + theta = theta + dtheta + return values \ No newline at end of file diff --git a/cython/gtsam_utils/np_utils.py b/cython/gtsam_utils/np_utils.py new file mode 100644 index 000000000..a6c970685 --- /dev/null +++ b/cython/gtsam_utils/np_utils.py @@ -0,0 +1,4 @@ +import numpy as np + +def Vector(list1d): return np.array(list1d, dtype = 'float') +def Matrix(list2d): return np.array(list2d, dtype = 'float', order = 'F') \ No newline at end of file diff --git a/cython/gtsam_utils/visual_data_generator.py b/cython/gtsam_utils/visual_data_generator.py new file mode 100644 index 000000000..180f74b47 --- /dev/null +++ b/cython/gtsam_utils/visual_data_generator.py @@ -0,0 +1,118 @@ +from np_utils import * +from math import * +from gtsam import * + + +class Options: + """ + Options to generate test scenario + """ + + def __init__(self, triangle=False, nrCameras=3, K=Cal3_S2()): + """ + Options to generate test scenario + @param triangle: generate a triangle scene with 3 points if True, otherwise + a cube with 8 points + @param nrCameras: number of cameras to generate + @param K: camera calibration object + """ + self.triangle = triangle + self.nrCameras = nrCameras + + +class GroundTruth: + """ + Object holding generated ground-truth data + """ + + def __init__(self, K=Cal3_S2(), nrCameras=3, nrPoints=4): + self.K = K + self.cameras = [Pose3()] * nrCameras + self.points = [Point3()] * nrPoints + + def print_(self, s=""): + print s + print "K = ", self.K + print "Cameras: ", len(self.cameras) + for camera in self.cameras: + print "\t", camera + print "Points: ", len(self.points) + for point in self.points: + print "\t", point + pass + + +class Data: + """ + Object holding generated measurement data + """ + + class NoiseModels: + pass + + def __init__(self, K=Cal3_S2(), nrCameras=3, nrPoints=4): + self.K = K + self.Z = [x[:] for x in [[Point2()] * nrPoints] * nrCameras] + self.J = [x[:] for x in [[0] * nrPoints] * nrCameras] + self.odometry = [Pose3()] * nrCameras + + # Set Noise parameters + self.noiseModels = Data.NoiseModels() + self.noiseModels.posePrior = noiseModel_Diagonal.Sigmas( + Vector([0.001, 0.001, 0.001, 0.1, 0.1, 0.1])) + # noiseModels.odometry = noiseModel_Diagonal.Sigmas( + # Vector([0.001,0.001,0.001,0.1,0.1,0.1])) + self.noiseModels.odometry = noiseModel_Diagonal.Sigmas( + Vector([0.05, 0.05, 0.05, 0.2, 0.2, 0.2])) + self.noiseModels.pointPrior = noiseModel_Isotropic.Sigma(3, 0.1) + self.noiseModels.measurement = noiseModel_Isotropic.Sigma(2, 1.0) + + +def generate_data(options): + """ + Generate ground-truth and measurement data + """ + + K = Cal3_S2(500, 500, 0, 640. / 2., 480. / 2.) + nrPoints = 3 if options.triangle else 8 + + truth = GroundTruth(K=K, nrCameras=options.nrCameras, nrPoints=nrPoints) + data = Data(K, nrCameras=options.nrCameras, nrPoints=nrPoints) + + # Generate simulated data + if options.triangle: # Create a triangle target, just 3 points on a plane + r = 10 + for j in range(len(truth.points)): + theta = j * 2 * pi / nrPoints + truth.points[j] = Point3( + v=Vector([r * cos(theta), r * sin(theta), 0])) + else: # 3D landmarks as vertices of a cube + truth.points = [Point3(v=Vector([10, 10, 10])), + Point3(v=Vector([-10, 10, 10])), + Point3(v=Vector([-10, -10, 10])), + Point3(v=Vector([10, -10, 10])), + Point3(v=Vector([10, 10, -10])), + Point3(v=Vector([-10, 10, -10])), + Point3(v=Vector([-10, -10, -10])), + Point3(v=Vector([10, -10, -10]))] + + # Create camera cameras on a circle around the triangle + height = 10 + r = 40 + for i in range(options.nrCameras): + theta = i * 2 * pi / options.nrCameras + t = Point3(v=Vector([r * cos(theta), r * sin(theta), height])) + truth.cameras[i] = SimpleCamera.Lookat( + t, Point3(), Point3(v=Vector([0, 0, 1])), truth.K) + # Create measurements + for j in range(nrPoints): + # All landmarks seen in every frame + data.Z[i][j] = truth.cameras[i].project(truth.points[j]) + data.J[i][j] = j + + # Calculate odometry between cameras + for i in range(1, options.nrCameras): + data.odometry[i] = truth.cameras[ + i - 1].pose().between(truth.cameras[i].pose()) + + return data, truth diff --git a/cython/gtsam_utils/visual_isam.py b/cython/gtsam_utils/visual_isam.py new file mode 100644 index 000000000..222a468a9 --- /dev/null +++ b/cython/gtsam_utils/visual_isam.py @@ -0,0 +1,125 @@ +from np_utils import * +from math import * +from gtsam import * + + +class Options: + """ + Options for visual isam example + """ + def __init__(self): + self.hardConstraint = False + self.pointPriors = False + self.batchInitialization = True + self.reorderInterval = 10 + self.alwaysRelinearize = False + + +def initialize(data, truth, options): + # Initialize iSAM + params = gtsam.ISAM2Params() + if options.alwaysRelinearize: + params.setRelinearizeSkip(1) + isam = ISAM2(params = params) + + # Add constraints/priors + # TODO: should not be from ground truth! + newFactors = NonlinearFactorGraph() + initialEstimates = Values() + for i in range(2): + ii = symbol(ord('x'), i) + if i == 0: + if options.hardConstraint: # add hard constraint + newFactors.add(NonlinearEqualityPose3( + ii, truth.cameras[0].pose())) + else: + newFactors.add(PriorFactorPose3( + ii, truth.cameras[i].pose(), data.noiseModels.posePrior)) + initialEstimates.insertPose3(ii, truth.cameras[i].pose()) + + nextPoseIndex = 2 + + # Add visual measurement factors from two first poses and initialize + # observed landmarks + for i in range(2): + ii = symbol(ord('x'), i) + for k in range(len(data.Z[i])): + j = data.J[i][k] + jj = symbol(ord('l'), j) + newFactors.add(GenericProjectionFactorCal3_S2( + data.Z[i][k], data.noiseModels.measurement, ii, jj, data.K)) + # TODO: initial estimates should not be from ground truth! + if not initialEstimates.exists(jj): + initialEstimates.insertPoint3(jj, truth.points[j]) + if options.pointPriors: # add point priors + newFactors.add(PriorFactorPoint3( + jj, truth.points[j], data.noiseModels.pointPrior)) + + # Add odometry between frames 0 and 1 + newFactors.add(BetweenFactorPose3(symbol(ord('x'), 0), symbol( + ord('x'), 1), data.odometry[1], data.noiseModels.odometry)) + + # Update ISAM + if options.batchInitialization: # Do a full optimize for first two poses + batchOptimizer = LevenbergMarquardtOptimizer( + newFactors, initialEstimates) + fullyOptimized = batchOptimizer.optimize() + isam.update(newFactors, fullyOptimized) + else: + isam.update(newFactors, initialEstimates) + + # figure(1)tic + # t=toc plot(frame_i,t,'r.') tic + result = isam.calculateEstimate() + # t=toc plot(frame_i,t,'g.') + + return isam, result, nextPoseIndex + + +def step(data, isam, result, truth, currPoseIndex): + """ + Do one step isam update + @param[in] data: measurement data (odometry and visual measurements and their noiseModels) + @param[in/out] isam: current isam object, will be updated + @param[in/out] result: current result object, will be updated + @param[in] truth: ground truth data, used to initialize new variables + @param[currPoseIndex]: index of the current pose + """ + # iSAM expects us to give it a new set of factors + # along with initial estimates for any new variables introduced. + newFactors = NonlinearFactorGraph() + initialEstimates = Values() + + # Add odometry + prevPoseIndex = currPoseIndex - 1 + odometry = data.odometry[prevPoseIndex] + newFactors.add(BetweenFactorPose3(symbol(ord('x'), prevPoseIndex), + symbol(ord('x'), currPoseIndex), + odometry, data.noiseModels.odometry)) + + # Add visual measurement factors and initializations as necessary + for k in range(len(data.Z[currPoseIndex])): + zij = data.Z[currPoseIndex][k] + j = data.J[currPoseIndex][k] + jj = symbol(ord('l'), j) + newFactors.add(GenericProjectionFactorCal3_S2( + zij, data.noiseModels.measurement, + symbol(ord('x'), currPoseIndex), jj, data.K)) + # TODO: initialize with something other than truth + if not result.exists(jj) and not initialEstimates.exists(jj): + lmInit = truth.points[j] + initialEstimates.insert(jj, lmInit) + + # Initial estimates for the new pose. + prevPose = result.atPose3(symbol(ord('x'), prevPoseIndex)) + initialEstimates.insertPose3(symbol(ord('x'), currPoseIndex), + prevPose.compose(odometry)) + + # Update ISAM + # figure(1)tic + isam.update(newFactors, initialEstimates) + # t=toc plot(frame_i,t,'r.') tic + newResult = isam.calculateEstimate() + # t=toc plot(frame_i,t,'g.') + + return isam, newResult diff --git a/cython/test/__init__.py b/cython/test/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/cython/test/test_Cal3Unified.py b/cython/test/test_Cal3Unified.py new file mode 100644 index 000000000..b75674e5d --- /dev/null +++ b/cython/test/test_Cal3Unified.py @@ -0,0 +1,15 @@ +import unittest +from gtsam import * +from math import * +import numpy as np + + +class TestCal3Unified(unittest.TestCase): + + def test_Cal3Unified(self): + K = Cal3Unified() + self.assertEqual(K.fx(), 1.) + self.assertEqual(K.fx(), 1.) + +if __name__ == "__main__": + unittest.main() diff --git a/cython/test/test_JacobianFactor.py b/cython/test/test_JacobianFactor.py new file mode 100644 index 000000000..fe8c7cbfa --- /dev/null +++ b/cython/test/test_JacobianFactor.py @@ -0,0 +1,78 @@ +import unittest +from gtsam import * +from math import * +import numpy as np +from gtsam_utils import Matrix, Vector + +class TestJacobianFactor(unittest.TestCase): + + def test_eliminate(self): + Ax2 = Matrix([ + [-5., 0.], + [+0., -5.], + [10., 0.], + [+0., 10.]]) + + Al1 = Matrix([ + [5., 0.], + [0., 5.], + [0., 0.], + [0., 0.]]) + + Ax1 = Matrix([ + [0.00, 0.], # f4 + [0.00, 0.], # f4 + [-10., 0.], # f2 + [0.00, -10.]]) # f2 + + x2 = 1 + l1 = 2 + x1 = 3 + + # the RHS + b2 = Vector([-1., 1.5, 2., -1.]) + sigmas = Vector([1., 1., 1., 1.]) + model4 = noiseModel_Diagonal.Sigmas(sigmas) + combined = JacobianFactor(x2, np.transpose( + Ax2), l1, Al1, x1, Ax1, b2, model4) + + # eliminate the first variable (x2) in the combined factor, destructive + # ! + ord = Ordering() + ord.push_back(x2) + actualCG, lf = combined.eliminate(ord) + + # create expected Conditional Gaussian + R11 = Matrix([[11.1803, 0.00], + [0.00, 11.1803]]) + S12 = Matrix([[-2.23607, 0.00], + [+0.00, -2.23607] ]) + S13 = Matrix([[-8.94427, 0.00], + [+0.00, -8.94427]]) + d = Vector([2.23607, -1.56525]) + expectedCG = GaussianConditional( + x2, d, R11, l1, S12, x1, S13, noiseModel_Unit.Create(2)) + # check if the result matches + self.assertTrue(actualCG.equals(expectedCG, 1e-4)) + + # the expected linear factor + Bl1 = Matrix([ + [4.47214, 0.00], + [0.00, 4.47214]]) + + Bx1 = Matrix([ + # x1 + [-4.47214, 0.00], + [+0.00, -4.47214]]) + + # the RHS + b1 = Vector([0.0, 0.894427]) + + model2 = noiseModel_Diagonal.Sigmas(np.array([1., 1.])) + expectedLF = JacobianFactor(l1, Bl1, x1, Bx1, b1, model2) + + # check if the result matches the combined (reduced) factor + self.assertTrue(lf.equals(expectedLF, 1e-4)) + +if __name__ == "__main__": + unittest.main() diff --git a/cython/test/test_KalmanFilter.py b/cython/test/test_KalmanFilter.py new file mode 100644 index 000000000..1e4dee339 --- /dev/null +++ b/cython/test/test_KalmanFilter.py @@ -0,0 +1,70 @@ +import unittest +from gtsam import * +from math import * +import numpy as np + +class TestKalmanFilter(unittest.TestCase): + + def test_KalmanFilter(self): + F = np.eye(2) + B = np.eye(2) + u = np.array([1.0, 0.0]) + modelQ = noiseModel_Diagonal.Sigmas(np.array([0.1, 0.1])) + Q = 0.01 * np.eye(2) + H = np.eye(2) + z1 = np.array([1.0, 0.0]) + z2 = np.array([2.0, 0.0]) + z3 = np.array([3.0, 0.0]) + modelR = noiseModel_Diagonal.Sigmas(np.array([0.1, 0.1])) + R = 0.01 * np.eye(2) + + # Create the set of expected output TestValues + expected0 = np.array([0.0, 0.0]) + P00 = 0.01 * np.eye(2) + + expected1 = np.array([1.0, 0.0]) + P01 = P00 + Q + I11 = np.linalg.inv(P01) + np.linalg.inv(R) + + expected2 = np.array([2.0, 0.0]) + P12 = np.linalg.inv(I11) + Q + I22 = np.linalg.inv(P12) + np.linalg.inv(R) + + expected3 = np.array([3.0, 0.0]) + P23 = np.linalg.inv(I22) + Q + I33 = np.linalg.inv(P23) + np.linalg.inv(R) + + # Create an KalmanFilter object + KF = KalmanFilter(n=2) + + # Create the Kalman Filter initialization point + x_initial = np.array([0.0, 0.0]) + P_initial = 0.01 * np.eye(2) + + # Create an KF object + state = KF.init(x_initial, P_initial) + self.assertTrue(np.allclose(expected0, state.mean())) + self.assertTrue(np.allclose(P00, state.covariance())) + + # Run iteration 1 + state = KF.predict(state, F, B, u, modelQ) + self.assertTrue(np.allclose(expected1, state.mean().ravel())) + self.assertTrue(np.allclose(P01, state.covariance())) + state = KF.update(state, H, z1, modelR) + self.assertTrue(np.allclose(expected1, state.mean().ravel())) + self.assertTrue(np.allclose(I11, state.information())) + + # Run iteration 2 + state = KF.predict(state, F, B, u, modelQ) + self.assertTrue(np.allclose(expected2, state.mean().ravel())) + state = KF.update(state, H, z2, modelR) + self.assertTrue(np.allclose(expected2, state.mean().ravel())) + + # Run iteration 3 + state = KF.predict(state, F, B, u, modelQ) + self.assertTrue(np.allclose(expected3, state.mean().ravel())) + state = KF.update(state, H, z3, modelR) + self.assertTrue(np.allclose(expected3, state.mean().ravel())) + +if __name__ == "__main__": + unittest.main() diff --git a/cython/test/test_LocalizationExample.py b/cython/test/test_LocalizationExample.py new file mode 100644 index 000000000..61f2ff995 --- /dev/null +++ b/cython/test/test_LocalizationExample.py @@ -0,0 +1,51 @@ +import unittest +from gtsam import * +from math import * +import numpy as np + +class TestLocalizationExample(unittest.TestCase): + + def test_LocalizationExample(self): + # Create the graph (defined in pose2SLAM.h, derived from + # NonlinearFactorGraph) + graph = NonlinearFactorGraph() + + # Add two odometry factors + # create a measurement for both factors (the same in this case) + odometry = Pose2(2.0, 0.0, 0.0) + odometryNoise = noiseModel_Diagonal.Sigmas( + np.array([0.2, 0.2, 0.1])) # 20cm std on x,y, 0.1 rad on theta + graph.add(BetweenFactorPose2(0, 1, odometry, odometryNoise)) + graph.add(BetweenFactorPose2(1, 2, odometry, odometryNoise)) + + # Add three "GPS" measurements + # We use Pose2 Priors here with high variance on theta + groundTruth = Values() + groundTruth.insertPose2(0, Pose2(0.0, 0.0, 0.0)) + groundTruth.insertPose2(1, Pose2(2.0, 0.0, 0.0)) + groundTruth.insertPose2(2, Pose2(4.0, 0.0, 0.0)) + model = noiseModel_Diagonal.Sigmas(np.array([0.1, 0.1, 10.])) + for i in range(3): + graph.add(PriorFactorPose2(i, groundTruth.atPose2(i), model)) + + # Initialize to noisy points + initialEstimate = Values() + initialEstimate.insertPose2(0, Pose2(0.5, 0.0, 0.2)) + initialEstimate.insertPose2(1, Pose2(2.3, 0.1, -0.2)) + initialEstimate.insertPose2(2, Pose2(4.1, 0.1, 0.1)) + + # Optimize using Levenberg-Marquardt optimization with an ordering from + # colamd + optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate) + result = optimizer.optimizeSafely() + + # Plot Covariance Ellipses + marginals = Marginals(graph, result) + P = [None] * result.size() + for i in range(0, result.size()): + pose_i = result.atPose2(i) + self.assertTrue(pose_i.equals(groundTruth.atPose2(i), 1e-4)) + P[i] = marginals.marginalCovariance(i) + +if __name__ == "__main__": + unittest.main() diff --git a/cython/test/test_OdometryExample.py b/cython/test/test_OdometryExample.py new file mode 100644 index 000000000..895866385 --- /dev/null +++ b/cython/test/test_OdometryExample.py @@ -0,0 +1,46 @@ +import unittest +from gtsam import * +from math import * +import numpy as np + +class TestOdometryExample(unittest.TestCase): + + def test_OdometryExample(self): + # Create the graph (defined in pose2SLAM.h, derived from + # NonlinearFactorGraph) + graph = NonlinearFactorGraph() + + # Add a Gaussian prior on pose x_1 + priorMean = Pose2(0.0, 0.0, 0.0) # prior mean is at origin + priorNoise = noiseModel_Diagonal.Sigmas( + np.array([0.3, 0.3, 0.1])) # 30cm std on x,y, 0.1 rad on theta + # add directly to graph + graph.add(PriorFactorPose2(1, priorMean, priorNoise)) + + # Add two odometry factors + # create a measurement for both factors (the same in this case) + odometry = Pose2(2.0, 0.0, 0.0) + odometryNoise = noiseModel_Diagonal.Sigmas( + np.array([0.2, 0.2, 0.1])) # 20cm std on x,y, 0.1 rad on theta + graph.add(BetweenFactorPose2(1, 2, odometry, odometryNoise)) + graph.add(BetweenFactorPose2(2, 3, odometry, odometryNoise)) + + # Initialize to noisy points + initialEstimate = Values() + initialEstimate.insertPose2(1, Pose2(0.5, 0.0, 0.2)) + initialEstimate.insertPose2(2, Pose2(2.3, 0.1, -0.2)) + initialEstimate.insertPose2(3, Pose2(4.1, 0.1, 0.1)) + + # Optimize using Levenberg-Marquardt optimization with an ordering from + # colamd + optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate) + result = optimizer.optimizeSafely() + marginals = Marginals(graph, result) + marginals.marginalCovariance(1) + + # Check first pose equality + pose_1 = result.atPose2(1) + self.assertTrue(pose_1.equals(Pose2(), 1e-4)) + +if __name__ == "__main__": + unittest.main() diff --git a/cython/test/test_PlanarSLAMExample.py b/cython/test/test_PlanarSLAMExample.py new file mode 100644 index 000000000..22c1f19bd --- /dev/null +++ b/cython/test/test_PlanarSLAMExample.py @@ -0,0 +1,64 @@ +import unittest +from gtsam import * +from math import * +import numpy as np + +class TestPose2SLAMExample(unittest.TestCase): + + def test_Pose2SLAMExample(self): + # Assumptions + # - All values are axis aligned + # - Robot poses are facing along the X axis (horizontal, to the right in images) + # - We have full odometry for measurements + # - The robot is on a grid, moving 2 meters each step + + # Create graph container and add factors to it + graph = NonlinearFactorGraph() + + # Add prior + # gaussian for prior + priorMean = Pose2(0.0, 0.0, 0.0) # prior at origin + priorNoise = noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) + # add directly to graph + graph.add(PriorFactorPose2(1, priorMean, priorNoise)) + + # Add odometry + # general noisemodel for odometry + odometryNoise = noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) + graph.add(BetweenFactorPose2( + 1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise)) + graph.add(BetweenFactorPose2( + 2, 3, Pose2(2.0, 0.0, pi / 2), odometryNoise)) + graph.add(BetweenFactorPose2( + 3, 4, Pose2(2.0, 0.0, pi / 2), odometryNoise)) + graph.add(BetweenFactorPose2( + 4, 5, Pose2(2.0, 0.0, pi / 2), odometryNoise)) + + # Add pose constraint + model = noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) + graph.add(BetweenFactorPose2(5, 2, Pose2(2.0, 0.0, pi / 2), model)) + + # Initialize to noisy points + initialEstimate = Values() + initialEstimate.insertPose2(1, Pose2(0.5, 0.0, 0.2)) + initialEstimate.insertPose2(2, Pose2(2.3, 0.1, -0.2)) + initialEstimate.insertPose2(3, Pose2(4.1, 0.1, pi / 2)) + initialEstimate.insertPose2(4, Pose2(4.0, 2.0, pi)) + initialEstimate.insertPose2(5, Pose2(2.1, 2.1, -pi / 2)) + + # Optimize using Levenberg-Marquardt optimization with an ordering from + # colamd + optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate) + result = optimizer.optimizeSafely() + + # Plot Covariance Ellipses + marginals = Marginals(graph, result) + P = marginals.marginalCovariance(1) + + pose_1 = result.atPose2(1) + self.assertTrue(pose_1.equals(Pose2(), 1e-4)) + + + +if __name__ == "__main__": + unittest.main() diff --git a/cython/test/test_Pose2SLAMExample.py b/cython/test/test_Pose2SLAMExample.py new file mode 100644 index 000000000..536e8d782 --- /dev/null +++ b/cython/test/test_Pose2SLAMExample.py @@ -0,0 +1,62 @@ +import unittest +from gtsam import * +from math import * +import numpy as np + +class TestPose2SLAMExample(unittest.TestCase): + + def test_Pose2SLAMExample(self): + # Assumptions + # - All values are axis aligned + # - Robot poses are facing along the X axis (horizontal, to the right in images) + # - We have full odometry for measurements + # - The robot is on a grid, moving 2 meters each step + + # Create graph container and add factors to it + graph = NonlinearFactorGraph() + + # Add prior + # gaussian for prior + priorMean = Pose2(0.0, 0.0, 0.0) # prior at origin + priorNoise = noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) + # add directly to graph + graph.add(PriorFactorPose2(1, priorMean, priorNoise)) + + # Add odometry + # general noisemodel for odometry + odometryNoise = noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) + graph.add(BetweenFactorPose2( + 1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise)) + graph.add(BetweenFactorPose2( + 2, 3, Pose2(2.0, 0.0, pi / 2), odometryNoise)) + graph.add(BetweenFactorPose2( + 3, 4, Pose2(2.0, 0.0, pi / 2), odometryNoise)) + graph.add(BetweenFactorPose2( + 4, 5, Pose2(2.0, 0.0, pi / 2), odometryNoise)) + + # Add pose constraint + model = noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) + graph.add(BetweenFactorPose2(5, 2, Pose2(2.0, 0.0, pi / 2), model)) + + # Initialize to noisy points + initialEstimate = Values() + initialEstimate.insertPose2(1, Pose2(0.5, 0.0, 0.2)) + initialEstimate.insertPose2(2, Pose2(2.3, 0.1, -0.2)) + initialEstimate.insertPose2(3, Pose2(4.1, 0.1, pi / 2)) + initialEstimate.insertPose2(4, Pose2(4.0, 2.0, pi)) + initialEstimate.insertPose2(5, Pose2(2.1, 2.1, -pi / 2)) + + # Optimize using Levenberg-Marquardt optimization with an ordering from + # colamd + optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate) + result = optimizer.optimizeSafely() + + # Plot Covariance Ellipses + marginals = Marginals(graph, result) + P = marginals.marginalCovariance(1) + + pose_1 = result.atPose2(1) + self.assertTrue(pose_1.equals(Pose2(), 1e-4)) + +if __name__ == "__main__": + unittest.main() diff --git a/cython/test/test_Pose3SLAMExample.py b/cython/test/test_Pose3SLAMExample.py new file mode 100644 index 000000000..779b3d68e --- /dev/null +++ b/cython/test/test_Pose3SLAMExample.py @@ -0,0 +1,46 @@ +import unittest +from gtsam import * +from math import * +import numpy as np +from gtsam_utils.circlePose3 import * + +class TestPose3SLAMExample(unittest.TestCase): + + def test_Pose3SLAMExample(self): + # Create a hexagon of poses + hexagon = circlePose3(6, 1.0) + p0 = hexagon.atPose3(0) + p1 = hexagon.atPose3(1) + + # create a Pose graph with one equality constraint and one measurement + fg = NonlinearFactorGraph() + fg.add(NonlinearEqualityPose3(0, p0)) + delta = p0.between(p1) + covariance = noiseModel_Diagonal.Sigmas( + np.array([0.05, 0.05, 0.05, 5. * pi / 180, 5. * pi / 180, 5. * pi / 180])) + fg.add(BetweenFactorPose3(0, 1, delta, covariance)) + fg.add(BetweenFactorPose3(1, 2, delta, covariance)) + fg.add(BetweenFactorPose3(2, 3, delta, covariance)) + fg.add(BetweenFactorPose3(3, 4, delta, covariance)) + fg.add(BetweenFactorPose3(4, 5, delta, covariance)) + fg.add(BetweenFactorPose3(5, 0, delta, covariance)) + + # Create initial config + initial = Values() + s = 0.10 + initial.insertPose3(0, p0) + initial.insertPose3(1, hexagon.atPose3(1).retract(s * np.random.randn(6, 1))) + initial.insertPose3(2, hexagon.atPose3(2).retract(s * np.random.randn(6, 1))) + initial.insertPose3(3, hexagon.atPose3(3).retract(s * np.random.randn(6, 1))) + initial.insertPose3(4, hexagon.atPose3(4).retract(s * np.random.randn(6, 1))) + initial.insertPose3(5, hexagon.atPose3(5).retract(s * np.random.randn(6, 1))) + + # optimize + optimizer = LevenbergMarquardtOptimizer(fg, initial) + result = optimizer.optimizeSafely() + + pose_1 = result.atPose3(1) + self.assertTrue(pose_1.equals(p1, 1e-4)) + +if __name__ == "__main__": + unittest.main() diff --git a/cython/test/test_PriorFactor.py b/cython/test/test_PriorFactor.py new file mode 100644 index 000000000..38c5cd622 --- /dev/null +++ b/cython/test/test_PriorFactor.py @@ -0,0 +1,26 @@ +import unittest +from gtsam import * +from math import * +import numpy as np + +class TestPriorFactor(unittest.TestCase): + + def test_PriorFactor(self): + values = Values() + + key = 5 + priorPose3 = Pose3() + model = noiseModel_Unit.Create(6) + factor = PriorFactorPose3(key, priorPose3, model) + values.insertPose3(key, priorPose3) + self.assertEqual(factor.error(values), 0) + + key = 3 + priorVector = np.array([0., 0., 0.]) + model = noiseModel_Unit.Create(3) + factor = PriorFactorVector(key, priorVector, model) + values.insertVector(key, priorVector) + self.assertEqual(factor.error(values), 0) + +if __name__ == "__main__": + unittest.main() diff --git a/cython/test/test_SFMExample.py b/cython/test/test_SFMExample.py new file mode 100644 index 000000000..112c4b9d4 --- /dev/null +++ b/cython/test/test_SFMExample.py @@ -0,0 +1,70 @@ +import unittest +from gtsam import * +from math import * +import numpy as np +from gtsam_utils import Vector, Matrix +import gtsam_utils.visual_data_generator as generator + + +class TestSFMExample(unittest.TestCase): + + def test_SFMExample(self): + options = generator.Options() + options.triangle = False + options.nrCameras = 10 + + [data, truth] = generator.generate_data(options) + + measurementNoiseSigma = 1.0 + pointNoiseSigma = 0.1 + poseNoiseSigmas = Vector([0.001, 0.001, 0.001, 0.1, 0.1, 0.1]) + + graph = NonlinearFactorGraph() + + # Add factors for all measurements + measurementNoise = noiseModel_Isotropic.Sigma(2, measurementNoiseSigma) + for i in range(len(data.Z)): + for k in range(len(data.Z[i])): + j = data.J[i][k] + graph.add(GenericProjectionFactorCal3_S2( + data.Z[i][k], measurementNoise, + symbol(ord('x'), i), symbol(ord('p'), j), data.K)) + + posePriorNoise = noiseModel_Diagonal.Sigmas(poseNoiseSigmas) + graph.add(PriorFactorPose3(symbol(ord('x'), 0), + truth.cameras[0].pose(), posePriorNoise)) + pointPriorNoise = noiseModel_Isotropic.Sigma(3, pointNoiseSigma) + graph.add(PriorFactorPoint3(symbol(ord('p'), 0), + truth.points[0], pointPriorNoise)) + + # Initial estimate + initialEstimate = Values() + for i in range(len(truth.cameras)): + pose_i = truth.cameras[i].pose() + initialEstimate.insertPose3(symbol(ord('x'), i), pose_i) + for j in range(len(truth.points)): + point_j = truth.points[j] + initialEstimate.insertPoint3(symbol(ord('p'), j), point_j) + + # Optimization + optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate) + for i in range(5): + optimizer.iterate() + result = optimizer.values() + + # Marginalization + marginals = Marginals(graph, result) + marginals.marginalCovariance(symbol(ord('p'), 0)) + marginals.marginalCovariance(symbol(ord('x'), 0)) + + # Check optimized results, should be equal to ground truth + for i in range(len(truth.cameras)): + pose_i = result.atPose3(symbol(ord('x'), i)) + self.assertTrue(pose_i.equals(truth.cameras[i].pose(), 1e-5)) + + for j in range(len(truth.points)): + point_j = result.atPoint3(symbol(ord('p'), j)) + self.assertTrue(point_j.equals(truth.points[j], 1e-5)) + +if __name__ == "__main__": + unittest.main() diff --git a/cython/test/test_StereoVOExample.py b/cython/test/test_StereoVOExample.py new file mode 100644 index 000000000..71e2674f5 --- /dev/null +++ b/cython/test/test_StereoVOExample.py @@ -0,0 +1,70 @@ +import unittest +from gtsam import * +from math import * +import numpy as np +from gtsam_utils import Vector, Matrix + + +class TestStereoVOExample(unittest.TestCase): + + def test_StereoVOExample(self): + ## Assumptions + # - For simplicity this example is in the camera's coordinate frame + # - X: right, Y: down, Z: forward + # - Pose x1 is at the origin, Pose 2 is 1 meter forward (along Z-axis) + # - x1 is fixed with a constraint, x2 is initialized with noisy values + # - No noise on measurements + + ## Create keys for variables + x1 = symbol(ord('x'),1) + x2 = symbol(ord('x'),2) + l1 = symbol(ord('l'),1) + l2 = symbol(ord('l'),2) + l3 = symbol(ord('l'),3) + + ## Create graph container and add factors to it + graph = NonlinearFactorGraph() + + ## add a constraint on the starting pose + first_pose = Pose3() + graph.add(NonlinearEqualityPose3(x1, first_pose)) + + ## Create realistic calibration and measurement noise model + # format: fx fy skew cx cy baseline + K = Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2) + stereo_model = noiseModel_Diagonal.Sigmas(Vector([1.0, 1.0, 1.0])) + + ## Add measurements + # pose 1 + graph.add(GenericStereoFactor3D(StereoPoint2(520, 480, 440), stereo_model, x1, l1, K)) + graph.add(GenericStereoFactor3D(StereoPoint2(120, 80, 440), stereo_model, x1, l2, K)) + graph.add(GenericStereoFactor3D(StereoPoint2(320, 280, 140), stereo_model, x1, l3, K)) + + #pose 2 + graph.add(GenericStereoFactor3D(StereoPoint2(570, 520, 490), stereo_model, x2, l1, K)) + graph.add(GenericStereoFactor3D(StereoPoint2( 70, 20, 490), stereo_model, x2, l2, K)) + graph.add(GenericStereoFactor3D(StereoPoint2(320, 270, 115), stereo_model, x2, l3, K)) + + ## Create initial estimate for camera poses and landmarks + initialEstimate = Values() + initialEstimate.insertPose3(x1, first_pose) + # noisy estimate for pose 2 + initialEstimate.insertPose3(x2, Pose3(Rot3(), Point3(0.1,-.1,1.1))) + expected_l1 = Point3( 1, 1, 5) + initialEstimate.insertPoint3(l1, expected_l1) + initialEstimate.insertPoint3(l2, Point3(-1, 1, 5)) + initialEstimate.insertPoint3(l3, Point3( 0,-.5, 5)) + + ## optimize + optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate) + result = optimizer.optimize() + + ## check equality for the first pose and point + pose_x1 = result.atPose3(x1) + self.assertTrue(pose_x1.equals(first_pose,1e-4)) + + point_l1 = result.atPoint3(l1) + self.assertTrue(point_l1.equals(expected_l1,1e-4)) + +if __name__ == "__main__": + unittest.main() diff --git a/cython/test/test_Values.py b/cython/test/test_Values.py new file mode 100644 index 000000000..350642988 --- /dev/null +++ b/cython/test/test_Values.py @@ -0,0 +1,51 @@ +import unittest +from gtsam import * +from math import * +import numpy as np + +class TestValues(unittest.TestCase): + + def test_values(self): + values = Values() + E = EssentialMatrix(Rot3, Unit3) + tol = 1e-9 + + values.insertPoint2(0, Point2()) + values.insertPoint3(1, Point3()) + values.insertRot2(2, Rot2()) + values.insertPose2(3, Pose2()) + values.insertRot3(4, Rot3()) + values.insertPose3(5, Pose3()) + values.insertCal3_S2(6, Cal3_S2()) + values.insertCal3DS2(7, Cal3DS2()) + values.insertCal3Bundler(8, Cal3Bundler()) + values.insertEssentialMatrix(9, E) + values.insertimuBias_ConstantBias(10, imuBias_ConstantBias()) + + # special cases for Vector and Matrix: + vec = np.array([1., 2., 3.]) + values.insertVector(11, vec) + mat = np.array([[1., 2.], [3., 4.]], order='F') + values.insertMatrix(12, mat) + + self.assertTrue(values.atPoint2(0).equals(Point2(), tol)) + self.assertTrue(values.atPoint3(1).equals(Point3(), tol)) + self.assertTrue(values.atRot2(2).equals(Rot2(), tol)) + self.assertTrue(values.atPose2(3).equals(Pose2(), tol)) + self.assertTrue(values.atRot3(4).equals(Rot3(), tol)) + self.assertTrue(values.atPose3(5).equals(Pose3(), tol)) + self.assertTrue(values.atCal3_S2(6).equals(Cal3_S2(), tol)) + self.assertTrue(values.atCal3DS2(7).equals(Cal3DS2(), tol)) + self.assertTrue(values.atCal3Bundler(8).equals(Cal3Bundler(), tol)) + self.assertTrue(values.atEssentialMatrix(9).equals(E, tol)) + self.assertTrue(values.atimuBias_ConstantBias( + 10).equals(imuBias_ConstantBias(), tol)) + + # special cases for Vector and Matrix: + actualVector = values.atVector(11) + self.assertTrue(np.allclose(vec, actualVector.ravel(), tol)) + actualMatrix = values.atMatrix(12) + self.assertTrue(np.allclose(mat, actualMatrix, tol)) + +if __name__ == "__main__": + unittest.main() diff --git a/cython/test/test_VisualISAMExample.py b/cython/test/test_VisualISAMExample.py new file mode 100644 index 000000000..5e7133d74 --- /dev/null +++ b/cython/test/test_VisualISAMExample.py @@ -0,0 +1,44 @@ +import unittest +from gtsam import * +from math import * +import numpy as np +from gtsam_utils import Vector, Matrix +import gtsam_utils.visual_data_generator as generator +import gtsam_utils.visual_isam as visual_isam + +class TestVisualISAMExample(unittest.TestCase): + + def test_VisualISAMExample(self): + # Data Options + options = generator.Options() + options.triangle = False + options.nrCameras = 20 + + # iSAM Options + isamOptions = visual_isam.Options() + isamOptions.hardConstraint = False + isamOptions.pointPriors = False + isamOptions.batchInitialization = True + isamOptions.reorderInterval = 10 + isamOptions.alwaysRelinearize = False + + # Generate data + data, truth = generator.generate_data(options) + + # Initialize iSAM with the first pose and points + isam, result, nextPose = visual_isam.initialize(data, truth, isamOptions) + + # Main loop for iSAM: stepping through all poses + for currentPose in range(nextPose, options.nrCameras): + isam, result = visual_isam.step(data, isam, result, truth, currentPose) + + for i in range(len(truth.cameras)): + pose_i = result.atPose3(symbol(ord('x'), i)) + self.assertTrue(pose_i.equals(truth.cameras[i].pose(), 1e-5)) + + for j in range(len(truth.points)): + point_j = result.atPoint3(symbol(ord('l'), j)) + self.assertTrue(point_j.equals(truth.points[j], 1e-5)) + +if __name__ == "__main__": + unittest.main()