Added python example and test

cython/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py

@@ -0,0 +1,35 @@
+"""
+GTSAM Copyright 2010-2018, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+See LICENSE for the license information
+
+Initialize PoseSLAM with Chordal init
+Author: Luca Carlone, Frank Dellaert (python port)
+"""
+# pylint: disable=invalid-name, E1101
+
+from __future__ import print_function
+
+import numpy as np
+
+import gtsam
+
+# Read graph from file
+g2oFile = gtsam.findExampleDataFile("pose3example.txt")
+
+is3D = True
+graph, initial = gtsam.readG2o(g2oFile, is3D)
+
+# Add prior on the first key. TODO: assumes first key ios z
+priorModel = gtsam.noiseModel_Diagonal.Variances(
+    np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4]))
+firstKey = initial.keys().at(0)
+graph.add(gtsam.PriorFactorPose3(0, gtsam.Pose3(), priorModel))
+
+# Initializing Pose3 - chordal relaxation"
+initialization = gtsam.InitializePose3.initialize(graph)
+
+print(initialization)

cython/gtsam/tests/test_initialize_pose3.py

@@ -0,0 +1,88 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Unit tests for 3D SLAM initialization, using rotation relaxation.
+Author: Luca Carlone and Frank Dellaert (Python)
+"""
+# pylint: disable=invalid-name, E1101, E0611
+import unittest
+
+import numpy as np
+
+import gtsam
+from gtsam import NonlinearFactorGraph, Point3, Pose3, Rot3, Values
+
+x0, x1, x2, x3 = 0, 1, 2, 3
+
+
+class TestValues(unittest.TestCase):
+
+    def setUp(self):
+
+        model = gtsam.noiseModel_Isotropic.Sigma(6, 0.1)
+
+        # We consider a small graph:
+        #                            symbolic FG
+        #               x2               0  1
+        #             / | \              1  2
+        #            /  |  \             2  3
+        #          x3   |   x1           2  0
+        #           \   |   /            0  3
+        #            \  |  /
+        #               x0
+        #
+        p0 = Point3(0, 0, 0)
+        self.R0 = Rot3.Expmap(np.array([0.0, 0.0, 0.0]))
+        p1 = Point3(1, 2, 0)
+        self.R1 = Rot3.Expmap(np.array([0.0, 0.0, 1.570796]))
+        p2 = Point3(0, 2, 0)
+        self.R2 = Rot3.Expmap(np.array([0.0, 0.0, 3.141593]))
+        p3 = Point3(-1, 1, 0)
+        self.R3 = Rot3.Expmap(np.array([0.0, 0.0, 4.712389]))
+
+        pose0 = Pose3(self.R0, p0)
+        pose1 = Pose3(self.R1, p1)
+        pose2 = Pose3(self.R2, p2)
+        pose3 = Pose3(self.R3, p3)
+
+        g = NonlinearFactorGraph()
+        g.add(gtsam.BetweenFactorPose3(x0, x1, pose0.between(pose1), model))
+        g.add(gtsam.BetweenFactorPose3(x1, x2, pose1.between(pose2), model))
+        g.add(gtsam.BetweenFactorPose3(x2, x3, pose2.between(pose3), model))
+        g.add(gtsam.BetweenFactorPose3(x2, x0, pose2.between(pose0), model))
+        g.add(gtsam.BetweenFactorPose3(x0, x3, pose0.between(pose3), model))
+        g.add(gtsam.PriorFactorPose3(x0, pose0, model))
+        self.graph = g
+
+    def test_buildPose3graph(self):
+        pose3graph = gtsam.InitializePose3.buildPose3graph(self.graph)
+
+    def test_orientations(self):
+        pose3Graph = gtsam.InitializePose3.buildPose3graph(self.graph)
+
+        initial = gtsam.InitializePose3.computeOrientationsChordal(pose3Graph)
+
+        # comparison is up to M_PI, that's why we add some multiples of 2*M_PI
+        self.assertTrue(initial.atRot3(x0).equals(self.R0, 1e-6))
+        self.assertTrue(initial.atRot3(x1).equals(self.R1, 1e-6))
+        self.assertTrue(initial.atRot3(x2).equals(self.R2, 1e-6))
+        self.assertTrue(initial.atRot3(x3).equals(self.R3, 1e-6))
+
+    def test_initializePoses(self):
+        g2oFile = gtsam.findExampleDataFile("pose3example-grid")
+        is3D = True
+        inputGraph, expectedValues = gtsam.readG2o(g2oFile, is3D)
+        priorModel = gtsam.noiseModel_Unit.Create(6)
+        inputGraph.add(gtsam.PriorFactorPose3(0, Pose3(), priorModel))
+
+        initial = gtsam.InitializePose3.initialize(inputGraph)
+        # TODO(frank): very loose !!
+        self.assertTrue(initial.equals(expectedValues, 0.1))
+
+
+if __name__ == "__main__":
+    unittest.main()