diff --git a/CMakeLists.txt b/CMakeLists.txt
index b9f3ad7358ad2dad416d6cf0c8923f23d5a90ece..51eb17354b1a4a5bab11f89924d8ee63c47bcc67 100644
--- a/CMakeLists.txt
+++ b/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
diff --git a/include/core/utils/graph_search.h b/include/core/utils/graph_search.h
new file mode 100644
index 0000000000000000000000000000000000000000..5551dead4ef15a2bd518717a3a6bdc56b8be40d6
--- /dev/null
+++ b/include/core/utils/graph_search.h
@@ -0,0 +1,31 @@
+#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 shortestPath(FrameBasePtr frm1, FrameBasePtr frm2, const unsigned int max_graph_dist = 0);
+
+        std::set<FrameBasePtr> getNeighborFrames(const std::set<FrameBasePtr>& frms);
+};
+
+}  // namespace wolf
+#endif
diff --git a/src/utils/graph_search.cpp b/src/utils/graph_search.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9c3684e5110e6526c33bae41db31b9e2ef5771cb
--- /dev/null
+++ b/src/utils/graph_search.cpp
@@ -0,0 +1,97 @@
+#include "core/utils/graph_search.h"
+
+using namespace wolf;
+
+GraphSearch::GraphSearch() :
+                    parents_()
+{
+
+}
+
+GraphSearch::~GraphSearch()
+{
+
+}
+
+FactorBasePtrList GraphSearch::shortestPath(FrameBasePtr frm1,
+                                            FrameBasePtr frm2,
+                                            const unsigned int max_graph_dist)
+{
+    std::set<FrameBasePtr> frm_neigs({frm1});
+    unsigned int depth = 0;
+
+    while (not frm_neigs.empty())
+    {
+        frm_neigs = getNeighborFrames(frm_neigs);
+        depth++;
+
+        // finish
+        if (frm_neigs.count(frm2) != 0)
+        {
+            assert(parents_.count(frm2) != 0);
+            FactorBasePtrList factor_path;
+            auto prev_frm = frm1;
+
+            while (parents_.at(prev_frm).second != frm2)
+            {
+                factor_path.push_back(parents_.at(prev_frm).first);
+                prev_frm = parents_.at(prev_frm).second;
+            }
+
+            return factor_path;
+        }
+
+        // stop
+        if (max_graph_dist > 0 and depth == max_graph_dist)
+            break;
+    }
+
+    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)
+        {
+            if (fac_by and
+                fac_by->getFrame() and
+                parents_.count(fac_by->getFrame()) != 0)
+            {
+                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)
+            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))
+                    {
+                        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;
+}