diff --git a/g2o/core/base_vertex.h b/g2o/core/base_vertex.h index 0ef53c007..a766dc8e1 100644 --- a/g2o/core/base_vertex.h +++ b/g2o/core/base_vertex.h @@ -100,18 +100,18 @@ class BaseVertex : public OptimizableGraph::Vertex { HessianBlockType& A() { return hessian_; } const HessianBlockType& A() const { return hessian_; } - void push() final { backup_.push(estimate_); } - void pop() final { + void push() override { backup_.push(estimate_); } + void pop() override { assert(!backup_.empty()); estimate_ = backup_.top(); backup_.pop(); updateCache(); } - void discardTop() final { + void discardTop() override { assert(!backup_.empty()); backup_.pop(); } - [[nodiscard]] int stackSize() const final { return backup_.size(); } + [[nodiscard]] int stackSize() const override { return backup_.size(); } //! return the current estimate of the vertex const EstimateType& estimate() const { return estimate_; } diff --git a/unit_test/general/CMakeLists.txt b/unit_test/general/CMakeLists.txt index f9f128466..9f8e8140a 100644 --- a/unit_test/general/CMakeLists.txt +++ b/unit_test/general/CMakeLists.txt @@ -9,6 +9,7 @@ add_executable(unittest_general robust_kernel_tests.cpp sparse_block_matrix.cpp type_traits_tests.cpp + optimization_basics.cpp ) target_link_libraries(unittest_general unittest_helper) target_link_libraries(unittest_general types_slam3d types_slam2d types_data) diff --git a/unit_test/general/optimization_basics.cpp b/unit_test/general/optimization_basics.cpp new file mode 100644 index 000000000..27784f95d --- /dev/null +++ b/unit_test/general/optimization_basics.cpp @@ -0,0 +1,166 @@ +// g2o - General Graph Optimization +// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED +// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A +// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +// 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 "g2o/core/factory.h" +#include "g2o/core/hyper_graph.h" +#include "g2o/core/hyper_graph_action.h" +#include "g2o/core/sparse_optimizer.h" +#include "g2o/types/slam2d/edge_se2.h" +#include "g2o/types/slam2d/vertex_se2.h" +#include "unit_test/test_helper/allocate_optimizer.h" +#include "unit_test/test_helper/eigen_matcher.h" + +using namespace testing; // NOLINT + +G2O_USE_TYPE_GROUP(slam2d); // NOLINT + +namespace { +class MockVertexSE2 : public g2o::VertexSE2 { + public: + MOCK_METHOD(void, push, (), (override)); +}; + +class MockAction : public g2o::HyperGraphAction { + public: + bool operator()(const g2o::HyperGraph& graph, + g2o::HyperGraphAction::Parameters& params) override { + return BracketOperator(graph, params); + }; + MOCK_METHOD(bool, BracketOperator, + (const g2o::HyperGraph&, g2o::HyperGraphAction::Parameters&), ()); +}; +} // namespace + +TEST(OptimizationBasics, BackupLevenberg) { + constexpr size_t kNumVertices = 10; + constexpr int kNumIter = 1; + std::shared_ptr gauge; + std::shared_ptr free_vertex; + std::vector> mock_vertices; + + std::shared_ptr optimizer = + g2o::internal::createLmOptimizerForTests(); + + // Add vertices + for (size_t i = 0; i < kNumVertices; ++i) { + auto v = std::make_shared(); + v->setEstimate(g2o::SE2()); + v->setId(i); + if (i == 0) { + v->setFixed(true); // fix the first vertex + gauge = v; + } else { + free_vertex = v; + } + optimizer->addVertex(v); + mock_vertices.emplace_back(std::move(v)); + } + + // Add edges + for (size_t i = 0; i < kNumVertices; ++i) { + auto e1 = std::make_shared(); + e1->vertices()[0] = optimizer->vertex((i + 0) % kNumVertices); + e1->vertices()[1] = optimizer->vertex((i + 1) % kNumVertices); + e1->setMeasurement(g2o::SE2(1., 0., 0.)); + e1->setInformation(g2o::EdgeSE2::InformationType::Identity()); + optimizer->addEdge(e1); + } + + // Mock calls setup + for (auto& v : mock_vertices) { + EXPECT_CALL(*v, push).Times(AtLeast(1)).WillRepeatedly([&v]() { + (*v).g2o::VertexSE2::push(); + }); + } + + optimizer->initializeOptimization(); + EXPECT_THAT(optimizer->activeVertices(), Contains(free_vertex)); + EXPECT_THAT(optimizer->activeVertices(), Contains(gauge)); + EXPECT_THAT(optimizer->activeVertices(), SizeIs(kNumVertices)); + + optimizer->optimize(kNumIter); + + EXPECT_THAT(free_vertex->stackSize(), Eq(0)); +} + +TEST(OptimizationBasics, ActionCalls) { + constexpr size_t kNumVertices = 10; + constexpr int kNumIter = 1; + + std::shared_ptr optimizer = + g2o::internal::createOptimizerForTests(); + + // Add vertices + for (size_t i = 0; i < kNumVertices; ++i) { + auto v = std::make_shared(); + v->setEstimate(g2o::SE2()); + v->setId(i); + v->setFixed(i == 0); // fix the first vertex + optimizer->addVertex(v); + } + + // Add edges + for (size_t i = 0; i < kNumVertices; ++i) { + auto e1 = std::make_shared(); + e1->vertices()[0] = optimizer->vertex((i + 0) % kNumVertices); + e1->vertices()[1] = optimizer->vertex((i + 1) % kNumVertices); + e1->setMeasurement(g2o::SE2(1., 0., 0.)); + e1->setInformation(g2o::EdgeSE2::InformationType::Identity()); + optimizer->addEdge(e1); + } + + // Mock Setup + auto generic_action = std::make_shared(); + auto pre_action = std::make_shared(); + auto post_action = std::make_shared(); + Sequence action_sequence; + + ON_CALL(*generic_action, BracketOperator).WillByDefault(Return(true)); + ON_CALL(*pre_action, BracketOperator).WillByDefault(Return(true)); + ON_CALL(*post_action, BracketOperator).WillByDefault(Return(true)); + EXPECT_CALL(*generic_action, BracketOperator).Times(AtLeast(kNumIter * 2)); + // pre iteration + EXPECT_CALL(*pre_action, BracketOperator).InSequence(action_sequence); + EXPECT_CALL(*post_action, BracketOperator).InSequence(action_sequence); + // actual iterations + for (int i = 0; i < kNumIter; ++i) { + EXPECT_CALL(*pre_action, BracketOperator).InSequence(action_sequence); + EXPECT_CALL(*post_action, BracketOperator).InSequence(action_sequence); + } + + optimizer->addPreIterationAction(generic_action); + optimizer->addPreIterationAction(pre_action); + optimizer->addPostIterationAction(generic_action); + optimizer->addPostIterationAction(post_action); + optimizer->initializeOptimization(); + optimizer->optimize(kNumIter); +} diff --git a/unit_test/test_helper/allocate_optimizer.cpp b/unit_test/test_helper/allocate_optimizer.cpp index 0678ba6db..6e29d5e0a 100644 --- a/unit_test/test_helper/allocate_optimizer.cpp +++ b/unit_test/test_helper/allocate_optimizer.cpp @@ -31,8 +31,7 @@ #include "g2o/core/optimization_algorithm_levenberg.h" #include "g2o/solvers/eigen/linear_solver_eigen.h" -namespace g2o { -namespace internal { +namespace g2o::internal { using SlamBlockSolver = g2o::BlockSolver>; using SlamLinearSolver = @@ -64,5 +63,4 @@ std::unique_ptr createLmOptimizerForTests() { return create(true); } -} // namespace internal -} // namespace g2o +} // namespace g2o::internal