From f8cad34bf912269b1bfc54db4ef08a1da65f1f2d Mon Sep 17 00:00:00 2001
From: Bradley Solliday <bradley.solliday@skydio.com>
Date: Wed, 5 Apr 2023 04:53:08 +0000
Subject: [PATCH] [SymForce] Add helpful err msg if unopt'zed factor

As demonstrated in https://github.com/symforce-org/symforce/issues/318
the AssertionError that's raised if the user attempts to create an
optimizer with a factor that doesn't touch any optimized keys isn't very
helpful.

Adds a test that the intersection of the factor keys and the keys to
optimize is not empty and raises a ValueError with extra information if
it is (the old `AssertionError` would have been raised otherwise).

Topic: err_msg_if_py_factor_unoptimized
GitOrigin-RevId: bed40079b586a82a2c3e7072cf7c5a8bdf91ceb3
---
 symforce/opt/optimizer.py          |  5 +++++
 test/symforce_py_optimizer_test.py | 21 +++++++++++++++++++++
 2 files changed, 26 insertions(+)

diff --git a/symforce/opt/optimizer.py b/symforce/opt/optimizer.py
index 05603ce60..3d956a830 100644
--- a/symforce/opt/optimizer.py
+++ b/symforce/opt/optimizer.py
@@ -219,6 +219,11 @@ def __init__(
                 # We compute the linearization in the same order as `optimized_keys`
                 # so that e.g. columns of the generated jacobians are in the same order
                 factor_opt_keys = [opt_key for opt_key in optimized_keys if opt_key in factor.keys]
+                if not factor_opt_keys:
+                    raise ValueError(
+                        f"Factor {factor.name} has no arguments (keys: {factor.keys}) in "
+                        + f"optimized_keys ({optimized_keys})."
+                    )
                 numeric_factors.append(factor.to_numeric_factor(factor_opt_keys))
             else:
                 # Add unique keys to optimized keys
diff --git a/test/symforce_py_optimizer_test.py b/test/symforce_py_optimizer_test.py
index 596524dcf..721be57a8 100644
--- a/test/symforce_py_optimizer_test.py
+++ b/test/symforce_py_optimizer_test.py
@@ -13,6 +13,7 @@
 
 import symforce.symbolic as sf
 from symforce import logger
+from symforce import typing as T
 from symforce.opt.factor import Factor
 from symforce.opt.optimizer import Optimizer
 from symforce.test_util import TestCase
@@ -97,6 +98,26 @@ def prior_residual(x: sf.Rot3, epsilon: sf.Scalar, x_prior: sf.Rot3) -> sf.V3:
         index_entry2 = optimizer.linearization_index_entry("x1")
         self.assertEqual(index_entry, index_entry2)
 
+    def test_unoptimized_factor_exception(self) -> None:
+        """
+        Tests that a ValueError is raised if none of the factor keys match the optimizer keys.
+        """
+
+        def residual(x: T.Scalar) -> sf.V1:
+            return sf.V1((x - 2.71828) ** 2)
+
+        factor1 = Factor(["present_key"], residual)
+        factor2 = Factor(["absent_key"], residual)
+
+        optimized_keys = ["present_key", "other_present_key"]
+        with self.assertRaises(ValueError):
+            try:
+                Optimizer([factor1, factor2], optimized_keys)
+            except ValueError as err:
+                self.assertIn(str(factor2.keys), str(err))
+                self.assertIn(str(optimized_keys), str(err))
+                raise err from None
+
 
 if __name__ == "__main__":
     TestCase.main()