Change dimensions to unordered_set

g2o/apps/g2o_cli/g2o.cpp

@@ -300,7 +300,7 @@ int main(int argc, char** argv) {
     return 1;
   }
 
-  std::set<int> vertexDimensions = optimizer.dimensions();
+  std::unordered_set<int> vertexDimensions = optimizer.dimensions();
   if (!optimizer.isSolverSuitable(solverProperty, vertexDimensions)) {
     cerr << "The selected solver is not suitable for optimizing the given "
             "graph\n";
@@ -346,8 +346,10 @@ int main(int argc, char** argv) {
 
   // if schur, we wanna marginalize the landmarks...
   if (marginalize || solverProperty.requiresMarginalize) {
-    int maxDim = *vertexDimensions.rbegin();
-    int minDim = *vertexDimensions.begin();
+    const int maxDim =
+        *std::max_element(vertexDimensions.begin(), vertexDimensions.end());
+    const int minDim =
+        *std::min_element(vertexDimensions.begin(), vertexDimensions.end());
     if (maxDim != minDim) {
       cerr << "# Preparing Marginalization of the Landmarks ... ";
       for (auto& it : optimizer.vertices()) {
@@ -420,9 +422,9 @@ int main(int argc, char** argv) {
     cerr << "#\t iterations  " << incIterations << '\n';
 
     SparseOptimizer::VertexIDMap vertices = optimizer.vertices();
-    for (auto& vertice : vertices) {
-      auto v =
-          std::static_pointer_cast<SparseOptimizer::Vertex>(vertice.second);
+    for (auto& vertex : vertices) {
+      const auto* v =
+          static_cast<SparseOptimizer::Vertex*>(vertex.second.get());
       maxDim = std::max(maxDim, v->dimension());
     }
 
@@ -649,7 +651,8 @@ int main(int argc, char** argv) {
 
       int nLandmarks = 0;
       int nPoses = 0;
-      int maxDim = *vertexDimensions.rbegin();
+      const int maxDim =
+          *std::max_element(vertexDimensions.begin(), vertexDimensions.end());
       for (auto& it : optimizer.vertices()) {
         auto v = std::static_pointer_cast<OptimizableGraph::Vertex>(it.second);
         if (v->dimension() != maxDim) {

g2o/apps/g2o_viewer/main_window.cpp

@@ -282,7 +282,7 @@ bool MainWindow::load(const QString& filename, g2o::io::Format format) {
   g2o::SparseOptimizer* optimizer = viewer->graph.get();
 
   // update the solvers which are suitable for this graph
-  const std::set<int> vertDims = optimizer->dimensions();
+  const std::unordered_set<int> vertDims = optimizer->dimensions();
   for (size_t i = 0; i < knownSolvers_.size(); ++i) {
     const g2o::OptimizationAlgorithmProperty& sp = knownSolvers_[i];
     if (sp.name.empty() && sp.desc.empty()) continue;

g2o/core/optimizable_graph.cpp

@@ -34,6 +34,7 @@
 #include <iterator>
 #include <memory>
 #include <sstream>
+#include <unordered_set>
 #include <utility>
 #include <vector>
 
@@ -578,7 +579,7 @@ void OptimizableGraph::setRenamedTypesFromString(const std::string& types) {
   Factory* factory = Factory::instance();
   const std::vector<std::string> typesMap = strSplit(types, ",");
   for (const auto& i : typesMap) {
-    std::vector<std::string> m = strSplit(i, "=");
+    const std::vector<std::string> m = strSplit(i, "=");
     if (m.size() != 2) {
       G2O_ERROR("unable to extract type map from {}", i);
       continue;
@@ -594,20 +595,16 @@ void OptimizableGraph::setRenamedTypesFromString(const std::string& types) {
   }
 
   G2O_DEBUG("Load look up table:");
-  for (auto it = renamedTypesLookup_.begin(); it != renamedTypesLookup_.end();
-       ++it) {
-    G2O_DEBUG("{} -> {}", it->first, it->second);
+  for (const auto& rtl : renamedTypesLookup_) {
+    G2O_DEBUG("{} -> {}", rtl.first, rtl.second);
   }
 }
 
 bool OptimizableGraph::isSolverSuitable(
     const OptimizationAlgorithmProperty& solverProperty,
-    const std::set<int>& vertDims_) const {
-  std::set<int> auxDims;
-  if (vertDims_.empty()) {
-    auxDims = dimensions();
-  }
-  const std::set<int>& vertDims = vertDims_.empty() ? auxDims : vertDims_;
+    const std::unordered_set<int>& vertDims_) const {
+  const std::unordered_set<int>& vertDims =
+      vertDims_.empty() ? dimensions() : vertDims_;
   bool suitableSolver = true;
   if (vertDims.size() == 2) {
     if (solverProperty.requiresMarginalize) {
@@ -626,13 +623,13 @@ bool OptimizableGraph::isSolverSuitable(
   return suitableSolver;
 }
 
-std::set<int> OptimizableGraph::dimensions() const {
-  std::set<int> auxDims;
-  for (const auto& it : vertices()) {
-    auto* v = static_cast<OptimizableGraph::Vertex*>(it.second.get());
-    auxDims.insert(v->dimension());
+std::unordered_set<int> OptimizableGraph::dimensions() const {
+  std::unordered_set<int> result;
+  for (const auto& id_v : vertices()) {
+    auto* v = static_cast<OptimizableGraph::Vertex*>(id_v.second.get());
+    result.insert(v->dimension());
   }
-  return auxDims;
+  return result;
 }
 
 void OptimizableGraph::performActions(int iter, HyperGraphActionSet& actions) {
@@ -657,15 +654,15 @@ bool OptimizableGraph::addPostIterationAction(
     std::shared_ptr<HyperGraphAction> action) {
   const std::pair<HyperGraphActionSet::iterator, bool> insertResult =
       graphActions_[static_cast<int>(ActionType::kAtPostiteration)].emplace(
-          action);
+          std::move(action));
   return insertResult.second;
 }
 
 bool OptimizableGraph::addPreIterationAction(
     std::shared_ptr<HyperGraphAction> action) {
   const std::pair<HyperGraphActionSet::iterator, bool> insertResult =
       graphActions_[static_cast<int>(ActionType::kAtPreiteration)].emplace(
-          action);
+          std::move(action));
   return insertResult.second;
 }
 

g2o/core/optimizable_graph.h

@@ -33,9 +33,9 @@
 #include <functional>
 #include <iosfwd>
 #include <memory>
-#include <set>
 #include <string>
 #include <unordered_map>
+#include <unordered_set>
 #include <vector>
 
 #include "g2o/core/eigen_types.h"
@@ -74,7 +74,8 @@ class G2O_CORE_API OptimizableGraph : public HyperGraph {
     kAtNumElements,  // keep as last element
   };
 
-  using HyperGraphActionSet = std::set<std::shared_ptr<HyperGraphAction>>;
+  using HyperGraphActionSet =
+      std::unordered_set<std::shared_ptr<HyperGraphAction>>;
 
   // forward declarations
   class G2O_CORE_API Vertex;
@@ -645,7 +646,7 @@ class G2O_CORE_API OptimizableGraph : public HyperGraph {
    * iterates over all vertices and returns a set of all the vertex dimensions
    * in the graph
    */
-  std::set<int> dimensions() const;
+  std::unordered_set<int> dimensions() const;
 
   /**
    * carry out n iterations
@@ -734,7 +735,8 @@ class G2O_CORE_API OptimizableGraph : public HyperGraph {
    * re-evaluating.
    */
   bool isSolverSuitable(const OptimizationAlgorithmProperty& solverProperty,
-                        const std::set<int>& vertDims = std::set<int>()) const;
+                        const std::unordered_set<int>& vertDims =
+                            std::unordered_set<int>()) const;
 
   //! remove all edges and vertices
   void clear() override;

g2o/examples/g2o_hierarchical/g2o_hierarchical.cpp

@@ -24,10 +24,12 @@
 // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
+#include <algorithm>
 #include <csignal>
 #include <fstream>
 #include <sstream>
 #include <string>
+#include <unordered_set>
 
 #include "edge_creator.h"
 #include "edge_labeler.h"
@@ -270,30 +272,31 @@ int run_hierarchical(int argc, char** argv) {
       solverFactory->construct(strHSolver, hsolverProperty);
   if (!solver) {
     cerr << "Error allocating solver. Allocating \"" << strSolver
-         << "\" failed!" << endl;
+         << "\" failed!\n";
     return 0;
   }
   if (!hsolver) {
     cerr << "Error allocating hsolver. Allocating \"" << strHSolver
-         << "\" failed!" << endl;
+         << "\" failed!\n";
     return 0;
   }
 
-  std::set<int> vertexDimensions = optimizer.dimensions();
+  const std::unordered_set<int> vertexDimensions = optimizer.dimensions();
   if (!optimizer.isSolverSuitable(solverProperty, vertexDimensions)) {
-    cerr << "The selected solver is not suitable for optimizing the given graph"
-         << endl;
+    cerr << "The selected solver is not suitable for optimizing the given "
+            "graph\n";
     return 3;
   }
   if (!optimizer.isSolverSuitable(hsolverProperty, vertexDimensions)) {
-    cerr << "The selected solver is not suitable for optimizing the given graph"
-         << endl;
+    cerr << "The selected solver is not suitable for optimizing the given "
+            "graph\n";
     return 3;
   }
 
   optimizer.setAlgorithm(solver);
 
-  int poseDim = *vertexDimensions.rbegin();
+  const int poseDim =
+      *std::max_element(vertexDimensions.begin(), vertexDimensions.end());
   string backboneVertexType;
   string backboneEdgeType;
   switch (poseDim) {
@@ -320,8 +323,7 @@ int run_hierarchical(int argc, char** argv) {
       break;
     default:
       cerr << "Fatal: unknown backbone type. The largest vertex dimension is: "
-           << poseDim << "." << endl
-           << "Exiting." << endl;
+           << poseDim << ".\nExiting.\n";
       return -1;
   }
 
@@ -519,7 +521,8 @@ int run_hierarchical(int argc, char** argv) {
 
     int nLandmarks = 0;
     int nPoses = 0;
-    int maxDim = *vertexDimensions.rbegin();
+    const int maxDim =
+        *std::max_element(vertexDimensions.begin(), vertexDimensions.end());
     for (auto& it : optimizer.vertices()) {
       auto* v = static_cast<OptimizableGraph::Vertex*>(it.second.get());
       if (v->dimension() != maxDim) {

python/core/py_optimizable_graph.cpp

@@ -177,7 +177,7 @@ void declareOptimizableGraph(py::module& m) {
           py::keep_alive<1, 2>());               // -> void
   cls.def("chi2", &CLS::chi2);                   // -> double
   cls.def("max_dimension", &CLS::maxDimension);  // -> int
-  cls.def("dimensions", &CLS::dimensions);       // -> std::set<int>
+  cls.def("dimensions", &CLS::dimensions);       // -> std::unordered_set<int>
 
   cls.def("optimize", &CLS::optimize, "iterations"_a,
           "online"_a = false);                     // (int, bool) -> int

unit_test/general/graph_operations.cpp

@@ -51,6 +51,7 @@
 #include "g2o/types/slam3d/edge_se3_pointxyz.h"
 #include "g2o/types/slam3d/vertex_pointxyz.h"
 #include "g2o/types/slam3d/vertex_se3.h"
+#include "gmock/gmock.h"
 #include "unit_test/test_helper/allocate_optimizer.h"
 #include "unit_test/test_helper/eigen_matcher.h"
 
@@ -558,7 +559,7 @@ TEST_F(GeneralGraphOperations, LoadingGraph) {
   optimizer_->load(graphData);
   ASSERT_THAT(optimizer_->vertices(), testing::SizeIs(kNumVertices));
   ASSERT_THAT(optimizer_->edges(), testing::SizeIs(kNumVertices));
-  ASSERT_THAT(optimizer_->dimensions(), testing::ElementsAre(3));
+  ASSERT_THAT(optimizer_->dimensions(), testing::UnorderedElementsAre(3));
 
   ASSERT_THAT(optimizer_->vertices(), testing::UnorderedElementsAreArray(
                                           VectorIntToKeys(expectedIds())));
@@ -810,14 +811,14 @@ TEST_F(GeneralGraphOperations, Dimensions) {
   optimizer_->initializeOptimization();
 
   EXPECT_EQ(optimizer_->maxDimension(), 3);
-  EXPECT_THAT(optimizer_->dimensions(), testing::ElementsAre(3));
+  EXPECT_THAT(optimizer_->dimensions(), testing::UnorderedElementsAre(3));
 
   auto point = std::make_shared<g2o::VertexPointXY>();
   point->setId(kNumVertices + 1);
   optimizer_->addVertex(point);
 
   EXPECT_EQ(optimizer_->maxDimension(), 3);
-  EXPECT_THAT(optimizer_->dimensions(), testing::ElementsAre(2, 3));
+  EXPECT_THAT(optimizer_->dimensions(), testing::UnorderedElementsAre(2, 3));
 }
 
 TEST_F(GeneralGraphOperations, VerifyInformationMatrices) {
@@ -861,8 +862,8 @@ TEST_F(GeneralGraphOperations, SolverSuitable) {
   solverPropertyFix63.poseDim = 6;
   solverPropertyFix63.landmarkDim = 3;
 
-  const std::set<int> vertexDims = {2, 3};
-  const std::set<int> vertexDimsNoMatch = {1, 5};
+  const std::unordered_set<int> vertexDims = {2, 3};
+  const std::unordered_set<int> vertexDimsNoMatch = {1, 5};
 
   EXPECT_TRUE(optimizer_->isSolverSuitable(solverPropertyVar));
   EXPECT_TRUE(optimizer_->isSolverSuitable(solverPropertyVar, vertexDims));