diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 71cb203af..125b55277 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -11,7 +11,7 @@ repos: # C++ - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v19.1.5 + rev: v19.1.7 hooks: - id: clang-format diff --git a/g2o/apps/g2o_cli/g2o.cpp b/g2o/apps/g2o_cli/g2o.cpp index a4be6cdb4..0d4ac49fe 100644 --- a/g2o/apps/g2o_cli/g2o.cpp +++ b/g2o/apps/g2o_cli/g2o.cpp @@ -300,7 +300,7 @@ int main(int argc, char** argv) { return 1; } - std::set vertexDimensions = optimizer.dimensions(); + std::unordered_set 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(vertice.second); + for (auto& vertex : vertices) { + const auto* v = + static_cast(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(it.second); if (v->dimension() != maxDim) { diff --git a/g2o/apps/g2o_viewer/main_window.cpp b/g2o/apps/g2o_viewer/main_window.cpp index 45462da7d..431059ff8 100644 --- a/g2o/apps/g2o_viewer/main_window.cpp +++ b/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 vertDims = optimizer->dimensions(); + const std::unordered_set 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; diff --git a/g2o/core/optimizable_graph.cpp b/g2o/core/optimizable_graph.cpp index 0732233b8..999701ad5 100644 --- a/g2o/core/optimizable_graph.cpp +++ b/g2o/core/optimizable_graph.cpp @@ -34,6 +34,7 @@ #include #include #include +#include #include #include @@ -578,7 +579,7 @@ void OptimizableGraph::setRenamedTypesFromString(const std::string& types) { Factory* factory = Factory::instance(); const std::vector typesMap = strSplit(types, ","); for (const auto& i : typesMap) { - std::vector m = strSplit(i, "="); + const std::vector 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& vertDims_) const { - std::set auxDims; - if (vertDims_.empty()) { - auxDims = dimensions(); - } - const std::set& vertDims = vertDims_.empty() ? auxDims : vertDims_; + const std::unordered_set& vertDims_) const { + const std::unordered_set& 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 OptimizableGraph::dimensions() const { - std::set auxDims; - for (const auto& it : vertices()) { - auto* v = static_cast(it.second.get()); - auxDims.insert(v->dimension()); +std::unordered_set OptimizableGraph::dimensions() const { + std::unordered_set result; + for (const auto& id_v : vertices()) { + auto* v = static_cast(id_v.second.get()); + result.insert(v->dimension()); } - return auxDims; + return result; } void OptimizableGraph::performActions(int iter, HyperGraphActionSet& actions) { @@ -657,7 +654,7 @@ bool OptimizableGraph::addPostIterationAction( std::shared_ptr action) { const std::pair insertResult = graphActions_[static_cast(ActionType::kAtPostiteration)].emplace( - action); + std::move(action)); return insertResult.second; } @@ -665,7 +662,7 @@ bool OptimizableGraph::addPreIterationAction( std::shared_ptr action) { const std::pair insertResult = graphActions_[static_cast(ActionType::kAtPreiteration)].emplace( - action); + std::move(action)); return insertResult.second; } diff --git a/g2o/core/optimizable_graph.h b/g2o/core/optimizable_graph.h index 326c6639b..916fcaf24 100644 --- a/g2o/core/optimizable_graph.h +++ b/g2o/core/optimizable_graph.h @@ -33,9 +33,9 @@ #include #include #include -#include #include #include +#include #include #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>; + using HyperGraphActionSet = + std::unordered_set>; // 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 dimensions() const; + std::unordered_set 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& vertDims = std::set()) const; + const std::unordered_set& vertDims = + std::unordered_set()) const; //! remove all edges and vertices void clear() override; diff --git a/g2o/examples/g2o_hierarchical/g2o_hierarchical.cpp b/g2o/examples/g2o_hierarchical/g2o_hierarchical.cpp index b7dfba819..3139e67c9 100644 --- a/g2o/examples/g2o_hierarchical/g2o_hierarchical.cpp +++ b/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 #include #include #include #include +#include #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 vertexDimensions = optimizer.dimensions(); + const std::unordered_set 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(it.second.get()); if (v->dimension() != maxDim) { diff --git a/python/core/py_optimizable_graph.cpp b/python/core/py_optimizable_graph.cpp index da337b3c4..fe26eb109 100644 --- a/python/core/py_optimizable_graph.cpp +++ b/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 + cls.def("dimensions", &CLS::dimensions); // -> std::unordered_set cls.def("optimize", &CLS::optimize, "iterations"_a, "online"_a = false); // (int, bool) -> int diff --git a/unit_test/general/graph_operations.cpp b/unit_test/general/graph_operations.cpp index fb8ec49e7..7edd55e83 100644 --- a/unit_test/general/graph_operations.cpp +++ b/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(); 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 vertexDims = {2, 3}; - const std::set vertexDimsNoMatch = {1, 5}; + const std::unordered_set vertexDims = {2, 3}; + const std::unordered_set vertexDimsNoMatch = {1, 5}; EXPECT_TRUE(optimizer_->isSolverSuitable(solverPropertyVar)); EXPECT_TRUE(optimizer_->isSolverSuitable(solverPropertyVar, vertexDims));