get keys from NonlinearFactors as well

borglab · Sep 28, 2024 · b2c73af · b2c73af
1 parent 6d57055
commit b2c73af
Showing 1 changed file with 3 additions and 0 deletions.
diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp
@@ -18,6 +18,7 @@
  */
 
 #include <gtsam/hybrid/HybridFactorGraph.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
 
 namespace gtsam {
 
@@ -58,6 +59,8 @@ const KeySet HybridFactorGraph::continuousKeySet() const {
       }
     } else if (auto p = std::dynamic_pointer_cast<GaussianFactor>(factor)) {
       keys.insert(p->keys().begin(), p->keys().end());
+    } else if (auto p = std::dynamic_pointer_cast<NonlinearFactor>(factor)) {
+      keys.insert(p->keys().begin(), p->keys().end());
     }
   }
   return keys;