Enable fixing vertices (#25)

* fix poses option

control which poses to hold fixed

* fix vertex

* Remove unused `dim` variable

* Enable fixing vertices

* Add tests

* Fix the vertices

Co-authored-by: Jenia Golbstein <jenia@novellusdx.com>

graphslam/graph.py

@@ -163,6 +163,8 @@ class Graph(object):
         The current :math:`\chi^2` error, or ``None`` if it has not yet been computed
     _edges : list[graphslam.edge.base_edge.BaseEdge]
         A list of the edges (i.e., constraints) in the graph
+    _fixed_vertices : set[int]
+        The set of vertices that are fixed
     _gradient : numpy.ndarray, None
         The gradient :math:`\mathbf{b}` of the :math:`\chi^2` error, or ``None`` if it has not yet been computed
     _hessian : scipy.sparse.lil_matrix, None
@@ -175,6 +177,7 @@ def __init__(self, edges, vertices):
         # The vertices and edges lists
         self._edges = edges
         self._vertices = vertices
+        self._fixed_vertices = set()
 
         # The chi^2 error, gradient, and Hessian
         self._chi2 = None
@@ -222,11 +225,19 @@ def _calc_chi2_gradient_hessian(self):
         # Fill in the gradient vector
         self._gradient = np.zeros(n * dim, dtype=np.float64)
         for idx, contrib in chi2_gradient_hessian.gradient.items():
-            self._gradient[idx * dim: (idx + 1) * dim] += contrib
+            # If a vertex is fixed, its block in the gradient vector is zero and so there is nothing to do
+            if idx not in self._fixed_vertices:
+                self._gradient[idx * dim: (idx + 1) * dim] += contrib
 
         # Fill in the Hessian matrix
         self._hessian = lil_matrix((n * dim, n * dim), dtype=np.float64)
         for (row_idx, col_idx), contrib in chi2_gradient_hessian.hessian.items():
+            if row_idx in self._fixed_vertices or col_idx in self._fixed_vertices:
+                # For fixed vertices, the diagonal block is the identity matrix and the off-diagonal blocks are zero
+                if row_idx == col_idx:
+                    self._hessian[row_idx * dim: (row_idx + 1) * dim, col_idx * dim: (col_idx + 1) * dim] = np.eye(dim)
+                continue
+
             self._hessian[row_idx * dim: (row_idx + 1) * dim, col_idx * dim: (col_idx + 1) * dim] = contrib
 
             if row_idx != col_idx:
@@ -246,7 +257,12 @@ def optimize(self, tol=1e-4, max_iter=20, fix_first_pose=True):
 
         """
         n = len(self._vertices)
-        dim = len(self._vertices[0].pose.to_compact())
+
+        if fix_first_pose:
+            self._vertices[0].fixed = True
+
+        # Populate the set of fixed vertices
+        self._fixed_vertices = {i for i, v in enumerate(self._vertices) if v.fixed}
 
         # Previous iteration's chi^2 error
         chi2_prev = -1.
@@ -270,13 +286,6 @@ def optimize(self, tol=1e-4, max_iter=20, fix_first_pose=True):
             # Update the previous iteration's chi^2 error
             chi2_prev = self._chi2
 
-            # Hold the first pose fixed
-            if fix_first_pose:
-                self._hessian[:dim, :] = 0.
-                self._hessian[:, :dim] = 0.
-                self._hessian[:dim, :dim] += np.eye(dim)
-                self._gradient[:dim] = 0.
-
             # Solve for the updates
             dx = spsolve(self._hessian, -self._gradient)  # pylint: disable=invalid-unary-operand-type
 

graphslam/vertex.py

@@ -27,6 +27,8 @@ class Vertex:
         The pose associated with the vertex
     vertex_index : int, None
         The vertex's index in the graph's ``vertices`` list
+    fixed : bool
+        Whether this vertex should be fixed
 
     Attributes
     ----------
@@ -36,12 +38,15 @@ class Vertex:
         The vertex's index in the graph's ``vertices`` list
     pose : graphslam.pose.base_pose.BasePose
         The pose associated with the vertex
+    fixed : bool
+        Whether this vertex should be fixed
 
     """
-    def __init__(self, vertex_id, pose, vertex_index=None):
+    def __init__(self, vertex_id, pose, vertex_index=None, fixed=False):
         self.id = vertex_id
         self.pose = pose
         self.index = vertex_index
+        self.fixed = fixed
 
     def to_g2o(self):
         """Export the vertex to the .g2o format.

tests/test_graph.py

@@ -65,6 +65,54 @@ def test_optimize(self):
         # Make sure the first pose was held fixed
         self.assertAlmostEqual(np.linalg.norm(p0 - self.g._vertices[0].pose.to_array()), 0.)  # pylint: disable=protected-access
 
+    def _test_optimize_fixed_vertices(self, fixed_indices):
+        """Test that a ``Graph`` can be optimized with vertices held fixed.
+
+        """
+        chi2_orig = self.g.calc_chi2()
+
+        poses_before = [self.g._vertices[i].pose.to_array() for i in fixed_indices]  # pylint: disable=protected-access
+        for i in fixed_indices:
+            self.g._vertices[i].fixed = True  # pylint: disable=protected-access
+
+        self.g.optimize(fix_first_pose=False)
+        self.assertLess(self.g.calc_chi2(), chi2_orig)
+
+        # Make sure the poses were held fixed
+        poses_after = [self.g._vertices[i].pose.to_array() for i in fixed_indices]  # pylint: disable=protected-access
+        for before, after in zip(poses_before, poses_after):
+            self.assertAlmostEqual(np.linalg.norm(before - after), 0.)
+
+    def test_optimize_fix_1(self):
+        """Test that the ``optimize`` method works correctly when fixing vertex 1.
+
+        """
+        self._test_optimize_fixed_vertices([1])
+
+    def test_optimize_fix_2(self):
+        """Test that the ``optimize`` method works correctly when fixing vertex 2.
+
+        """
+        self._test_optimize_fixed_vertices([2])
+
+    def test_optimize_fix_01(self):
+        """Test that the ``optimize`` method works correctly when fixing vertices 0 and 1.
+
+        """
+        self._test_optimize_fixed_vertices([0, 1])
+
+    def test_optimize_fix_02(self):
+        """Test that the ``optimize`` method works correctly when fixing vertices 0 and 2.
+
+        """
+        self._test_optimize_fixed_vertices([0, 2])
+
+    def test_optimize_fix_12(self):
+        """Test that the ``optimize`` method works correctly when fixing vertices 1 and 2.
+
+        """
+        self._test_optimize_fixed_vertices([1, 2])
+
     # pylint: disable=protected-access
     def test_to_g2o(self):
         """Test that the ``to_g2o`` method is implemented correctly, or raises ``NotImplementedError``.