diff --git a/navlie/batch/residuals.py b/navlie/batch/residuals.py index ba2540a7..e7747ded 100644 --- a/navlie/batch/residuals.py +++ b/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): """ diff --git a/tests/unit/test_utils.py b/tests/unit/test_utils.py index 81781709..65b6ab5b 100644 --- a/tests/unit/test_utils.py +++ b/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