symforce-org / symforce

Fast symbolic computation, code generation, and nonlinear optimization for robotics
https://symforce.org
Apache License 2.0
1.41k stars 145 forks source link

Factor Cache broken for custom jacobians #234

Open aaron-skydio opened 2 years ago

aaron-skydio commented 2 years ago

Describe the bug The generated factor cache does not store the jacobians, because they're assumed to be a deterministic function of the residual expressions and the arguments being differentiated with respect to. This is not the case if you have custom jacobians.

To Reproduce This test fails (added in symforce_py_factor_test.py):

        inputs = Values(a=sf.V3.zero(), b=sf.V3.zero())
        optimized_keys = ["a"]

        def between(a: sf.V3, b: sf.V3) -> sf.V3:
            return a - b

        numeric_factor = Factor(
            keys=inputs.keys_recursive(),
            residual=between,
        ).to_numeric_factor(optimized_keys=optimized_keys)

        residual, jacobian, _, _ = numeric_factor.linearize(inputs)
        self.assertStorageNear(residual, np.zeros((3,)))
        self.assertStorageNear(jacobian, np.eye(3))

        numeric_factor = Factor(
            keys=inputs.keys_recursive(),
            residual=between,
            custom_jacobian_func=lambda args: sf.M33(),
        ).to_numeric_factor(optimized_keys=optimized_keys)

        residual, jacobian, _, _ = numeric_factor.linearize(inputs)
        self.assertStorageNear(residual, np.zeros((3,)))
        self.assertStorageNear(jacobian, np.zeros((3, 3)))

The test passes if a Factor._generated_residual_cache = GeneratedResidualCache() is added before creating the second factor.

Expected behavior The above test should pass