[SymForce] Move ComputeKeysToOptimize to factor.h

I want to reuse this function for the dense linearizer, so I need it in
some neutral ground outside of `linearizer.h`.

As an aside, I think it makes more sense for this function to live in
`factor.h` regardless, so I'm moving it there.

Topic: factor_out_common_linearizer_code
GitOrigin-RevId: 7af7b60c5f4db2bc71160b5c2e5b09dc6f3b346c

symforce/opt/factor.h

@@ -6,6 +6,7 @@
 #pragma once
 
 #include <ostream>
+#include <unordered_set>
 
 #include <Eigen/Sparse>
 
@@ -325,6 +326,38 @@ std::ostream& operator<<(std::ostream& os, const sym::linearized_dense_factorf_t
 std::ostream& operator<<(std::ostream& os, const sym::linearized_sparse_factor_t& factor);
 std::ostream& operator<<(std::ostream& os, const sym::linearized_sparse_factorf_t& factor);
 
+/**
+ * Compute the combined set of keys to optimize from the given factors. Order using the given
+ * comparison function.
+ */
+template <typename Scalar, typename Compare>
+std::vector<Key> ComputeKeysToOptimize(const std::vector<Factor<Scalar>>& factors,
+                                       Compare key_compare) {
+  // Some thoughts on efficiency at
+  // https://stackoverflow.com/questions/1041620/whats-the-most-efficient-way-to-erase-duplicates-and-sort-a-vector
+
+  // Aggregate uniques
+  std::unordered_set<Key> key_set;
+  for (const Factor<Scalar>& factor : factors) {
+    key_set.insert(factor.OptimizedKeys().begin(), factor.OptimizedKeys().end());
+  }
+
+  // Copy to vector
+  std::vector<Key> keys;
+  keys.insert(keys.end(), key_set.begin(), key_set.end());
+
+  // Order
+  std::sort(keys.begin(), keys.end(), key_compare);
+
+  return keys;
+}
+
+// If no comparator is specified, use sym::Key::LexicalLessThan.
+template <typename Scalar>
+std::vector<Key> ComputeKeysToOptimize(const std::vector<Factor<Scalar>>& factors) {
+  return ComputeKeysToOptimize(factors, &sym::Key::LexicalLessThan);
+}
+
 }  // namespace sym
 
 // Template method implementations

symforce/opt/linearizer.h

@@ -5,8 +5,6 @@
 
 #pragma once
 
-#include <unordered_set>
-
 #include <Eigen/Sparse>
 
 #include <lcmtypes/sym/linearization_dense_factor_helper_t.hpp>
@@ -175,38 +173,6 @@ Linearization<Scalar> Linearize(const std::vector<Factor<Scalar>>& factors,
   return linearization;
 }
 
-/**
- * Compute the combined set of keys to optimize from the given factors. Order using the given
- * comparison function.
- */
-template <typename Scalar, typename Compare>
-std::vector<Key> ComputeKeysToOptimize(const std::vector<Factor<Scalar>>& factors,
-                                       Compare key_compare) {
-  // Some thoughts on efficiency at
-  // https://stackoverflow.com/questions/1041620/whats-the-most-efficient-way-to-erase-duplicates-and-sort-a-vector
-
-  // Aggregate uniques
-  std::unordered_set<Key> key_set;
-  for (const Factor<Scalar>& factor : factors) {
-    key_set.insert(factor.OptimizedKeys().begin(), factor.OptimizedKeys().end());
-  }
-
-  // Copy to vector
-  std::vector<Key> keys;
-  keys.insert(keys.end(), key_set.begin(), key_set.end());
-
-  // Order
-  std::sort(keys.begin(), keys.end(), key_compare);
-
-  return keys;
-}
-
-// If no comparator is specified, use sym::Key::LexicalLessThan.
-template <typename Scalar>
-std::vector<Key> ComputeKeysToOptimize(const std::vector<Factor<Scalar>>& factors) {
-  return ComputeKeysToOptimize(factors, &sym::Key::LexicalLessThan);
-}
-
 }  // namespace sym
 
 #include "./linearizer.tcc"

symforce/opt/optimizer.h

@@ -7,6 +7,7 @@
 
 #include <sym/util/epsilon.h>
 
+#include "./factor.h"
 #include "./levenberg_marquardt_solver.h"
 #include "./linearizer.h"
 #include "./optimization_stats.h"