From bec4afa3f8543525b0532dbe8c7c285a158516cb Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Thu, 24 Oct 2024 13:09:58 -0700
Subject: [PATCH] EdgeKey for view graphs etc...

---
 gtsam/inference/EdgeKey.cpp           | 33 ++++++++++++
 gtsam/inference/EdgeKey.h             | 74 +++++++++++++++++++++++++++
 gtsam/inference/tests/testEdgeKey.cpp | 57 +++++++++++++++++++++
 3 files changed, 164 insertions(+)
 create mode 100644 gtsam/inference/EdgeKey.cpp
 create mode 100644 gtsam/inference/EdgeKey.h
 create mode 100644 gtsam/inference/tests/testEdgeKey.cpp

diff --git a/gtsam/inference/EdgeKey.cpp b/gtsam/inference/EdgeKey.cpp
new file mode 100644
index 000000000..d44f7a2a2
--- /dev/null
+++ b/gtsam/inference/EdgeKey.cpp
@@ -0,0 +1,33 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file EdgeKey.cpp
+ * @date Oct 24, 2024
+ * @author: Frank Dellaert
+ * @author: Akshay Krishnan
+ */
+
+#include <gtsam/inference/EdgeKey.h>
+
+namespace gtsam {
+
+// Output stream operator implementation
+GTSAM_EXPORT std::ostream& operator<<(std::ostream& os, const EdgeKey& key) {
+  os << "{" << key.i() << ", " << key.j() << "}";
+  return os;
+}
+
+void EdgeKey::print(const std::string& s) const {
+  std::cout << s << *this << std::endl;
+}
+
+}  // namespace gtsam
diff --git a/gtsam/inference/EdgeKey.h b/gtsam/inference/EdgeKey.h
new file mode 100644
index 000000000..13de55f9e
--- /dev/null
+++ b/gtsam/inference/EdgeKey.h
@@ -0,0 +1,74 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+#pragma once
+
+/**
+ * @file EdgeKey.h
+ * @date Oct 24, 2024
+ * @author: Frank Dellaert
+ * @author: Akshay Krishnan
+ */
+
+#include <gtsam/base/Testable.h>
+#include <gtsam/inference/Key.h>
+
+namespace gtsam {
+class GTSAM_EXPORT EdgeKey {
+ protected:
+  std::uint32_t i_;  ///< Upper 32 bits
+  std::uint32_t j_;  ///< Lower 32 bits
+
+ public:
+  /// @name Constructors
+  /// @{
+
+  /// Default constructor
+  EdgeKey() : i_(0), j_(0) {}
+
+  /// Constructor
+  EdgeKey(std::uint32_t i, std::uint32_t j) : i_(i), j_(j) {}
+
+  /// @}
+  /// @name API
+  /// @{
+
+  /// Cast to Key
+  operator Key() const { return ((std::uint64_t)i_ << 32) | j_; }
+
+  /// Retrieve high 32 bits
+  inline std::uint32_t i() const { return i_; }
+
+  /// Retrieve low 32 bits
+  inline std::uint32_t j() const { return j_; }
+
+  /// Output stream operator
+  friend GTSAM_EXPORT std::ostream& operator<<(std::ostream&, const EdgeKey&);
+
+  /// @}
+  /// @name Testable
+  /// @{
+
+  /// Prints the EdgeKey with an optional prefix string.
+  void print(const std::string& s = "") const;
+
+  /// Checks if this EdgeKey is equal to another, tolerance is ignored.
+  bool equals(const EdgeKey& expected, double tol = 0.0) const {
+    return (*this) == expected;
+  }
+  /// @}
+};
+
+/// traits
+template <>
+struct traits<EdgeKey> : public Testable<EdgeKey> {};
+
+}  // namespace gtsam
diff --git a/gtsam/inference/tests/testEdgeKey.cpp b/gtsam/inference/tests/testEdgeKey.cpp
new file mode 100644
index 000000000..88fafbdd1
--- /dev/null
+++ b/gtsam/inference/tests/testEdgeKey.cpp
@@ -0,0 +1,57 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/*
+ * @file testEdgeKey.cpp
+ * @date Oct 24, 2024
+ * @author: Frank Dellaert
+ * @author: Akshay Krishnan
+ */
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/inference/EdgeKey.h>
+
+#include <sstream>
+
+using namespace std;
+using namespace gtsam;
+
+/* ************************************************************************* */
+TEST(EdgeKey, Construction) {
+  EdgeKey edge(1, 2);
+  EXPECT(edge.i() == 1);
+  EXPECT(edge.j() == 2);
+}
+
+/* ************************************************************************* */
+TEST(EdgeKey, Equality) {
+  EdgeKey edge1(1, 2);
+  EdgeKey edge2(1, 2);
+  EdgeKey edge3(2, 3);
+
+  EXPECT(assert_equal(edge1, edge2));
+  EXPECT(!edge1.equals(edge3));
+}
+
+/* ************************************************************************* */
+TEST(EdgeKey, StreamOutput) {
+  EdgeKey edge(1, 2);
+  std::ostringstream oss;
+  oss << edge;
+  EXPECT("{1, 2}" == oss.str());
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */