diff --git a/cython/gtsam/examples/SFMExample.py b/cython/gtsam/examples/SFMExample.py index e02def2f9..68053f862 100644 --- a/cython/gtsam/examples/SFMExample.py +++ b/cython/gtsam/examples/SFMExample.py @@ -20,7 +20,7 @@ from gtsam.gtsam import (Cal3_S2, DoglegOptimizer, GenericProjectionFactorCal3_S2, Marginals, NonlinearFactorGraph, PinholeCameraCal3_S2, Point3, Pose3, PriorFactorPoint3, PriorFactorPose3, Rot3, - SimpleCamera, Values) + PinholeCameraCal3_S2, Values) from gtsam.utils import plot diff --git a/python/gtsam_py/python/tests/test_GaussianFactorGraph.py b/python/gtsam_py/python/tests/test_GaussianFactorGraph.py index 759de0f3b..d520b0fa4 100644 --- a/python/gtsam_py/python/tests/test_GaussianFactorGraph.py +++ b/python/gtsam_py/python/tests/test_GaussianFactorGraph.py @@ -15,17 +15,18 @@ from __future__ import print_function import unittest import gtsam +import numpy as np +from gtsam.gtsam.symbol_shorthand import X from gtsam.utils.test_case import GtsamTestCase -import numpy as np def create_graph(): """Create a basic linear factor graph for testing""" graph = gtsam.GaussianFactorGraph() - x0 = gtsam.symbol('x', 0) - x1 = gtsam.symbol('x', 1) - x2 = gtsam.symbol('x', 2) + x0 = X(0) + x1 = X(1) + x2 = X(2) BETWEEN_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.ones(1)) PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.ones(1)) diff --git a/python/gtsam_py/python/tests/test_NonlinearOptimizer.py b/python/gtsam_py/python/tests/test_NonlinearOptimizer.py index 3041e325c..e9234a43b 100644 --- a/python/gtsam_py/python/tests/test_NonlinearOptimizer.py +++ b/python/gtsam_py/python/tests/test_NonlinearOptimizer.py @@ -15,10 +15,11 @@ from __future__ import print_function import unittest import gtsam -from gtsam import (DoglegOptimizer, DoglegParams, GaussNewtonOptimizer, +from gtsam import (DoglegOptimizer, DoglegParams, + DummyPreconditionerParameters, GaussNewtonOptimizer, GaussNewtonParams, LevenbergMarquardtOptimizer, LevenbergMarquardtParams, NonlinearFactorGraph, Ordering, - Point2, PriorFactorPoint2, Values) + PCGSolverParameters, Point2, PriorFactorPoint2, Values) from gtsam.utils.test_case import GtsamTestCase KEY1 = 1 @@ -61,6 +62,16 @@ class TestScenario(GtsamTestCase): fg, initial_values, lmParams).optimize() self.assertAlmostEqual(0, fg.error(actual2)) + # Levenberg-Marquardt + lmParams = LevenbergMarquardtParams.CeresDefaults() + lmParams.setLinearSolverType("ITERATIVE") + cgParams = PCGSolverParameters() + cgParams.setPreconditionerParams(DummyPreconditionerParameters()) + lmParams.setIterativeParams(cgParams) + actual2 = LevenbergMarquardtOptimizer( + fg, initial_values, lmParams).optimize() + self.assertAlmostEqual(0, fg.error(actual2)) + # Dogleg dlParams = DoglegParams() dlParams.setOrdering(ordering) diff --git a/python/gtsam_py/python/tests/test_PriorFactor.py b/python/gtsam_py/python/tests/test_PriorFactor.py index da072d0bd..0582cf5d7 100644 --- a/python/gtsam_py/python/tests/test_PriorFactor.py +++ b/python/gtsam_py/python/tests/test_PriorFactor.py @@ -35,5 +35,27 @@ class TestPriorFactor(GtsamTestCase): values.insert(key, priorVector) self.assertEqual(factor.error(values), 0) + def test_AddPrior(self): + """ + Test adding prior factors directly to factor graph via the .addPrior method. + """ + # define factor graph + graph = gtsam.NonlinearFactorGraph() + + # define and add Pose3 prior + key = 5 + priorPose3 = gtsam.Pose3() + model = gtsam.noiseModel.Unit.Create(6) + graph.addPriorPose3(key, priorPose3, model) + self.assertEqual(graph.size(), 1) + + # define and add Vector prior + key = 3 + priorVector = np.array([0., 0., 0.]) + model = gtsam.noiseModel.Unit.Create(3) + graph.addPriorVector(key, priorVector, model) + self.assertEqual(graph.size(), 2) + + if __name__ == "__main__": unittest.main() diff --git a/python/gtsam_py/python/tests/test_SFMExample.py b/python/gtsam_py/python/tests/test_SFMExample.py index 8b5473ff0..1425902cf 100644 --- a/python/gtsam_py/python/tests/test_SFMExample.py +++ b/python/gtsam_py/python/tests/test_SFMExample.py @@ -15,6 +15,7 @@ import numpy as np import gtsam import gtsam.utils.visual_data_generator as generator from gtsam import symbol +from gtsam.gtsam.noiseModel improt Isotropic, Diagonal from gtsam.utils.test_case import GtsamTestCase @@ -34,7 +35,7 @@ class TestSFMExample(GtsamTestCase): graph = gtsam.NonlinearFactorGraph() # Add factors for all measurements - measurementNoise = gtsam.noiseModel.Isotropic.Sigma(2, measurementNoiseSigma) + measurementNoise = Isotropic.Sigma(2, measurementNoiseSigma) for i in range(len(data.Z)): for k in range(len(data.Z[i])): j = data.J[i][k] @@ -42,10 +43,10 @@ class TestSFMExample(GtsamTestCase): data.Z[i][k], measurementNoise, symbol('x', i), symbol('p', j), data.K)) - posePriorNoise = gtsam.noiseModel.Diagonal.Sigmas(poseNoiseSigmas) + posePriorNoise = Diagonal.Sigmas(poseNoiseSigmas) graph.add(gtsam.PriorFactorPose3(symbol('x', 0), truth.cameras[0].pose(), posePriorNoise)) - pointPriorNoise = gtsam.noiseModel.Isotropic.Sigma(3, pointNoiseSigma) + pointPriorNoise = Isotropic.Sigma(3, pointNoiseSigma) graph.add(gtsam.PriorFactorPoint3(symbol('p', 0), truth.points[0], pointPriorNoise)) diff --git a/python/gtsam_py/python/utils/visual_data_generator.py b/python/gtsam_py/python/utils/visual_data_generator.py index bc60dcd70..24404ca03 100644 --- a/python/gtsam_py/python/utils/visual_data_generator.py +++ b/python/gtsam_py/python/utils/visual_data_generator.py @@ -3,6 +3,7 @@ from __future__ import print_function import numpy as np from math import pi, cos, sin import gtsam +from gtsam import PinholeCameraCal3_S2 class Options: @@ -99,10 +100,10 @@ def generate_data(options): for i in range(options.nrCameras): theta = i * 2 * pi / options.nrCameras t = gtsam.Point3(r * cos(theta), r * sin(theta), height) - truth.cameras[i] = gtsam.SimpleCamera.Lookat(t, - gtsam.Point3(0, 0, 0), - gtsam.Point3(0, 0, 1), - truth.K) + truth.cameras[i] = PinholeCameraCal3_S2.Lookat(t, + gtsam.Point3(0, 0, 0), + gtsam.Point3(0, 0, 1), + truth.K) # Create measurements for j in range(nrPoints): # All landmarks seen in every frame