From 3daf8d73515dac76d109c8dacf8de019c78bb56a Mon Sep 17 00:00:00 2001 From: Duy-Nguyen Ta Date: Sat, 18 Mar 2017 19:50:35 -0400 Subject: [PATCH] fix bad import * style --- cython/gtsam/tests/experiments.py | 59 +++++++++---------- cython/gtsam/tests/test_Cal3Unified.py | 4 +- cython/gtsam/tests/test_JacobianFactor.py | 17 +++--- cython/gtsam/tests/test_KalmanFilter.py | 9 ++- .../gtsam/tests/test_LocalizationExample.py | 38 ++++++------ cython/gtsam/tests/test_OdometryExample.py | 33 +++++------ cython/gtsam/tests/test_PlanarSLAMExample.py | 52 ++++++++-------- cython/gtsam/tests/test_Pose2SLAMExample.py | 52 ++++++++-------- cython/gtsam/tests/test_Pose3SLAMExample.py | 26 ++++---- cython/gtsam/tests/test_PriorFactor.py | 15 +++-- cython/gtsam/tests/test_SFMExample.py | 24 ++++---- cython/gtsam/tests/test_StereoVOExample.py | 38 ++++++------ cython/gtsam/tests/test_Values.py | 47 ++++++++------- cython/gtsam/tests/test_VisualISAMExample.py | 3 +- cython/gtsam/utils/circlePose3.py | 4 +- cython/gtsam/utils/visual_data_generator.py | 58 +++++++++--------- cython/gtsam/utils/visual_isam.py | 36 ++++++----- 17 files changed, 252 insertions(+), 263 deletions(-) diff --git a/cython/gtsam/tests/experiments.py b/cython/gtsam/tests/experiments.py index 8f7dab5cf..425180173 100644 --- a/cython/gtsam/tests/experiments.py +++ b/cython/gtsam/tests/experiments.py @@ -1,20 +1,19 @@ """ -This file contains small experiments to test the wrapper with gtsam_short, -not real unittests. Its name convention is different from other tests so it -won't be discovered. +This file is not a real python unittest. It contains small experiments +to test the wrapper with gtsam_test, a short version of gtsam.h. +Its name convention is different from other tests so it won't be discovered. """ -from gtsam import * +import gtsam import numpy as np -from utils import Vector, Matrix -r = Rot3() +r = gtsam.Rot3() print(r) print(r.pitch()) -r2 = Rot3() +r2 = gtsam.Rot3() r3 = r.compose(r2) print("r3 pitch:", r3.pitch()) -v = Vector(1, 1, 1) +v = np.array([1, 1, 1]) print("v = ", v) r4 = r3.retract(v) print("r4 pitch:", r4.pitch()) @@ -29,35 +28,35 @@ Rmat = np.array([ [0.104218, 0.990074, -0.0942928], [-0.0942928, 0.104218, 0.990074] ]) -r5 = Rot3(Rmat) +r5 = gtsam.Rot3(Rmat) r5.print_(b"r5: ") -l = Rot3.Logmap(r5) +l = gtsam.Rot3.Logmap(r5) print("l = ", l) -noise = noiseModel_Gaussian.Covariance(Rmat) +noise = gtsam.noiseModel_Gaussian.Covariance(Rmat) noise.print_(b"noise:") D = np.array([1.,2.,3.]) -diag = noiseModel_Diagonal.Variances(D) +diag = gtsam.noiseModel_Diagonal.Variances(D) print("diag:", diag) diag.print_(b"diag:") print("diag R:", diag.R()) -p = Point3() +p = gtsam.Point3() p.print_("p:") -factor = BetweenFactorPoint3(1,2,p, noise) +factor = gtsam.BetweenFactorPoint3(1,2,p, noise) factor.print_(b"factor:") -vv = VectorValues() +vv = gtsam.VectorValues() vv.print_(b"vv:") vv.insert(1, np.array([1.,2.,3.])) vv.insert(2, np.array([3.,4.])) vv.insert(3, np.array([5.,6.,7.,8.])) vv.print_(b"vv:") -vv2 = VectorValues(vv) +vv2 = gtsam.VectorValues(vv) vv2.insert(4, np.array([4.,2.,1])) vv2.print_(b"vv2:") vv.print_(b"vv:") @@ -68,17 +67,17 @@ vv3 = vv.add(vv2) vv3.print_(b"vv3:") -values = Values() -values.insert(1, Point3()) -values.insert(2, Rot3()) +values = gtsam.Values() +values.insert(1, gtsam.Point3()) +values.insert(2, gtsam.Rot3()) values.print_(b"values:") -factor = PriorFactorVector(1, np.array([1.,2.,3.]), diag) +factor = gtsam.PriorFactorVector(1, np.array([1.,2.,3.]), diag) print "Prior factor vector: ", factor -keys = KeyVector() +keys = gtsam.KeyVector() keys.push_back(1) keys.push_back(2) @@ -86,28 +85,28 @@ print 'size: ', keys.size() print keys.at(0) print keys.at(1) -noise = noiseModel_Isotropic.Precision(2, 3.0) +noise = gtsam.noiseModel_Isotropic.Precision(2, 3.0) noise.print_('noise:') print 'noise print:', noise -f = JacobianFactor(7, np.ones([2,2]), model=noise, b=np.ones(2)) +f = gtsam.JacobianFactor(7, np.ones([2,2]), model=noise, b=np.ones(2)) print 'JacobianFactor(7):\n', f print "A = ", f.getA() print "b = ", f.getb() -f = JacobianFactor(np.ones(2)) +f = gtsam.JacobianFactor(np.ones(2)) f.print_('jacoboian b_in:') print "JacobianFactor initalized with b_in:", f -diag = noiseModel_Diagonal.Sigmas(np.array([1.,2.,3.])) -fv = PriorFactorVector(1, np.array([4.,5.,6.]), diag) +diag = gtsam.noiseModel_Diagonal.Sigmas(np.array([1.,2.,3.])) +fv = gtsam.PriorFactorVector(1, np.array([4.,5.,6.]), diag) print "priorfactorvector: ", fv print "base noise: ", fv.get_noiseModel() -print "casted to gaussian2: ", dynamic_cast_noiseModel_Diagonal_noiseModel_Base(fv.get_noiseModel()) +print "casted to gaussian2: ", gtsam.dynamic_cast_noiseModel_Diagonal_noiseModel_Base(fv.get_noiseModel()) -X = symbol(65, 19) +X = gtsam.symbol(65, 19) print X -print symbolChr(X) -print symbolIndex(X) +print gtsam.symbolChr(X) +print gtsam.symbolIndex(X) diff --git a/cython/gtsam/tests/test_Cal3Unified.py b/cython/gtsam/tests/test_Cal3Unified.py index b75674e5d..b7097a24e 100644 --- a/cython/gtsam/tests/test_Cal3Unified.py +++ b/cython/gtsam/tests/test_Cal3Unified.py @@ -1,5 +1,5 @@ import unittest -from gtsam import * +import gtsam from math import * import numpy as np @@ -7,7 +7,7 @@ import numpy as np class TestCal3Unified(unittest.TestCase): def test_Cal3Unified(self): - K = Cal3Unified() + K = gtsam.Cal3Unified() self.assertEqual(K.fx(), 1.) self.assertEqual(K.fx(), 1.) diff --git a/cython/gtsam/tests/test_JacobianFactor.py b/cython/gtsam/tests/test_JacobianFactor.py index ebca30b04..bf63c839b 100644 --- a/cython/gtsam/tests/test_JacobianFactor.py +++ b/cython/gtsam/tests/test_JacobianFactor.py @@ -1,6 +1,5 @@ import unittest -from gtsam import * -from math import * +import gtsam import numpy as np class TestJacobianFactor(unittest.TestCase): @@ -35,12 +34,12 @@ class TestJacobianFactor(unittest.TestCase): # the RHS b2 = np.array([-1., 1.5, 2., -1.]) sigmas = np.array([1., 1., 1., 1.]) - model4 = noiseModel_Diagonal.Sigmas(sigmas) - combined = JacobianFactor(x2, Ax2, l1, Al1, x1, Ax1, b2, model4) + model4 = gtsam.noiseModel_Diagonal.Sigmas(sigmas) + combined = gtsam.JacobianFactor(x2, Ax2, l1, Al1, x1, Ax1, b2, model4) # eliminate the first variable (x2) in the combined factor, destructive # ! - ord = Ordering() + ord = gtsam.Ordering() ord.push_back(x2) actualCG, lf = combined.eliminate(ord) @@ -52,8 +51,8 @@ class TestJacobianFactor(unittest.TestCase): S13 = np.array([[-8.94427, 0.00], [+0.00, -8.94427]]) d = np.array([2.23607, -1.56525]) - expectedCG = GaussianConditional( - x2, d, R11, l1, S12, x1, S13, noiseModel_Unit.Create(2)) + expectedCG = gtsam.GaussianConditional( + x2, d, R11, l1, S12, x1, S13, gtsam.noiseModel_Unit.Create(2)) # check if the result matches self.assertTrue(actualCG.equals(expectedCG, 1e-4)) @@ -69,8 +68,8 @@ class TestJacobianFactor(unittest.TestCase): # the RHS b1 = np.array([0.0, 0.894427]) - model2 = noiseModel_Diagonal.Sigmas(np.array([1., 1.])) - expectedLF = JacobianFactor(l1, Bl1, x1, Bx1, b1, model2) + model2 = gtsam.noiseModel_Diagonal.Sigmas(np.array([1., 1.])) + expectedLF = gtsam.JacobianFactor(l1, Bl1, x1, Bx1, b1, model2) # check if the result matches the combined (reduced) factor self.assertTrue(lf.equals(expectedLF, 1e-4)) diff --git a/cython/gtsam/tests/test_KalmanFilter.py b/cython/gtsam/tests/test_KalmanFilter.py index 45b48778a..56f9e2573 100644 --- a/cython/gtsam/tests/test_KalmanFilter.py +++ b/cython/gtsam/tests/test_KalmanFilter.py @@ -1,6 +1,5 @@ import unittest -from gtsam import * -from math import * +import gtsam import numpy as np class TestKalmanFilter(unittest.TestCase): @@ -9,13 +8,13 @@ class TestKalmanFilter(unittest.TestCase): 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])) + modelQ = gtsam.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])) + modelR = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.1])) R = 0.01 * np.eye(2) # Create the set of expected output TestValues @@ -35,7 +34,7 @@ class TestKalmanFilter(unittest.TestCase): I33 = np.linalg.inv(P23) + np.linalg.inv(R) # Create an KalmanFilter object - KF = KalmanFilter(n=2) + KF = gtsam.KalmanFilter(n=2) # Create the Kalman Filter initialization point x_initial = np.array([0.0, 0.0]) diff --git a/cython/gtsam/tests/test_LocalizationExample.py b/cython/gtsam/tests/test_LocalizationExample.py index cc7792df2..c373f162c 100644 --- a/cython/gtsam/tests/test_LocalizationExample.py +++ b/cython/gtsam/tests/test_LocalizationExample.py @@ -1,7 +1,5 @@ import unittest -from gtsam import * -from gtsam.utils import * -from math import * +import gtsam import numpy as np class TestLocalizationExample(unittest.TestCase): @@ -9,39 +7,39 @@ class TestLocalizationExample(unittest.TestCase): def test_LocalizationExample(self): # Create the graph (defined in pose2SLAM.h, derived from # NonlinearFactorGraph) - graph = NonlinearFactorGraph() + graph = gtsam.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( + odometry = gtsam.Pose2(2.0, 0.0, 0.0) + odometryNoise = gtsam.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)) + graph.add(gtsam.BetweenFactorPose2(0, 1, odometry, odometryNoise)) + graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, odometryNoise)) # Add three "GPS" measurements # We use Pose2 Priors here with high variance on theta - groundTruth = Values() - groundTruth.insert(0, Pose2(0.0, 0.0, 0.0)) - groundTruth.insert(1, Pose2(2.0, 0.0, 0.0)) - groundTruth.insert(2, Pose2(4.0, 0.0, 0.0)) - model = noiseModel_Diagonal.Sigmas(np.array([0.1, 0.1, 10.])) + groundTruth = gtsam.Values() + groundTruth.insert(0, gtsam.Pose2(0.0, 0.0, 0.0)) + groundTruth.insert(1, gtsam.Pose2(2.0, 0.0, 0.0)) + groundTruth.insert(2, gtsam.Pose2(4.0, 0.0, 0.0)) + model = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.1, 0.1, 10.])) for i in range(3): - graph.add(PriorFactorPose2(i, groundTruth.atPose2(i), model)) + graph.add(gtsam.PriorFactorPose2(i, groundTruth.atPose2(i), model)) # Initialize to noisy points - initialEstimate = Values() - initialEstimate.insert(0, Pose2(0.5, 0.0, 0.2)) - initialEstimate.insert(1, Pose2(2.3, 0.1, -0.2)) - initialEstimate.insert(2, Pose2(4.1, 0.1, 0.1)) + initialEstimate = gtsam.Values() + initialEstimate.insert(0, gtsam.Pose2(0.5, 0.0, 0.2)) + initialEstimate.insert(1, gtsam.Pose2(2.3, 0.1, -0.2)) + initialEstimate.insert(2, gtsam.Pose2(4.1, 0.1, 0.1)) # Optimize using Levenberg-Marquardt optimization with an ordering from # colamd - optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate) + optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate) result = optimizer.optimizeSafely() # Plot Covariance Ellipses - marginals = Marginals(graph, result) + marginals = gtsam.Marginals(graph, result) P = [None] * result.size() for i in range(0, result.size()): pose_i = result.atPose2(i) diff --git a/cython/gtsam/tests/test_OdometryExample.py b/cython/gtsam/tests/test_OdometryExample.py index f38709e9f..1100e8334 100644 --- a/cython/gtsam/tests/test_OdometryExample.py +++ b/cython/gtsam/tests/test_OdometryExample.py @@ -1,6 +1,5 @@ import unittest -from gtsam import * -from math import * +import gtsam import numpy as np class TestOdometryExample(unittest.TestCase): @@ -8,39 +7,39 @@ class TestOdometryExample(unittest.TestCase): def test_OdometryExample(self): # Create the graph (defined in pose2SLAM.h, derived from # NonlinearFactorGraph) - graph = NonlinearFactorGraph() + graph = gtsam.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( + priorMean = gtsam.Pose2(0.0, 0.0, 0.0) # prior mean is at origin + priorNoise = gtsam.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)) + graph.add(gtsam.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( + odometry = gtsam.Pose2(2.0, 0.0, 0.0) + odometryNoise = gtsam.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)) + graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, odometryNoise)) + graph.add(gtsam.BetweenFactorPose2(2, 3, odometry, odometryNoise)) # Initialize to noisy points - initialEstimate = Values() - initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2)) - initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2)) - initialEstimate.insert(3, Pose2(4.1, 0.1, 0.1)) + initialEstimate = gtsam.Values() + initialEstimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2)) + initialEstimate.insert(2, gtsam.Pose2(2.3, 0.1, -0.2)) + initialEstimate.insert(3, gtsam.Pose2(4.1, 0.1, 0.1)) # Optimize using Levenberg-Marquardt optimization with an ordering from # colamd - optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate) + optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate) result = optimizer.optimizeSafely() - marginals = Marginals(graph, result) + marginals = gtsam.Marginals(graph, result) marginals.marginalCovariance(1) # Check first pose equality pose_1 = result.atPose2(1) - self.assertTrue(pose_1.equals(Pose2(), 1e-4)) + self.assertTrue(pose_1.equals(gtsam.Pose2(), 1e-4)) if __name__ == "__main__": unittest.main() diff --git a/cython/gtsam/tests/test_PlanarSLAMExample.py b/cython/gtsam/tests/test_PlanarSLAMExample.py index 9053f3fb3..046a93f35 100644 --- a/cython/gtsam/tests/test_PlanarSLAMExample.py +++ b/cython/gtsam/tests/test_PlanarSLAMExample.py @@ -1,6 +1,6 @@ import unittest -from gtsam import * -from math import * +import gtsam +from math import pi import numpy as np class TestPose2SLAMExample(unittest.TestCase): @@ -13,50 +13,50 @@ class TestPose2SLAMExample(unittest.TestCase): # - The robot is on a grid, moving 2 meters each step # Create graph container and add factors to it - graph = NonlinearFactorGraph() + graph = gtsam.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])) + priorMean = gtsam.Pose2(0.0, 0.0, 0.0) # prior at origin + priorNoise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) # add directly to graph - graph.add(PriorFactorPose2(1, priorMean, priorNoise)) + graph.add(gtsam.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)) + odometryNoise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) + graph.add(gtsam.BetweenFactorPose2( + 1, 2, gtsam.Pose2(2.0, 0.0, 0.0), odometryNoise)) + graph.add(gtsam.BetweenFactorPose2( + 2, 3, gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise)) + graph.add(gtsam.BetweenFactorPose2( + 3, 4, gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise)) + graph.add(gtsam.BetweenFactorPose2( + 4, 5, gtsam.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)) + model = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) + graph.add(gtsam.BetweenFactorPose2(5, 2, gtsam.Pose2(2.0, 0.0, pi / 2), model)) # Initialize to noisy points - initialEstimate = Values() - initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2)) - initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2)) - initialEstimate.insert(3, Pose2(4.1, 0.1, pi / 2)) - initialEstimate.insert(4, Pose2(4.0, 2.0, pi)) - initialEstimate.insert(5, Pose2(2.1, 2.1, -pi / 2)) + initialEstimate = gtsam.Values() + initialEstimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2)) + initialEstimate.insert(2, gtsam.Pose2(2.3, 0.1, -0.2)) + initialEstimate.insert(3, gtsam.Pose2(4.1, 0.1, pi / 2)) + initialEstimate.insert(4, gtsam.Pose2(4.0, 2.0, pi)) + initialEstimate.insert(5, gtsam.Pose2(2.1, 2.1, -pi / 2)) # Optimize using Levenberg-Marquardt optimization with an ordering from # colamd - optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate) + optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate) result = optimizer.optimizeSafely() # Plot Covariance Ellipses - marginals = Marginals(graph, result) + marginals = gtsam.Marginals(graph, result) P = marginals.marginalCovariance(1) pose_1 = result.atPose2(1) - self.assertTrue(pose_1.equals(Pose2(), 1e-4)) + self.assertTrue(pose_1.equals(gtsam.Pose2(), 1e-4)) diff --git a/cython/gtsam/tests/test_Pose2SLAMExample.py b/cython/gtsam/tests/test_Pose2SLAMExample.py index dce96d7b1..bcaa7be4f 100644 --- a/cython/gtsam/tests/test_Pose2SLAMExample.py +++ b/cython/gtsam/tests/test_Pose2SLAMExample.py @@ -1,6 +1,6 @@ import unittest -from gtsam import * -from math import * +import gtsam +from math import pi import numpy as np class TestPose2SLAMExample(unittest.TestCase): @@ -13,50 +13,50 @@ class TestPose2SLAMExample(unittest.TestCase): # - The robot is on a grid, moving 2 meters each step # Create graph container and add factors to it - graph = NonlinearFactorGraph() + graph = gtsam.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])) + priorMean = gtsam.Pose2(0.0, 0.0, 0.0) # prior at origin + priorNoise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) # add directly to graph - graph.add(PriorFactorPose2(1, priorMean, priorNoise)) + graph.add(gtsam.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)) + odometryNoise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) + graph.add(gtsam.BetweenFactorPose2( + 1, 2, gtsam.Pose2(2.0, 0.0, 0.0), odometryNoise)) + graph.add(gtsam.BetweenFactorPose2( + 2, 3, gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise)) + graph.add(gtsam.BetweenFactorPose2( + 3, 4, gtsam.Pose2(2.0, 0.0, pi / 2), odometryNoise)) + graph.add(gtsam.BetweenFactorPose2( + 4, 5, gtsam.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)) + model = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) + graph.add(gtsam.BetweenFactorPose2(5, 2, gtsam.Pose2(2.0, 0.0, pi / 2), model)) # Initialize to noisy points - initialEstimate = Values() - initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2)) - initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2)) - initialEstimate.insert(3, Pose2(4.1, 0.1, pi / 2)) - initialEstimate.insert(4, Pose2(4.0, 2.0, pi)) - initialEstimate.insert(5, Pose2(2.1, 2.1, -pi / 2)) + initialEstimate = gtsam.Values() + initialEstimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2)) + initialEstimate.insert(2, gtsam.Pose2(2.3, 0.1, -0.2)) + initialEstimate.insert(3, gtsam.Pose2(4.1, 0.1, pi / 2)) + initialEstimate.insert(4, gtsam.Pose2(4.0, 2.0, pi)) + initialEstimate.insert(5, gtsam.Pose2(2.1, 2.1, -pi / 2)) # Optimize using Levenberg-Marquardt optimization with an ordering from # colamd - optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate) + optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate) result = optimizer.optimizeSafely() # Plot Covariance Ellipses - marginals = Marginals(graph, result) + marginals = gtsam.Marginals(graph, result) P = marginals.marginalCovariance(1) pose_1 = result.atPose2(1) - self.assertTrue(pose_1.equals(Pose2(), 1e-4)) + self.assertTrue(pose_1.equals(gtsam.Pose2(), 1e-4)) if __name__ == "__main__": unittest.main() diff --git a/cython/gtsam/tests/test_Pose3SLAMExample.py b/cython/gtsam/tests/test_Pose3SLAMExample.py index 81644f61c..e33db2145 100644 --- a/cython/gtsam/tests/test_Pose3SLAMExample.py +++ b/cython/gtsam/tests/test_Pose3SLAMExample.py @@ -1,7 +1,7 @@ import unittest -from gtsam import * -from math import * import numpy as np +import gtsam +from math import pi from gtsam.utils.circlePose3 import * class TestPose3SLAMExample(unittest.TestCase): @@ -13,20 +13,20 @@ class TestPose3SLAMExample(unittest.TestCase): p1 = hexagon.atPose3(1) # create a Pose graph with one equality constraint and one measurement - fg = NonlinearFactorGraph() - fg.add(NonlinearEqualityPose3(0, p0)) + fg = gtsam.NonlinearFactorGraph() + fg.add(gtsam.NonlinearEqualityPose3(0, p0)) delta = p0.between(p1) - covariance = noiseModel_Diagonal.Sigmas( + covariance = gtsam.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)) + fg.add(gtsam.BetweenFactorPose3(0, 1, delta, covariance)) + fg.add(gtsam.BetweenFactorPose3(1, 2, delta, covariance)) + fg.add(gtsam.BetweenFactorPose3(2, 3, delta, covariance)) + fg.add(gtsam.BetweenFactorPose3(3, 4, delta, covariance)) + fg.add(gtsam.BetweenFactorPose3(4, 5, delta, covariance)) + fg.add(gtsam.BetweenFactorPose3(5, 0, delta, covariance)) # Create initial config - initial = Values() + initial = gtsam.Values() s = 0.10 initial.insert(0, p0) initial.insert(1, hexagon.atPose3(1).retract(s * np.random.randn(6, 1))) @@ -36,7 +36,7 @@ class TestPose3SLAMExample(unittest.TestCase): initial.insert(5, hexagon.atPose3(5).retract(s * np.random.randn(6, 1))) # optimize - optimizer = LevenbergMarquardtOptimizer(fg, initial) + optimizer = gtsam.LevenbergMarquardtOptimizer(fg, initial) result = optimizer.optimizeSafely() pose_1 = result.atPose3(1) diff --git a/cython/gtsam/tests/test_PriorFactor.py b/cython/gtsam/tests/test_PriorFactor.py index 80e3622c0..95ec2ae94 100644 --- a/cython/gtsam/tests/test_PriorFactor.py +++ b/cython/gtsam/tests/test_PriorFactor.py @@ -1,24 +1,23 @@ import unittest -from gtsam import * -from math import * +import gtsam import numpy as np class TestPriorFactor(unittest.TestCase): def test_PriorFactor(self): - values = Values() + values = gtsam.Values() key = 5 - priorPose3 = Pose3() - model = noiseModel_Unit.Create(6) - factor = PriorFactorPose3(key, priorPose3, model) + priorPose3 = gtsam.Pose3() + model = gtsam.noiseModel_Unit.Create(6) + factor = gtsam.PriorFactorPose3(key, priorPose3, model) values.insert(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) + model = gtsam.noiseModel_Unit.Create(3) + factor = gtsam.PriorFactorVector(key, priorVector, model) values.insert(key, priorVector) self.assertEqual(factor.error(values), 0) diff --git a/cython/gtsam/tests/test_SFMExample.py b/cython/gtsam/tests/test_SFMExample.py index 8954a831e..606b26a43 100644 --- a/cython/gtsam/tests/test_SFMExample.py +++ b/cython/gtsam/tests/test_SFMExample.py @@ -1,6 +1,6 @@ import unittest -from gtsam import * -from math import * +import gtsam +from gtsam import symbol import numpy as np import gtsam.utils.visual_data_generator as generator @@ -18,26 +18,26 @@ class TestSFMExample(unittest.TestCase): pointNoiseSigma = 0.1 poseNoiseSigmas = np.array([0.001, 0.001, 0.001, 0.1, 0.1, 0.1]) - graph = NonlinearFactorGraph() + graph = gtsam.NonlinearFactorGraph() # Add factors for all measurements - measurementNoise = noiseModel_Isotropic.Sigma(2, measurementNoiseSigma) + measurementNoise = gtsam.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( + graph.add(gtsam.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), + posePriorNoise = gtsam.noiseModel_Diagonal.Sigmas(poseNoiseSigmas) + graph.add(gtsam.PriorFactorPose3(symbol(ord('x'), 0), truth.cameras[0].pose(), posePriorNoise)) - pointPriorNoise = noiseModel_Isotropic.Sigma(3, pointNoiseSigma) - graph.add(PriorFactorPoint3(symbol(ord('p'), 0), + pointPriorNoise = gtsam.noiseModel_Isotropic.Sigma(3, pointNoiseSigma) + graph.add(gtsam.PriorFactorPoint3(symbol(ord('p'), 0), truth.points[0], pointPriorNoise)) # Initial estimate - initialEstimate = Values() + initialEstimate = gtsam.Values() for i in range(len(truth.cameras)): pose_i = truth.cameras[i].pose() initialEstimate.insert(symbol(ord('x'), i), pose_i) @@ -46,13 +46,13 @@ class TestSFMExample(unittest.TestCase): initialEstimate.insert(symbol(ord('p'), j), point_j) # Optimization - optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate) + optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate) for i in range(5): optimizer.iterate() result = optimizer.values() # Marginalization - marginals = Marginals(graph, result) + marginals = gtsam.Marginals(graph, result) marginals.marginalCovariance(symbol(ord('p'), 0)) marginals.marginalCovariance(symbol(ord('x'), 0)) diff --git a/cython/gtsam/tests/test_StereoVOExample.py b/cython/gtsam/tests/test_StereoVOExample.py index 9dc282349..dacd4a116 100644 --- a/cython/gtsam/tests/test_StereoVOExample.py +++ b/cython/gtsam/tests/test_StereoVOExample.py @@ -1,6 +1,6 @@ import unittest -from gtsam import * -from math import * +import gtsam +from gtsam import symbol import numpy as np @@ -22,40 +22,40 @@ class TestStereoVOExample(unittest.TestCase): l3 = symbol(ord('l'),3) ## Create graph container and add factors to it - graph = NonlinearFactorGraph() + graph = gtsam.NonlinearFactorGraph() ## add a constraint on the starting pose - first_pose = Pose3() - graph.add(NonlinearEqualityPose3(x1, first_pose)) + first_pose = gtsam.Pose3() + graph.add(gtsam.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(np.array([1.0, 1.0, 1.0])) + K = gtsam.Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2) + stereo_model = gtsam.noiseModel_Diagonal.Sigmas(np.array([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)) + graph.add(gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(520, 480, 440), stereo_model, x1, l1, K)) + graph.add(gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(120, 80, 440), stereo_model, x1, l2, K)) + graph.add(gtsam.GenericStereoFactor3D(gtsam.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)) + graph.add(gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(570, 520, 490), stereo_model, x2, l1, K)) + graph.add(gtsam.GenericStereoFactor3D(gtsam.StereoPoint2( 70, 20, 490), stereo_model, x2, l2, K)) + graph.add(gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(320, 270, 115), stereo_model, x2, l3, K)) ## Create initial estimate for camera poses and landmarks - initialEstimate = Values() + initialEstimate = gtsam.Values() initialEstimate.insert(x1, first_pose) # noisy estimate for pose 2 - initialEstimate.insert(x2, Pose3(Rot3(), Point3(0.1,-.1,1.1))) - expected_l1 = Point3( 1, 1, 5) + initialEstimate.insert(x2, gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(0.1,-.1,1.1))) + expected_l1 = gtsam.Point3( 1, 1, 5) initialEstimate.insert(l1, expected_l1) - initialEstimate.insert(l2, Point3(-1, 1, 5)) - initialEstimate.insert(l3, Point3( 0,-.5, 5)) + initialEstimate.insert(l2, gtsam.Point3(-1, 1, 5)) + initialEstimate.insert(l3, gtsam.Point3( 0,-.5, 5)) ## optimize - optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate) + optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate) result = optimizer.optimize() ## check equality for the first pose and point diff --git a/cython/gtsam/tests/test_Values.py b/cython/gtsam/tests/test_Values.py index 6d5d8e077..cca38d8cd 100644 --- a/cython/gtsam/tests/test_Values.py +++ b/cython/gtsam/tests/test_Values.py @@ -1,26 +1,25 @@ import unittest -from gtsam import * -from math import * +import gtsam import numpy as np class TestValues(unittest.TestCase): def test_values(self): - values = Values() - E = EssentialMatrix(Rot3(), Unit3()) + values = gtsam.Values() + E = gtsam.EssentialMatrix(gtsam.Rot3(), gtsam.Unit3()) tol = 1e-9 - values.insert(0, Point2()) - values.insert(1, Point3()) - values.insert(2, Rot2()) - values.insert(3, Pose2()) - values.insert(4, Rot3()) - values.insert(5, Pose3()) - values.insert(6, Cal3_S2()) - values.insert(7, Cal3DS2()) - values.insert(8, Cal3Bundler()) + values.insert(0, gtsam.Point2()) + values.insert(1, gtsam.Point3()) + values.insert(2, gtsam.Rot2()) + values.insert(3, gtsam.Pose2()) + values.insert(4, gtsam.Rot3()) + values.insert(5, gtsam.Pose3()) + values.insert(6, gtsam.Cal3_S2()) + values.insert(7, gtsam.Cal3DS2()) + values.insert(8, gtsam.Cal3Bundler()) values.insert(9, E) - values.insert(10, imuBias_ConstantBias()) + values.insert(10, gtsam.imuBias_ConstantBias()) # Special cases for Vectors and Matrices # Note that gtsam's Eigen Vectors and Matrices requires double-precision @@ -42,18 +41,18 @@ class TestValues(unittest.TestCase): mat2 = np.array([[1,2,],[3,5]]) values.insert(13, mat2) - 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.atPoint2(0).equals(gtsam.Point2(), tol)) + self.assertTrue(values.atPoint3(1).equals(gtsam.Point3(), tol)) + self.assertTrue(values.atRot2(2).equals(gtsam.Rot2(), tol)) + self.assertTrue(values.atPose2(3).equals(gtsam.Pose2(), tol)) + self.assertTrue(values.atRot3(4).equals(gtsam.Rot3(), tol)) + self.assertTrue(values.atPose3(5).equals(gtsam.Pose3(), tol)) + self.assertTrue(values.atCal3_S2(6).equals(gtsam.Cal3_S2(), tol)) + self.assertTrue(values.atCal3DS2(7).equals(gtsam.Cal3DS2(), tol)) + self.assertTrue(values.atCal3Bundler(8).equals(gtsam.Cal3Bundler(), tol)) self.assertTrue(values.atEssentialMatrix(9).equals(E, tol)) self.assertTrue(values.atimuBias_ConstantBias( - 10).equals(imuBias_ConstantBias(), tol)) + 10).equals(gtsam.imuBias_ConstantBias(), tol)) # special cases for Vector and Matrix: actualVector = values.atVector(11) diff --git a/cython/gtsam/tests/test_VisualISAMExample.py b/cython/gtsam/tests/test_VisualISAMExample.py index bc4346780..39bfa6eb4 100644 --- a/cython/gtsam/tests/test_VisualISAMExample.py +++ b/cython/gtsam/tests/test_VisualISAMExample.py @@ -1,7 +1,6 @@ import unittest -from gtsam import * -from math import * import numpy as np +from gtsam import symbol import gtsam.utils.visual_data_generator as generator import gtsam.utils.visual_isam as visual_isam diff --git a/cython/gtsam/utils/circlePose3.py b/cython/gtsam/utils/circlePose3.py index 49b64d0b7..cdeb41c45 100644 --- a/cython/gtsam/utils/circlePose3.py +++ b/cython/gtsam/utils/circlePose3.py @@ -1,6 +1,6 @@ -from gtsam import * -from math import * +import gtsam import numpy as np +from math import pi, cos, sin def circlePose3(numPoses = 8, radius = 1.0, symbolChar = 0): """ diff --git a/cython/gtsam/utils/visual_data_generator.py b/cython/gtsam/utils/visual_data_generator.py index 6f404fcc0..3f6391164 100644 --- a/cython/gtsam/utils/visual_data_generator.py +++ b/cython/gtsam/utils/visual_data_generator.py @@ -1,14 +1,14 @@ -from np_utils import * -from math import * -from gtsam import * - +import numpy as np +from math import pi, cos, sin +import gtsam +from gtsam import symbol class Options: """ Options to generate test scenario """ - def __init__(self, triangle=False, nrCameras=3, K=Cal3_S2()): + def __init__(self, triangle=False, nrCameras=3, K=gtsam.Cal3_S2()): """ Options to generate test scenario @param triangle: generate a triangle scene with 3 points if True, otherwise @@ -25,10 +25,10 @@ class GroundTruth: Object holding generated ground-truth data """ - def __init__(self, K=Cal3_S2(), nrCameras=3, nrPoints=4): + def __init__(self, K=gtsam.Cal3_S2(), nrCameras=3, nrPoints=4): self.K = K - self.cameras = [Pose3()] * nrCameras - self.points = [Point3()] * nrPoints + self.cameras = [gtsam.Pose3()] * nrCameras + self.points = [gtsam.Point3()] * nrPoints def print_(self, s=""): print s @@ -50,22 +50,22 @@ class Data: class NoiseModels: pass - def __init__(self, K=Cal3_S2(), nrCameras=3, nrPoints=4): + def __init__(self, K=gtsam.Cal3_S2(), nrCameras=3, nrPoints=4): self.K = K - self.Z = [x[:] for x in [[Point2()] * nrPoints] * nrCameras] + self.Z = [x[:] for x in [[gtsam.Point2()] * nrPoints] * nrCameras] self.J = [x[:] for x in [[0] * nrPoints] * nrCameras] - self.odometry = [Pose3()] * nrCameras + self.odometry = [gtsam.Pose3()] * nrCameras # Set Noise parameters self.noiseModels = Data.NoiseModels() - self.noiseModels.posePrior = noiseModel_Diagonal.Sigmas( + self.noiseModels.posePrior = gtsam.noiseModel_Diagonal.Sigmas( np.array([0.001, 0.001, 0.001, 0.1, 0.1, 0.1])) - # noiseModels.odometry = noiseModel_Diagonal.Sigmas( + # noiseModels.odometry = gtsam.noiseModel_Diagonal.Sigmas( # np.array([0.001,0.001,0.001,0.1,0.1,0.1])) - self.noiseModels.odometry = noiseModel_Diagonal.Sigmas( + self.noiseModels.odometry = gtsam.noiseModel_Diagonal.Sigmas( np.array([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) + self.noiseModels.pointPrior = gtsam.noiseModel_Isotropic.Sigma(3, 0.1) + self.noiseModels.measurement = gtsam.noiseModel_Isotropic.Sigma(2, 1.0) def generate_data(options): @@ -73,7 +73,7 @@ def generate_data(options): Generate ground-truth and measurement data """ - K = Cal3_S2(500, 500, 0, 640. / 2., 480. / 2.) + K = gtsam.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) @@ -84,25 +84,25 @@ def generate_data(options): r = 10 for j in range(len(truth.points)): theta = j * 2 * pi / nrPoints - truth.points[j] = Point3(r * cos(theta), r * sin(theta), 0) + truth.points[j] = gtsam.Point3(r * cos(theta), r * sin(theta), 0) else: # 3D landmarks as vertices of a cube - truth.points = [Point3(10, 10, 10), - Point3(-10, 10, 10), - Point3(-10, -10, 10), - Point3(10, -10, 10), - Point3(10, 10, -10), - Point3(-10, 10, -10), - Point3(-10, -10, -10), - Point3(10, -10, -10)] + truth.points = [gtsam.Point3(10, 10, 10), + gtsam.Point3(-10, 10, 10), + gtsam.Point3(-10, -10, 10), + gtsam.Point3(10, -10, 10), + gtsam.Point3(10, 10, -10), + gtsam.Point3(-10, 10, -10), + gtsam.Point3(-10, -10, -10), + gtsam.Point3(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(r * cos(theta), r * sin(theta), height) - truth.cameras[i] = SimpleCamera.Lookat( - t, Point3(), Point3(0, 0, 1), truth.K) + t = gtsam.Point3(r * cos(theta), r * sin(theta), height) + truth.cameras[i] = gtsam.SimpleCamera.Lookat( + t, gtsam.Point3(), gtsam.Point3(0, 0, 1), truth.K) # Create measurements for j in range(nrPoints): # All landmarks seen in every frame diff --git a/cython/gtsam/utils/visual_isam.py b/cython/gtsam/utils/visual_isam.py index bd1fef8c3..c23ad194d 100644 --- a/cython/gtsam/utils/visual_isam.py +++ b/cython/gtsam/utils/visual_isam.py @@ -1,7 +1,5 @@ -from np_utils import * -from math import * -from gtsam import * - +import gtsam +from gtsam import symbol class Options: """ @@ -20,20 +18,20 @@ def initialize(data, truth, options): params = gtsam.ISAM2Params() if options.alwaysRelinearize: params.setRelinearizeSkip(1) - isam = ISAM2(params = params) + isam = gtsam.ISAM2(params = params) # Add constraints/priors # TODO: should not be from ground truth! - newFactors = NonlinearFactorGraph() - initialEstimates = Values() + newFactors = gtsam.NonlinearFactorGraph() + initialEstimates = gtsam.Values() for i in range(2): ii = symbol(ord('x'), i) if i == 0: if options.hardConstraint: # add hard constraint - newFactors.add(NonlinearEqualityPose3( + newFactors.add(gtsam.NonlinearEqualityPose3( ii, truth.cameras[0].pose())) else: - newFactors.add(PriorFactorPose3( + newFactors.add(gtsam.PriorFactorPose3( ii, truth.cameras[i].pose(), data.noiseModels.posePrior)) initialEstimates.insert(ii, truth.cameras[i].pose()) @@ -46,22 +44,22 @@ def initialize(data, truth, options): for k in range(len(data.Z[i])): j = data.J[i][k] jj = symbol(ord('l'), j) - newFactors.add(GenericProjectionFactorCal3_S2( + newFactors.add(gtsam.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.insert(jj, truth.points[j]) if options.pointPriors: # add point priors - newFactors.add(PriorFactorPoint3( + newFactors.add(gtsam.PriorFactorPoint3( jj, truth.points[j], data.noiseModels.pointPrior)) # Add odometry between frames 0 and 1 - newFactors.add(BetweenFactorPose3(symbol(ord('x'), 0), symbol( + newFactors.add(gtsam.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( + batchOptimizer = gtsam.LevenbergMarquardtOptimizer( newFactors, initialEstimates) fullyOptimized = batchOptimizer.optimize() isam.update(newFactors, fullyOptimized) @@ -87,22 +85,22 @@ def step(data, isam, result, truth, currPoseIndex): """ # iSAM expects us to give it a new set of factors # along with initial estimates for any new variables introduced. - newFactors = NonlinearFactorGraph() - initialEstimates = Values() + newFactors = gtsam.NonlinearFactorGraph() + initialEstimates = gtsam.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)) + newFactors.add(gtsam.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( + newFactors.add(gtsam.GenericProjectionFactorCal3_S2( zij, data.noiseModels.measurement, symbol(ord('x'), currPoseIndex), jj, data.K)) # TODO: initialize with something other than truth