replaced SimpleCamera with PinholeCamera and updated tests

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
 
 

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))

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)

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()

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))
 

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,7 +100,7 @@ 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,
+        truth.cameras[i] = PinholeCameraCal3_S2.Lookat(t,
                                                        gtsam.Point3(0, 0, 0),
                                                        gtsam.Point3(0, 0, 1),
                                                        truth.K)