From b010a240f35f42843463c173bb34e798e0cbebfd Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 15 Jan 2023 00:59:34 -0500
Subject: [PATCH] STL-based efficient transformation

---
 gtsam/hybrid/HybridFactorGraph.cpp | 7 ++++---
 gtsam/hybrid/HybridFactorGraph.h   | 2 +-
 2 files changed, 5 insertions(+), 4 deletions(-)

diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp
index 2224c4a38..2d46ae201 100644
--- a/gtsam/hybrid/HybridFactorGraph.cpp
+++ b/gtsam/hybrid/HybridFactorGraph.cpp
@@ -45,9 +45,10 @@ DiscreteKeys HybridFactorGraph::discreteKeys() const {
 /* ************************************************************************* */
 KeySet HybridFactorGraph::discreteKeySet() const {
   KeySet keys;
-  for (const DiscreteKey& k : discreteKeys()) {
-    keys.insert(k.first);
-  }
+  DiscreteKeys key_vector = discreteKeys();
+  std::transform(key_vector.begin(), key_vector.end(),
+                 std::inserter(keys, keys.begin()),
+                 [](const DiscreteKey& k) { return k.first; });
   return keys;
 }
 
diff --git a/gtsam/hybrid/HybridFactorGraph.h b/gtsam/hybrid/HybridFactorGraph.h
index a02d4a212..7d30663a3 100644
--- a/gtsam/hybrid/HybridFactorGraph.h
+++ b/gtsam/hybrid/HybridFactorGraph.h
@@ -65,7 +65,7 @@ class HybridFactorGraph : public FactorGraph<Factor> {
   /// @{
 
   /// Get all the discrete keys in the factor graph.
-  std::set<DiscreteKey> discreteKeys() const;
+  DiscreteKeys discreteKeys() const;
 
   /// Get all the discrete keys in the factor graph, as a set.
   KeySet discreteKeySet() const;