Revert "implementation"

This reverts commit 164786a9

CMakeLists.txt

@@ -167,7 +167,6 @@ 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
@@ -311,11 +310,10 @@ SET(SRCS_COMMON
 SET(SRCS_MATH
   )
 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
+  src/utils/loader.cpp
+  src/utils/check_log.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 shortestPath(FrameBasePtr frm1, FrameBasePtr frm2, const unsigned int max_graph_dist = 0);
-
-        std::set<FrameBasePtr> getNeighborFrames(const std::set<FrameBasePtr>& frms);
-};
-
-}  // namespace wolf
-#endif

src/utils/graph_search.cpp

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