diff --git a/CMakeLists.txt b/CMakeLists.txt
index f80b8e977697e1ab11c8bfcffea3e7e068724a09..7f2eab40b2c21b365065edb196c710259e2c1929 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -248,7 +248,6 @@ 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
@@ -346,7 +345,6 @@ 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
deleted file mode 100644
index 549c1f1dec8f5b45579904ac5ec1f0adebbca593..0000000000000000000000000000000000000000
--- a/include/core/utils/graph_search.h
+++ /dev/null
@@ -1,65 +0,0 @@
-//--------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
deleted file mode 100644
index 20dc65e0e7040cfe92c7d6a39b14f66092ef3f14..0000000000000000000000000000000000000000
--- a/src/utils/graph_search.cpp
+++ /dev/null
@@ -1,140 +0,0 @@
-//--------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 29fc45531a4b0de27c1c227883260b71913c96ab..b0396e22fd240af38382d5bcfcd058e691c1116b 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -80,10 +80,6 @@ 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
deleted file mode 100644
index 43d4b57242a3bb7d1e4532b893d34fcb47fcfcb2..0000000000000000000000000000000000000000
--- a/test/gtest_graph_search.cpp
+++ /dev/null
@@ -1,246 +0,0 @@
-//--------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();
-}
-
-