diff --git a/src/utils/graph_search.cpp b/src/utils/graph_search.cpp
index f1959a029f8b17b97612e047a6d3a6926c18ac3b..d2876701b84d3acabf5e4e9c761a49570bffc2fe 100644
--- a/src/utils/graph_search.cpp
+++ b/src/utils/graph_search.cpp
@@ -17,25 +17,40 @@ 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 prev_frm = frm1;
+ auto frm_it = frm2;
- while (parents_.at(prev_frm).second != frm2)
+ while (frm_it != frm1)
{
- factor_path.push_back(parents_.at(prev_frm).first);
- prev_frm = parents_.at(prev_frm).second;
+ factor_path.push_back(parents_.at(frm_it).first);
+ frm_it = parents_.at(frm_it).second;
}
return factor_path;
@@ -45,6 +60,7 @@ FactorBasePtrList GraphSearch::computeShortestPath(FrameBasePtr frm1,
if (max_graph_dist > 0 and depth == max_graph_dist)
break;
}
+ //WOLF_INFO("Path to frame ", frm2->id(), " NOT found!");
return FactorBasePtrList();
}
@@ -61,10 +77,13 @@ std::set<FrameBasePtr> GraphSearch::getNeighborFrames(const std::set<FrameBasePt
// 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)
+ 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);
}
@@ -76,22 +95,24 @@ std::set<FrameBasePtr> GraphSearch::getNeighborFrames(const std::set<FrameBasePt
// 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();
- if (frm_other and parents_.count(frm_other))
+ //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);
}
}
+ }
}
- // TODO
- // get list of factors and "constrained by" factor of each frame
- // check that these factors are not in factor_parents_
- // check that frames are not in frames_parents_ ant not in frm_neigs
- // store in factor_parents_ and frames_parents and frm_neigs
return frm_neigs;
}
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 8a5eb8fa3a068dca72974a86bdcdee1fd9453baf..be33df04ad6dc395ec55b63b9de14a4ea73ff904 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 0000000000000000000000000000000000000000..9693846e7994bf071a1aed368c99626c2311d43a
--- /dev/null
+++ b/test/gtest_graph_search.cpp
@@ -0,0 +1,225 @@
+/*
+ * 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();
+}
+
+