Resolve "Graph search"

CMakeLists.txt

@@ -167,6 +167,7 @@ SET(HDRS_UTILS
   include/core/utils/converter.h
   include/core/utils/eigen_assert.h
   include/core/utils/eigen_predicates.h
+  include/core/utils/graph_search.h
   include/core/utils/loader.h
   include/core/utils/logging.h
   include/core/utils/make_unique.h
@@ -310,10 +311,11 @@ SET(SRCS_COMMON
 SET(SRCS_MATH
   )
 SET(SRCS_UTILS
+  src/utils/check_log.cpp
   src/utils/converter_utils.cpp
-  src/utils/params_server.cpp
+  src/utils/graph_search.cpp
   src/utils/loader.cpp
-  src/utils/check_log.cpp
+  src/utils/params_server.cpp
   )
 
 SET(SRCS_CAPTURE

include/core/utils/graph_search.h

+#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

src/utils/graph_search.cpp

+#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;
+}
+

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})

test/gtest_graph_search.cpp

+/*
+ * 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();
+}
+
+