From 6b5b098ceb5d8326477cf9fdae9ee47dbb9d3f3c Mon Sep 17 00:00:00 2001
From: Rainer Kuemmerle <rainer.kuemmerle@gmail.com>
Date: Sat, 25 Jan 2025 12:40:30 +0100
Subject: [PATCH] Use call_once to initialize threading

---
 g2o/core/optimizable_graph.cpp         | 13 ++++++++++---
 unit_test/general/graph_operations.cpp |  7 +++++++
 2 files changed, 17 insertions(+), 3 deletions(-)

diff --git a/g2o/core/optimizable_graph.cpp b/g2o/core/optimizable_graph.cpp
index 999701ad5..491165fe7 100644
--- a/g2o/core/optimizable_graph.cpp
+++ b/g2o/core/optimizable_graph.cpp
@@ -33,6 +33,7 @@
 #include <iostream>
 #include <iterator>
 #include <memory>
+#include <mutex>
 #include <sstream>
 #include <unordered_set>
 #include <utility>
@@ -57,6 +58,14 @@
 namespace g2o {
 
 namespace {
+std::once_flag calledMultiThreadInit;
+
+void initThreading() {
+#if (defined G2O_OPENMP) && EIGEN_VERSION_AT_LEAST(3, 1, 0)
+  Eigen::initParallel();
+#endif
+}
+
 std::shared_ptr<OptimizableGraph::Vertex> kNonExistantVertex(nullptr);
 
 void saveUserData(AbstractGraph::AbstractGraphElement& graph_element,
@@ -791,9 +800,7 @@ void OptimizableGraph::addGraph(OptimizableGraph& other) {
 }
 
 bool OptimizableGraph::initMultiThreading() {
-#if (defined G2O_OPENMP) && EIGEN_VERSION_AT_LEAST(3, 1, 0)
-  Eigen::initParallel();
-#endif
+  std::call_once(calledMultiThreadInit, initThreading);
   return true;
 }
 
diff --git a/unit_test/general/graph_operations.cpp b/unit_test/general/graph_operations.cpp
index 7edd55e83..c7f417cce 100644
--- a/unit_test/general/graph_operations.cpp
+++ b/unit_test/general/graph_operations.cpp
@@ -69,8 +69,15 @@ class JacobianWorkspaceTestAdapter : public g2o::JacobianWorkspace {
 };
 }  // namespace
 
+TEST(General, InitMultiThreading) {
+  bool init = false;
+  EXPECT_NO_THROW({ init = g2o::OptimizableGraph::initMultiThreading(); });
+  EXPECT_TRUE(init);
+}
+
 TEST(General, BinaryEdgeConstructor) {
   g2o::EdgeSE2 e2;
+  ASSERT_THAT(e2.vertices(), SizeIs(2));
   ASSERT_EQ(nullptr, e2.vertices()[0]);
   ASSERT_EQ(nullptr, e2.vertices()[1]);
 }