Resolve "Graph search"

Closes #404 (closed)

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