From fcbb993d15b9fb3627be67cb85d2f6a81ed93edd Mon Sep 17 00:00:00 2001
From: Joan Sola <jsola@iri.upc.edu>
Date: Sun, 23 Jan 2022 20:58:04 +0100
Subject: [PATCH] Revert "Remove graph_search things"
This reverts commit 93b4fbd0eb37c52a585ba3f9ebfd2fcb9f4412b6.
---
CMakeLists.txt | 2 +
include/core/utils/graph_search.h | 65 ++++++++
src/utils/graph_search.cpp | 140 +++++++++++++++++
test/CMakeLists.txt | 4 +
test/gtest_graph_search.cpp | 246 ++++++++++++++++++++++++++++++
5 files changed, 457 insertions(+)
create mode 100644 include/core/utils/graph_search.h
create mode 100644 src/utils/graph_search.cpp
create mode 100644 test/gtest_graph_search.cpp
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 425ffc64a..5c994422a 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -248,6 +248,7 @@ SET(HDRS_UTILS
include/core/utils/check_log.h
include/core/utils/converter.h
include/core/utils/eigen_assert.h
+ include/core/utils/graph_search.h
include/core/utils/loader.h
include/core/utils/logging.h
include/core/utils/params_server.h
@@ -345,6 +346,7 @@ SET(SRCS_TREE_MANAGER
SET(SRCS_UTILS
src/utils/check_log.cpp
src/utils/converter_utils.cpp
+ src/utils/graph_search.cpp
src/utils/loader.cpp
src/utils/params_server.cpp
)
diff --git a/include/core/utils/graph_search.h b/include/core/utils/graph_search.h
new file mode 100644
index 000000000..549c1f1de
--- /dev/null
+++ b/include/core/utils/graph_search.h
@@ -0,0 +1,65 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#ifndef GRAPH_SEARCH_H
+#define GRAPH_SEARCH_H
+
+#include "core/common/wolf.h"
+#include "core/frame/frame_base.h"
+#include "core/factor/factor_base.h"
+
+#include <map>
+
+namespace wolf
+{
+
+class GraphSearch
+{
+ private:
+
+ std::map<FrameBasePtr, std::pair<FactorBasePtr,FrameBasePtr>> parents_;
+
+ public:
+
+ GraphSearch();
+
+ ~GraphSearch();
+
+ FactorBasePtrList computeShortestPath(FrameBasePtr frm1,
+ FrameBasePtr frm2,
+ const unsigned int max_graph_dist = 0);
+
+ std::set<FrameBasePtr> getNeighborFrames(const std::set<FrameBasePtr>& frms);
+
+ static FactorBasePtrList shortestPath(FrameBasePtr frm1,
+ FrameBasePtr frm2,
+ const unsigned int max_graph_dist = 0)
+ {
+ GraphSearch graph_search;
+
+ return graph_search.computeShortestPath(frm1, frm2, max_graph_dist);
+ }
+
+};
+
+
+} // namespace wolf
+#endif
diff --git a/src/utils/graph_search.cpp b/src/utils/graph_search.cpp
new file mode 100644
index 000000000..20dc65e0e
--- /dev/null
+++ b/src/utils/graph_search.cpp
@@ -0,0 +1,140 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include "core/utils/graph_search.h"
+
+using namespace wolf;
+
+GraphSearch::GraphSearch() :
+ parents_()
+{
+
+}
+
+GraphSearch::~GraphSearch()
+{
+
+}
+
+FactorBasePtrList GraphSearch::computeShortestPath(FrameBasePtr frm1,
+ FrameBasePtr frm2,
+ const unsigned int max_graph_dist)
+{
+ //WOLF_INFO("GraphSearch::computeShortestPath: from frame ", frm1->id(), " to frame ", frm2->id());
+
+ std::set<FrameBasePtr> frm_neigs({frm1});
+ parents_[frm1] = std::pair<FactorBasePtr,FrameBasePtr>(nullptr, nullptr);
+ unsigned int depth = 0;
+
+ //WOLF_INFO(frm1->id());
+
+ while (not frm_neigs.empty())
+ {
+ frm_neigs = getNeighborFrames(frm_neigs);
+ depth++;
+
+ //if (not frm_neigs.empty())
+ //{
+ // std::string frm_neigs_str(depth, '.');
+ // for (auto frm : frm_neigs)
+ // frm_neigs_str += std::to_string(frm->id()) + std::string(" ");
+ // WOLF_INFO(frm_neigs_str);
+ //}
+
+ // finish
+ if (frm_neigs.count(frm2) != 0)
+ {
+ //WOLF_INFO("Frame ", frm2->id(), " found!");
+
+ assert(parents_.count(frm2) != 0);
+ FactorBasePtrList factor_path;
+ auto frm_it = frm2;
+
+ while (frm_it != frm1)
+ {
+ factor_path.push_back(parents_.at(frm_it).first);
+ frm_it = parents_.at(frm_it).second;
+ }
+
+ return factor_path;
+ }
+
+ // stop
+ if (max_graph_dist > 0 and depth == max_graph_dist)
+ break;
+ }
+ //WOLF_INFO("Path to frame ", frm2->id(), " NOT found!");
+
+ return FactorBasePtrList();
+}
+
+std::set<FrameBasePtr> GraphSearch::getNeighborFrames(const std::set<FrameBasePtr>& frms)
+{
+ std::set<FrameBasePtr> frm_neigs;
+
+ for (auto frm : frms)
+ {
+ // get constrained by factors
+ FactorBasePtrList facs_by = frm->getConstrainedByList();
+
+ // Iterate over all factors_by
+ for (auto && fac_by : facs_by)
+ {
+ //WOLF_INFO_COND(fac_by, "fac_by: ", fac_by->id());
+ //WOLF_INFO_COND(fac_by->getFrame(), "fac_by->getFrame(): ", fac_by->getFrame()->id());
+ if (fac_by and
+ fac_by->getFrame() and
+ parents_.count(fac_by->getFrame()) == 0)
+ {
+ //WOLF_INFO("registering");
+ frm_neigs.insert(fac_by->getFrame());
+ parents_[fac_by->getFrame()] = std::pair<FactorBasePtr,FrameBasePtr>(fac_by, frm);
+ }
+ }
+
+ // get the factors of this frame
+ FactorBasePtrList facs_own;
+ frm->getFactorList(facs_own);
+
+ // Iterate over all factors_own
+ for (auto && fac_own : facs_own)
+ {
+ //WOLF_INFO_COND(fac_own, "fac_own: ", fac_own->id());
+ //WOLF_INFO_COND(fac_own->getFrameOtherList().empty(), "fac_own->getFrameOtherList() is empty");
+ if (fac_own and not fac_own->getFrameOtherList().empty())
+ for (auto frm_other_w : fac_own->getFrameOtherList())
+ {
+ auto frm_other = frm_other_w.lock();
+ //WOLF_INFO_COND(frm_other, "frm_other ", frm_other->id());
+ if (frm_other and
+ parents_.count(frm_other) == 0)
+ {
+ //WOLF_INFO("registering");
+ frm_neigs.insert(frm_other);
+ parents_[frm_other] = std::pair<FactorBasePtr,FrameBasePtr>(fac_own, frm);
+ }
+ }
+ }
+ }
+
+ return frm_neigs;
+}
+
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index f701d0fc7..743787f40 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -80,6 +80,10 @@ target_link_libraries(gtest_feature_base ${PLUGIN_NAME})
wolf_add_gtest(gtest_frame_base gtest_frame_base.cpp)
target_link_libraries(gtest_frame_base ${PLUGIN_NAME})
+# GraphSearch class test
+wolf_add_gtest(gtest_graph_search gtest_graph_search.cpp)
+target_link_libraries(gtest_graph_search ${PLUGIN_NAME})
+
# HasStateBlocks classes test
wolf_add_gtest(gtest_has_state_blocks gtest_has_state_blocks.cpp)
target_link_libraries(gtest_has_state_blocks ${PLUGIN_NAME})
diff --git a/test/gtest_graph_search.cpp b/test/gtest_graph_search.cpp
new file mode 100644
index 000000000..43d4b5724
--- /dev/null
+++ b/test/gtest_graph_search.cpp
@@ -0,0 +1,246 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+/*
+ * gtest_graph_search.cpp
+ *
+ * Created on: Jul, 2021
+ * Author: jvallve
+ */
+
+#include "core/utils/utils_gtest.h"
+
+#include "core/problem/problem.h"
+#include "core/capture/capture_void.h"
+#include "core/factor/factor_relative_pose_2d.h"
+#include "core/utils/graph_search.h"
+
+#include <iostream>
+#include <thread>
+
+using namespace wolf;
+using namespace Eigen;
+
+class GraphSearchTest : public testing::Test
+{
+ public:
+ ProblemPtr problem;
+
+ void SetUp() override
+ {
+ problem = Problem::create("PO", 2);
+ }
+
+ FrameBasePtr emplaceFrame(const TimeStamp& ts)
+ {
+ return problem->emplaceFrame(ts, Vector3d::Zero());
+ }
+
+ FactorBasePtr createFactor(FrameBasePtr frm1, FrameBasePtr frm2)
+ {
+ auto C12 = CaptureBase::emplace<CaptureVoid>(frm2, frm2->getTimeStamp(), nullptr);
+ auto f12 = FeatureBase::emplace<FeatureBase>(C12, "odom", Vector3d::Zero(), Matrix3d::Identity());
+ return FactorBase::emplace<FactorRelativePose2d>(f12, f12, frm1, nullptr, false, TOP_MOTION);
+ }
+};
+
+TEST_F(GraphSearchTest, setup)
+{
+ ASSERT_TRUE(problem->check());
+}
+
+TEST_F(GraphSearchTest, nonsense11)
+{
+ auto F1 = emplaceFrame(1);
+
+ auto fac_list = GraphSearch::shortestPath(F1, F1, 10);
+
+ ASSERT_TRUE(fac_list.empty());
+}
+
+TEST_F(GraphSearchTest, single12)
+{
+ auto F1 = emplaceFrame(1);
+ auto F2 = emplaceFrame(2);
+ auto fac12 = createFactor(F1,F2);
+
+ auto fac_list = GraphSearch::shortestPath(F1, F2, 10);
+
+ ASSERT_EQ(fac_list.size(), 1);
+ ASSERT_EQ(fac_list.front(), fac12);
+}
+
+TEST_F(GraphSearchTest, single21)
+{
+ auto F1 = emplaceFrame(1);
+ auto F2 = emplaceFrame(2);
+ auto fac12 = createFactor(F1,F2);
+
+ auto fac_list = GraphSearch::shortestPath(F2, F1, 10);
+
+ ASSERT_EQ(fac_list.size(), 1);
+ ASSERT_EQ(fac_list.front(), fac12);
+}
+
+TEST_F(GraphSearchTest, double12)
+{
+ auto F1 = emplaceFrame(1);
+ auto F2 = emplaceFrame(2);
+ auto fac12 = createFactor(F1,F2);
+ auto fac12b = createFactor(F1,F2);
+
+ auto fac_list = GraphSearch::shortestPath(F1, F2, 10);
+
+ ASSERT_EQ(fac_list.size(), 1);
+ ASSERT_TRUE(fac_list.front() == fac12 or fac_list.front() == fac12b);
+}
+
+TEST_F(GraphSearchTest, no_solution12)
+{
+ auto F1 = emplaceFrame(1);
+ auto F2 = emplaceFrame(2);
+ auto F3 = emplaceFrame(3);
+ auto fac23 = createFactor(F2,F3);
+
+ auto fac_list = GraphSearch::shortestPath(F1, F2, 10);
+
+ ASSERT_TRUE(fac_list.empty());
+}
+
+TEST_F(GraphSearchTest, no_solution21)
+{
+ auto F1 = emplaceFrame(1);
+ auto F2 = emplaceFrame(2);
+ auto F3 = emplaceFrame(3);
+ auto fac23 = createFactor(F2,F3);
+
+ auto fac_list = GraphSearch::shortestPath(F2, F1, 10);
+
+ ASSERT_TRUE(fac_list.empty());
+}
+
+TEST_F(GraphSearchTest, large)
+{
+ auto F1 = emplaceFrame(1);
+ auto F2 = emplaceFrame(2);
+ auto F3 = emplaceFrame(3);
+ auto F4 = emplaceFrame(4);
+ auto F5 = emplaceFrame(5);
+ auto F6 = emplaceFrame(6);
+ auto F7 = emplaceFrame(7);
+ auto F8 = emplaceFrame(8);
+ auto F9 = emplaceFrame(9);
+
+ auto fac12 = createFactor(F1,F2);
+ auto fac23 = createFactor(F2,F3);
+ auto fac34 = createFactor(F3,F4);
+ auto fac45 = createFactor(F4,F5);
+ auto fac56 = createFactor(F5,F6);
+ auto fac67 = createFactor(F6,F7);
+ auto fac78 = createFactor(F7,F8);
+ auto fac89 = createFactor(F8,F9);
+
+ auto fac_list = GraphSearch::shortestPath(F1, F9, 8);
+
+ ASSERT_EQ(fac_list.size(), 8);
+
+ auto fac_list_2 = GraphSearch::shortestPath(F1, F9, 7);
+
+ ASSERT_EQ(fac_list_2.size(), 0);
+}
+
+TEST_F(GraphSearchTest, large_no_solution)
+{
+ auto F1 = emplaceFrame(1);
+ auto F2 = emplaceFrame(2);
+ auto F3 = emplaceFrame(3);
+ auto F4 = emplaceFrame(4);
+ auto F5 = emplaceFrame(5);
+ auto F6 = emplaceFrame(6);
+ auto F7 = emplaceFrame(7);
+ auto F8 = emplaceFrame(8);
+ auto F9 = emplaceFrame(9);
+
+ auto fac12 = createFactor(F1,F2);
+ auto fac23 = createFactor(F2,F3);
+ auto fac34 = createFactor(F3,F4);
+ auto fac45 = createFactor(F4,F5);
+
+ auto fac67 = createFactor(F6,F7);
+ auto fac78 = createFactor(F7,F8);
+ auto fac89 = createFactor(F8,F9);
+
+ auto fac_list = GraphSearch::shortestPath(F1, F9, 10);
+
+ ASSERT_EQ(fac_list.size(), 0);
+
+ auto fac_list_2 = GraphSearch::shortestPath(F9, F1, 10);
+
+ ASSERT_EQ(fac_list_2.size(), 0);
+}
+
+TEST_F(GraphSearchTest, large_dense)
+{
+ /* ------- ---------------
+ * | | | |
+ * 1---2---3---4---5---6---7---8---9
+ * | |
+ * -----------
+ */
+
+ auto F1 = emplaceFrame(1);
+ auto F2 = emplaceFrame(2);
+ auto F3 = emplaceFrame(3);
+ auto F4 = emplaceFrame(4);
+ auto F5 = emplaceFrame(5);
+ auto F6 = emplaceFrame(6);
+ auto F7 = emplaceFrame(7);
+ auto F8 = emplaceFrame(8);
+ auto F9 = emplaceFrame(9);
+
+ auto fac12 = createFactor(F1,F2);
+ auto fac13 = createFactor(F1,F3);
+ auto fac23 = createFactor(F2,F3);
+ auto fac34 = createFactor(F3,F4);
+ auto fac36 = createFactor(F3,F6);
+ auto fac45 = createFactor(F4,F5);
+ auto fac56 = createFactor(F5,F6);
+ auto fac59 = createFactor(F5,F9);
+ auto fac67 = createFactor(F6,F7);
+ auto fac78 = createFactor(F7,F8);
+ auto fac89 = createFactor(F8,F9);
+
+ auto fac_list = GraphSearch::shortestPath(F1, F9, 10);
+
+ ASSERT_EQ(fac_list.size(), 4);
+
+ auto fac_list_2 = GraphSearch::shortestPath(F9, F1, 10);
+
+ ASSERT_EQ(fac_list_2.size(), 4);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+
+
--
GitLab