Merge pull request #103 from decargroup/add_batch_jacobian_fd

Add a function to compute the Jacobians of batch residuals via finite difference

navlie/batch/residuals.py

@@ -60,6 +60,47 @@ def evaluate(
         # corresponding state names than as a list.
         pass
 
+    def jacobian_fd(self, states: List[State], step_size=1e-6) -> List[np.ndarray]:
+        """
+        Calculates the model jacobian with finite difference.
+
+        Parameters
+        ----------
+        states : List[State]
+            Evaluation point of Jacobians, a list of states that 
+            the residual is a function of.
+        
+        Returns
+        -------
+        List[np.ndarray]
+            A list of Jacobians of the measurement model with respect to each of the input states.
+            For example, the first element of the return list is the Jacobian of the residual
+            w.r.t states[0], the second element is the Jacobian of the residual w.r.t states[1], etc.
+        """
+        jac_list: List[np.ndarray] = [None] * len(states)
+
+        # Compute the Jacobian for each state via finite difference
+        for state_num, X_bar in enumerate(states):
+            e_bar = self.evaluate(states)
+            size_error = e_bar.ravel().size
+            jac_fd = np.zeros((size_error, X_bar.dof))
+
+            for i in range(X_bar.dof):
+                dx = np.zeros((X_bar.dof, 1))
+                dx[i, 0] = step_size
+                X_temp = X_bar.plus(dx)
+                state_list_pert: List[State] = []
+                for state in states:
+                    state_list_pert.append(state.copy())
+
+                state_list_pert[state_num] = X_temp
+                e_temp = self.evaluate(state_list_pert)
+                jac_fd[:, i] = (e_temp - e_bar).flatten() / step_size
+
+            jac_list[state_num] = jac_fd
+
+        return jac_list
+
 
 class PriorResidual(Residual):
     """

tests/unit/test_utils.py

@@ -3,7 +3,7 @@
     GaussianResultList,
     schedule_sequential_measurements,
 )
-from navlie.types import StateWithCovariance
+from navlie.types import StateWithCovariance, Measurement
 from navlie.lib.states import (
     SE23State,
     SE3State,
@@ -13,10 +13,10 @@
 import numpy as np
 
 from navlie.utils import jacobian
+from navlie.batch.residuals import Residual, MeasurementResidual
 import numpy as np
 import pytest
-from navlie.lib.models import RangePoseToAnchor
-
+from navlie.lib.models import RangePoseToAnchor, GlobalPosition
 
 def test_gaussian_result_indexing():
     # Construct a dummy GaussianResultList
@@ -147,6 +147,26 @@ def test_meas_scheduling():
     assert new_freq == 10
     assert (np.array(offset_list) == np.array([0, 1, 2, 3, 4]) / freq).all()
 
+def test_residual_jacobian_fd():
+    # Test the finite difference for a measurement residual.
+
+    meas_model = GlobalPosition(np.identity(3))
+    measurement = Measurement(value=np.array([0.1, 0.2, 0.3]), stamp=0.0, model=meas_model)
+    residual = MeasurementResidual(["pose"], measurement)
+
+    # Create an SE(3) state
+    se3_state = SE3State(value=SE3.random(), direction="right")
+    jac_list = residual.jacobian_fd([se3_state])
+    jac_numerical = jac_list[0]
+
+    # Evaluate the Jacobian of the measurement model itself
+    jac_analytical = meas_model.jacobian(se3_state)
+
+    # For measurement residuals, the Jacobian of the residual should just be the 
+    # negative of the analytical measurement model Jacobian
+    assert (np.allclose(-jac_analytical, jac_numerical))
+
+
 
 if __name__ == "__main__":
     # just for debugging purposes