From a149a2f727903ebf767f803c32b0488bca4e5270 Mon Sep 17 00:00:00 2001
From: Rainer Kuemmerle <rainer.kuemmerle@gmail.com>
Date: Sun, 21 Jul 2024 12:15:18 +0200
Subject: [PATCH] Add test for index mapping

---
 .../optimization_algorithm_with_hessian.cpp   |  6 +-
 g2o/core/sparse_optimizer.cpp                 | 24 ++++----
 unit_test/general/graph_operations.cpp        | 60 +++++++++++++++++++
 3 files changed, 75 insertions(+), 15 deletions(-)

diff --git a/g2o/core/optimization_algorithm_with_hessian.cpp b/g2o/core/optimization_algorithm_with_hessian.cpp
index 632e6eec8..911da4be7 100644
--- a/g2o/core/optimization_algorithm_with_hessian.cpp
+++ b/g2o/core/optimization_algorithm_with_hessian.cpp
@@ -52,11 +52,7 @@ bool OptimizationAlgorithmWithHessian::init(bool online) {
       break;
     }
   }
-  if (useSchur) {
-    if (solver_.supportsSchur()) solver_.setSchur(true);
-  } else {
-    if (solver_.supportsSchur()) solver_.setSchur(false);
-  }
+  if (solver_.supportsSchur()) solver_.setSchur(useSchur);
 
   const bool initState = solver_.init(optimizer_, online);
   return initState;
diff --git a/g2o/core/sparse_optimizer.cpp b/g2o/core/sparse_optimizer.cpp
index 9ef05e59c..6a5c4440e 100644
--- a/g2o/core/sparse_optimizer.cpp
+++ b/g2o/core/sparse_optimizer.cpp
@@ -33,6 +33,7 @@
 #include <iostream>
 #include <set>
 #include <string_view>
+#include <unordered_set>
 #include <utility>
 
 #include "batch_stats.h"
@@ -173,19 +174,22 @@ bool SparseOptimizer::buildIndexMapping(
 
   ivMap_.resize(vlist.size());
   size_t i = 0;
-  for (int k = 0; k < 2; k++)
+  for (int k = 0; k < 2; k++) {
+    const bool expected_marginalized = k != 0;
     for (auto& it : vlist) {
       OptimizableGraph::Vertex* v = it.get();
-      if (!v->fixed()) {
-        if (static_cast<int>(v->marginalized()) == k) {
-          v->setHessianIndex(i);
-          ivMap_[i] = v;
-          i++;
-        }
-      } else {
+      if (v->fixed()) {
         v->setHessianIndex(-1);
+        continue;
+      }
+      if (v->marginalized() != expected_marginalized) {
+        continue;
       }
+      v->setHessianIndex(i);
+      ivMap_[i] = v;
+      ++i;
     }
+  }
   ivMap_.resize(i);
   return true;
 }
@@ -261,7 +265,7 @@ bool SparseOptimizer::initializeOptimization(HyperGraph::VertexSet& vset,
 
   activeEdges_.reserve(auxEdgeSet.size());
   for (const auto& it : auxEdgeSet)
-    activeEdges_.push_back(std::static_pointer_cast<Edge>(it));
+    activeEdges_.emplace_back(std::static_pointer_cast<Edge>(it));
 
   sortVectorContainers();
   bool indexMappingStatus = buildIndexMapping(activeVertices_);
@@ -306,7 +310,7 @@ void SparseOptimizer::computeInitialGuess() {
 
 void SparseOptimizer::computeInitialGuess(EstimatePropagatorCost& propagator) {
   OptimizableGraph::VertexSet emptySet;
-  std::set<Vertex*> backupVertices;
+  std::unordered_set<Vertex*> backupVertices;
   OptimizableGraph::VertexSet fixedVertices;  // these are the root nodes where
                                               // to start the initialization
   for (auto& e : activeEdges_) {
diff --git a/unit_test/general/graph_operations.cpp b/unit_test/general/graph_operations.cpp
index 18433a60d..50773292c 100644
--- a/unit_test/general/graph_operations.cpp
+++ b/unit_test/general/graph_operations.cpp
@@ -44,6 +44,7 @@
 #include "g2o/core/optimization_algorithm_property.h"
 #include "g2o/core/sparse_optimizer.h"
 #include "g2o/stuff/string_tools.h"
+#include "g2o/types/slam2d/edge_pointxy.h"
 #include "g2o/types/slam2d/edge_se2.h"
 #include "g2o/types/slam2d/vertex_point_xy.h"
 #include "g2o/types/slam2d/vertex_se2.h"
@@ -135,6 +136,65 @@ TEST(General, GraphAddEdge) {
   ASSERT_TRUE(optimizer->edges().empty());
 }
 
+TEST(General, GraphIndexMapping) {
+  auto optimizer = g2o::internal::createOptimizerForTests();
+
+  int id = 0;
+  auto p1 = std::make_shared<g2o::VertexPointXY>();
+  p1->setId(id++);
+  p1->setMarginalized(true);
+
+  auto v0 = std::make_shared<g2o::VertexSE2>();
+  v0->setId(id++);
+  v0->setFixed(true);
+  auto v1 = std::make_shared<g2o::VertexSE2>();
+  v1->setId(id++);
+  auto v2 = std::make_shared<g2o::VertexSE2>();
+  v2->setId(id++);
+
+  ASSERT_TRUE(optimizer->addVertex(p1));
+  ASSERT_TRUE(optimizer->addVertex(v1));
+  ASSERT_TRUE(optimizer->addVertex(v2));
+
+  auto e0 = std::make_shared<g2o::EdgeSE2>();
+  e0->setVertex(0, v0);
+  e0->setVertex(1, v1);
+  ASSERT_TRUE(optimizer->addEdge(e0));
+
+  auto e1 = std::make_shared<g2o::EdgeSE2>();
+  e1->setVertex(0, v1);
+  e1->setVertex(1, v2);
+  ASSERT_TRUE(optimizer->addEdge(e1));
+
+  auto e2 = std::make_shared<g2o::EdgePointXY>();
+  e2->setVertex(0, v1);
+  e2->setVertex(1, p1);
+  ASSERT_TRUE(optimizer->addEdge(e2));
+
+  optimizer->initializeOptimization();
+
+  // Check Hessian index
+  EXPECT_NE(p1->hessianIndex(), -1);
+  EXPECT_EQ(v0->hessianIndex(), -1);
+  EXPECT_NE(v1->hessianIndex(), -1);
+  EXPECT_NE(v2->hessianIndex(), -1);
+  EXPECT_LE(v1->hessianIndex(), p1->hessianIndex());
+  EXPECT_LE(v2->hessianIndex(), p1->hessianIndex());
+
+  // Check order in index mapping
+  ASSERT_THAT(optimizer->indexMapping(),
+              UnorderedElementsAre(p1.get(), v1.get(), v2.get()));
+  auto findVertex = [&optimizer](g2o::OptimizableGraph::Vertex* vertex) {
+    return std::find(optimizer->indexMapping().begin(),
+                     optimizer->indexMapping().end(), vertex);
+  };
+  auto p1_it = findVertex(p1.get());
+  auto v1_it = findVertex(v1.get());
+  auto v2_it = findVertex(v2.get());
+  EXPECT_LE(v1_it, p1_it);
+  EXPECT_LE(v2_it, p1_it);
+}
+
 TEST(General, GraphAddVertexAndClear) {
   auto optimizer = g2o::internal::createOptimizerForTests();