diff --git a/g2o/core/abstract_graph.cpp b/g2o/core/abstract_graph.cpp index e3540ea1f..e78a95d2c 100644 --- a/g2o/core/abstract_graph.cpp +++ b/g2o/core/abstract_graph.cpp @@ -37,6 +37,7 @@ #include "io/io_g2o.h" namespace { +#ifdef G2O_HAVE_LOGGING std::string_view to_string(g2o::io::Format format) { switch (format) { case g2o::io::Format::kG2O: @@ -50,6 +51,7 @@ std::string_view to_string(g2o::io::Format format) { } return ""; } +#endif /** * @brief Allocate an g2o::IoInterface to load/save data of the graph. diff --git a/unit_test/slam2d/CMakeLists.txt b/unit_test/slam2d/CMakeLists.txt index 845c65202..c00009874 100644 --- a/unit_test/slam2d/CMakeLists.txt +++ b/unit_test/slam2d/CMakeLists.txt @@ -1,6 +1,7 @@ add_executable(unittest_slam2d mappings_se2.cpp jacobians_slam2d.cpp + slam2d_io.cpp solve_direct_slam2d.cpp ) target_link_libraries(unittest_slam2d unittest_helper types_slam2d) diff --git a/unit_test/slam2d/slam2d_io.cpp b/unit_test/slam2d/slam2d_io.cpp new file mode 100644 index 000000000..ecacbb871 --- /dev/null +++ b/unit_test/slam2d/slam2d_io.cpp @@ -0,0 +1,116 @@ +// 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 "g2o/core/io/io_format.h" +#include "g2o/core/optimizable_graph.h" +#include "g2o/core/sparse_optimizer.h" +#include "g2o/types/slam2d/edge_se2.h" +#include "unit_test/test_helper/allocate_optimizer.h" +#include "unit_test/test_helper/random_state.h" + +using namespace g2o; // NOLINT +using namespace testing; // NOLINT + +template +struct FixedSizeEdgeIOFixture : public ::testing::Test { + public: + FixedSizeEdgeIOFixture() = default; + + void SetUp() override { + using EdgeType = T; + + // Construct a small graph + auto edge = std::make_shared(); + + for (size_t i = 0; i < edge->vertices().size(); ++i) { + auto v = std::shared_ptr(edge->createVertex(i)); + edge->vertices()[i] = v; + v->setId(i); + v->setFixed(i == 0); + initializeNthVertex(i, *edge); + this->optimizer_ptr_->addVertex(v); + } + + // TODO(Rainer): better randomization for information matrix (off-diag + // elemns) + VectorN info_diagonal = + VectorN::Random(); + edge->setInformation(info_diagonal.cwiseAbs().asDiagonal()); + edge->setMeasurement( + g2o::internal::RandomValue::create()); + this->optimizer_ptr_->addEdge(edge); + } + + protected: + std::unique_ptr optimizer_ptr_ = + g2o::internal::createOptimizerForTests(); + + template + static std::enable_if_t initializeNthVertex(int /*i*/, + EdgeType& /*t*/) {} + + template + static std::enable_if_t initializeNthVertex(int i, + EdgeType& t) { + if (i == I) { + auto vertex = t.template vertexXn(); + vertex->setEstimate( + g2o::internal::RandomValue::EstimateType>::create()); + return; + } + initializeNthVertex(i, t); + } +}; +TYPED_TEST_SUITE_P(FixedSizeEdgeIOFixture); + +TYPED_TEST_P(FixedSizeEdgeIOFixture, SaveAndLoad) { + g2o::io::Format format = io::Format::kG2O; + + std::stringstream buffer; + bool save_result = this->optimizer_ptr_->save(buffer, format); + ASSERT_THAT(save_result, IsTrue()); + EXPECT_THAT(buffer.str(), Not(IsEmpty())); + + auto loaded_optimizer = g2o::internal::createOptimizerForTests(); + loaded_optimizer->load(buffer, format); + + // Brutally check that serialization result is the same + std::stringstream buffer_after_loading; + save_result = loaded_optimizer->save(buffer_after_loading, format); + ASSERT_THAT(save_result, IsTrue()); + EXPECT_THAT(buffer.str(), Eq(buffer_after_loading.str())); +} + +REGISTER_TYPED_TEST_SUITE_P(FixedSizeEdgeIOFixture, SaveAndLoad); + +using Slam2DIoTypes = ::testing::Types; +INSTANTIATE_TYPED_TEST_SUITE_P(Slam2D, FixedSizeEdgeIOFixture, Slam2DIoTypes); diff --git a/unit_test/test_helper/random_state.h b/unit_test/test_helper/random_state.h index 19acd2bf8..d168c217a 100644 --- a/unit_test/test_helper/random_state.h +++ b/unit_test/test_helper/random_state.h @@ -26,13 +26,12 @@ #include -#include "g2o/config.h" #include "g2o/core/eigen_types.h" #include "g2o/stuff/sampler.h" +#include "g2o/types/slam2d/se2.h" #include "g2o/types/slam3d/se3quat.h" -namespace g2o { -namespace internal { +namespace g2o::internal { inline Isometry3 randomIsometry3() { const g2o::Vector3 rotAxisAngle = g2o::Vector3::Random() + g2o::Vector3::Random(); @@ -65,5 +64,36 @@ struct RandomDouble { } }; -} // namespace internal -} // namespace g2o +template +struct RandomValue { + // using Type = T; + // static Type create() { + // static_assert(false, "No specialization provided"); + // } +}; + +template <> +struct RandomValue { + using Type = double; + static Type create() { return RandomDouble::create(); } +}; + +template <> +struct RandomValue { + using Type = g2o::SE2; + static Type create() { return Type(g2o::Vector3::Random()); } +}; + +template <> +struct RandomValue { + using Type = g2o::SE3Quat; + static Type create() { return RandomSE3Quat::create(); } +}; + +template <> +struct RandomValue { + using Type = g2o::Isometry3; + static Type create() { return RandomIsometry3::create(); } +}; + +} // namespace g2o::internal