Implement angle range normalization correctly. Ignore anchor constraints in chi2 evaluation

Author: Pradeep Ranganathan

graph.py

@@ -5,7 +5,7 @@
 
 import pyximport; pyximport.install()
 from perfx import XYTConstraint_residual, XYTConstraint_jacobians
-from perfx import cycle_2PI_towards_zero
+from perfx import normalize_angle_range
 
 
 
@@ -103,7 +103,7 @@ def __init__(self, v, gaussian):
 
     def residual(self, aggregate_state=None):
         r = self._gaussian.mu - self._vx[0].state
-        r[2] = cycle_2PI_towards_zero(r[2])
+        r[2] = normalize_angle_range(r[2])
         return r
 
     def chi2(self):
@@ -118,11 +118,11 @@ def jacobian(self, roff=0, eps=1e-5):
         Compute the jacobian matrix of the residual error function
         evaluated at the current states of the connected vertices.
 
-        Returns a (dok format) sparse matrix since the jacobian of an
-        edge constraint is sparse. The `graph_state_length` parameter is
-        required to fix the column dimension of this sparse matrix.
-        Thus, the sparse matrix has `graph_state_length` columns and
-        `len(self.residual())` rows.
+        returns the sparse Jacobian matrix entries in triplet format
+        (i,j,v). The row index of the entries is offset by `roff`.
+
+        It is useful to specify `roff` when this Jacobian matrix is
+        computed as a sub-matrix of the graph Jacobian.
         """
         if self._jacobian_ijv_cache is None:
             J = -np.eye(3)
@@ -152,15 +152,19 @@ def __init__(self, vertices, edges):
             v._graph_state_ndx = i
 
     def anchor_first_vertex(self):
-        v0 = self.vertices[0]
+        if hasattr(self, '_anchor') == False:
+            v0 = self.vertices[0]
 
-        mu = v0.state.copy()
-        P = 1000. * np.eye(len(mu))
-        self._anchor = AnchorConstraint(v0, MultiVariateGaussian(mu, P))
+            mu = v0.state.copy()
+            P = 1000. * np.eye(len(mu))
+            self._anchor = AnchorConstraint(v0, MultiVariateGaussian(mu, P))
 
-        self.edges.append(self._anchor)
+            self.edges.append(self._anchor)
 
     def get_stats(self):
-        DOF = sum(e._DOF for e in self.edges) - len(self.state)
-        chi2 = sum(e.chi2() for e in self.edges)
+        original_edges = [ e for e in self.edges if e is not self._anchor ]
+
+        DOF = sum(e._DOF for e in original_edges) - len(self.state)
+        chi2 = sum(e.chi2() for e in original_edges)
+
         return GraphStats(chi2, chi2/DOF, DOF)

perfx.pyx

@@ -2,19 +2,16 @@ import numpy as np
 cimport numpy as np
 cimport cython
 
-from libc.math cimport sin, cos, fabs, M_PI
+from libc.math cimport sin, cos, fmod, M_PI
 
 
 
-cpdef inline double cycle_2PI_towards_zero(double t):
-    """
-    Return the closest to zero amongst `t`, `t+2*PI`, `t-2*PI`
-    """
-    cdef double _2PI = 2*M_PI
-    cdef double best
-    best = t if (fabs(t) < fabs(t + _2PI)) else (t + _2PI)
-    best = best if (fabs(best) < fabs(t - _2PI)) else (t - _2PI)
-    return best
+cpdef inline double normalize_angle_range(double t):
+    """ Normalize angle to be in [-PI, +PI] """
+    if t > 0:
+        return fmod(t+M_PI, 2.0*M_PI) - M_PI
+    else:
+        return fmod(t-M_PI, 2.0*M_PI) + M_PI
 
 
 @cython.boundscheck(False)
@@ -39,7 +36,7 @@ cpdef XYTConstraint_residual(
              observed[1] - ainvb[1],
              observed[2] - ainvb[2] ]
 
-    r[2] = cycle_2PI_towards_zero(r[2])
+    r[2] = normalize_angle_range(r[2])
 
     return np.array(r)
 

slam_solver.py

@@ -88,19 +88,19 @@ def _get_state_update(self):
 
 
     def solve(self, verbose=False, tol=1e-6, maxiter=1000, callback=None):
-        current_stats = None
+        current_stats = self._graph.get_stats()
 
         for iter_ in xrange(maxiter):
+            print '  chi2: %.6f    chi2 normalized: %.6f' % current_stats[:2]
+
             delta = self._get_state_update()
             self._graph.state -= delta
 
             new_stats = self._graph.get_stats()
-            if ( current_stats is not None and
-                 abs(new_stats.chi2_N - current_stats.chi2_N) < tol ):
+            if abs(new_stats.chi2_N - current_stats.chi2_N) < tol:
                 break
 
             current_stats = new_stats
-            print current_stats
 
 
 def main():
@@ -109,9 +109,6 @@ def main():
     g = load_graph(sys.argv[1] if len(sys.argv) > 1 else 'datasets/MITb.g2o')
     print 'graph has %d vertices, %d edges' % ( len(g.vertices), len(g.edges) )
 
-    # from mmath import xyt_inv_mult
-    # print [ xyt_inv_mult(e._vx[0].state, e._vx[1].state) for e in g.edges ]
-
     g.anchor_first_vertex()
 
     solver = SparseCholeskySolver(g)