implemented, test todo

CMakeLists.txt

@@ -292,6 +292,7 @@ SET(HDRS_TREE_MANAGER
   include/core/tree_manager/factory_tree_manager.h
   include/core/tree_manager/tree_manager_base.h
   include/core/tree_manager/tree_manager_sliding_window.h
+  include/core/tree_manager/tree_manager_sliding_window_dual_rate.h
   )
 SET(HDRS_YAML
   include/core/yaml/parser_yaml.h
@@ -381,6 +382,7 @@ SET(SRCS_SOLVER
   )
 SET(SRCS_TREE_MANAGER
   src/tree_manager/tree_manager_sliding_window.cpp
+  src/tree_manager/tree_manager_sliding_window_dual_rate.cpp
   )
 SET(SRCS_YAML
   src/yaml/parser_yaml.cpp

include/core/tree_manager/tree_manager_sliding_window.h

 #ifndef INCLUDE_TREE_MANAGER_SLIDING_WINDOW_H_
 #define INCLUDE_TREE_MANAGER_SLIDING_WINDOW_H_
 
-#include "../tree_manager/tree_manager_base.h"
+#include "core/tree_manager/tree_manager_base.h"
 
 namespace wolf
 {
@@ -15,20 +15,20 @@ struct ParamsTreeManagerSlidingWindow : public ParamsTreeManagerBase
         ParamsTreeManagerSlidingWindow(std::string _unique_name, const wolf::ParamsServer & _server) :
             ParamsTreeManagerBase(_unique_name, _server)
         {
-            n_key_frames                = _server.getParam<unsigned int>(prefix + "/n_key_frames");
-            fix_first_key_frame         = _server.getParam<bool>        (prefix + "/fix_first_key_frame");
+            n_frames                    = _server.getParam<unsigned int>(prefix + "/n_frames");
+            fix_first_frame             = _server.getParam<bool>        (prefix + "/fix_first_frame");
             viral_remove_empty_parent   = _server.getParam<bool>        (prefix + "/viral_remove_empty_parent");
         }
         std::string print() const
         {
             return "\n" + ParamsTreeManagerBase::print()                                            + "\n"
-                        + "n_key_frames: "              + std::to_string(n_key_frames)              + "\n"
-                        + "fix_first_key_frame: "       + std::to_string(fix_first_key_frame)       + "\n"
+                        + "n_frames: "                  + std::to_string(n_frames)                  + "\n"
+                        + "fix_first_frame: "           + std::to_string(fix_first_frame)           + "\n"
                         + "viral_remove_empty_parent: " + std::to_string(viral_remove_empty_parent) + "\n";
         }
 
-        unsigned int n_key_frames;
-        bool fix_first_key_frame;
+        unsigned int n_frames;
+        bool fix_first_frame;
         bool viral_remove_empty_parent;
 };
 
@@ -43,7 +43,7 @@ class TreeManagerSlidingWindow : public TreeManagerBase
 
         ~TreeManagerSlidingWindow() override{}
 
-        void keyFrameCallback(FrameBasePtr _key_frame) override;
+        void keyFrameCallback(FrameBasePtr _frame) override;
 
     protected:
         ParamsTreeManagerSlidingWindowPtr params_sw_;

include/core/tree_manager/tree_manager_sliding_window_dual_rate.h

+#ifndef INCLUDE_TREE_MANAGER_SLIDING_WINDOW_DUAL_RATE_H_
+#define INCLUDE_TREE_MANAGER_SLIDING_WINDOW_DUAL_RATE_H_
+
+#include "core/tree_manager/tree_manager_sliding_window.h"
+
+namespace wolf
+{
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsTreeManagerSlidingWindowDualRate)
+WOLF_PTR_TYPEDEFS(TreeManagerSlidingWindowDualRate)
+
+struct ParamsTreeManagerSlidingWindowDualRate : public ParamsTreeManagerSlidingWindow
+{
+        ParamsTreeManagerSlidingWindowDualRate() = default;
+        ParamsTreeManagerSlidingWindowDualRate(std::string _unique_name, const wolf::ParamsServer & _server) :
+            ParamsTreeManagerSlidingWindow(_unique_name, _server)
+        {
+            n_frames_recent = _server.getParam<unsigned int>(prefix + "/n_frames_recent");
+            assert(n_frames_recent <= n_frames);
+            rate_old_frames = _server.getParam<unsigned int>(prefix + "/rate_old_frames");
+        }
+        std::string print() const
+        {
+            return "\n" + ParamsTreeManagerBase::print()                            + "\n"
+                        + "n_frames_recent: "   + std::to_string(n_frames_recent)   + "\n"
+                        + "rate_old_frames: "   + std::to_string(rate_old_frames)   + "\n";
+        }
+
+        unsigned int n_frames_recent, rate_old_frames;
+};
+
+class TreeManagerSlidingWindowDualRate : public TreeManagerSlidingWindow
+{
+    public:
+        TreeManagerSlidingWindowDualRate(ParamsTreeManagerSlidingWindowDualRatePtr _params);
+        ;
+        WOLF_TREE_MANAGER_CREATE(TreeManagerSlidingWindowDualRate, ParamsTreeManagerSlidingWindowDualRate)
+
+        ~TreeManagerSlidingWindowDualRate() override{}
+
+        void keyFrameCallback(FrameBasePtr _key_frame) override;
+
+    protected:
+        ParamsTreeManagerSlidingWindowDualRatePtr params_swdr_;
+        unsigned int count_frames_;
+        //TrajectoryIter first_recent_frame_it_;
+};
+
+} /* namespace wolf */
+
+#endif /* INCLUDE_TREE_MANAGER_SLIDING_WINDOW_DUAL_RATE_H_ */

src/tree_manager/tree_manager_sliding_window.cpp

@@ -3,32 +3,22 @@
 namespace wolf
 {
 
-void TreeManagerSlidingWindow::keyFrameCallback(FrameBasePtr _key_frame)
+void TreeManagerSlidingWindow::keyFrameCallback(FrameBasePtr _frame)
 {
-    int n_kf(0);
-    FrameBasePtr first_KF(nullptr), second_KF(nullptr);
-    for (auto frm : *getProblem()->getTrajectory())
-    {
-        if (frm->isKey())
-        {
-            n_kf++;
-            if (first_KF == nullptr)
-                first_KF = frm;
-            else if (second_KF == nullptr)
-                second_KF = frm;
-        }
-    }
+    int n_f = getProblem()->getTrajectory()->getFrameMap().size(); // in trajectory there are only key frames
 
-    // remove first KF if too many KF
-    if (n_kf > params_sw_->n_key_frames)
+    // remove first frame if too many frames
+    if (n_f > params_sw_->n_frames)
     {
-        WOLF_DEBUG("TreeManagerSlidingWindow removing first frame");
-        first_KF->remove(params_sw_->viral_remove_empty_parent);
-        if (params_sw_->fix_first_key_frame)
+        if (params_sw_->fix_first_frame)
         {
             WOLF_DEBUG("TreeManagerSlidingWindow fixing new first frame");
-            second_KF->fix();
+            auto second_frame = std::next(getProblem()->getTrajectory()->begin())->second;
+            if (second_frame)
+                second_frame->fix();
         }
+        WOLF_DEBUG("TreeManagerSlidingWindow removing first frame");
+        getProblem()->getTrajectory()->getFirstFrame()->remove(params_sw_->viral_remove_empty_parent);
     }
 }
 

src/tree_manager/tree_manager_sliding_window_dual_rate.cpp

+#include "core/tree_manager/tree_manager_sliding_window_dual_rate.h"
+#include "core/capture/capture_motion.h"
+#include "core/processor/processor_motion.h"
+
+namespace wolf
+{
+
+TreeManagerSlidingWindowDualRate::TreeManagerSlidingWindowDualRate(ParamsTreeManagerSlidingWindowDualRatePtr _params) :
+        TreeManagerSlidingWindow(_params),
+        params_swdr_(_params),
+        count_frames_(0)
+{
+    NodeBase::setType("TreeManagerSlidingWindowDualRate");
+}
+
+void TreeManagerSlidingWindowDualRate::keyFrameCallback(FrameBasePtr _key_frame)
+{
+    int n_f = getProblem()->getTrajectory()->getFrameMap().size(); // in trajectory there are only key frames
+
+    // recent segment not complete
+    if (n_f <= params_swdr_->n_frames_recent)
+        return;
+
+
+    // REMOVE FIRST RECENT FRAME: all recent frames except one of each rate_old_frames
+    if (count_frames_ != 0)
+    {
+        FrameBasePtr remove_recent_frame    = std::next(getProblem()->getTrajectory()->rbegin(),
+                                                        params_swdr_->n_frames_recent)->second;
+        FrameBasePtr keep_recent_frame      = std::next(getProblem()->getTrajectory()->rbegin(),
+                                                        params_swdr_->n_frames_recent - 1)->second;
+
+        // compose motion captures for all processors motion
+        for (auto is_motion : getProblem()->getProcessorIsMotionList())
+        {
+            auto proc_motion = std::dynamic_pointer_cast<ProcessorMotion>(is_motion);
+            if (proc_motion == nullptr)
+                continue;
+
+            auto cap_prev = std::static_pointer_cast<CaptureMotion>(remove_recent_frame->getCaptureOf(proc_motion->getSensor()));
+            auto cap_next = std::static_pointer_cast<CaptureMotion>(keep_recent_frame->getCaptureOf(proc_motion->getSensor()));
+
+            // merge captures (if exist)
+            if (cap_prev and cap_next)
+            {
+                assert(cap_next->getOriginCapture() == cap_prev);
+                proc_motion->mergeCaptures(cap_prev, cap_next);
+            }
+        }
+
+        // remove frame
+        remove_recent_frame->remove(params_swdr_->viral_remove_empty_parent);
+    }
+    // REMOVE OLDEST FRAME: when first recent frame is kept, remove oldest frame (if max frames reached)
+    else if (n_f > params_swdr_->n_frames)
+    {
+        if (params_swdr_->fix_first_frame)
+        {
+            WOLF_DEBUG("TreeManagerSlidingWindow fixing new first frame");
+            auto second_frame = *std::next(getProblem()->getTrajectory()->begin());
+            if (second_frame)
+                second_frame->fix();
+        }
+        WOLF_DEBUG("TreeManagerSlidingWindow removing first frame");
+        getProblem()->getTrajectory()->getFirstFrame()->remove(params_swdr_->viral_remove_empty_parent);
+    }
+
+    // iterate counter
+    count_frames_++;
+    if (count_frames_ == params_swdr_->rate_old_frames)
+        count_frames_ = 0;
+}
+
+
+} /* namespace wolf */
+
+// Register in the FactoryTreeManager
+#include "core/tree_manager/factory_tree_manager.h"
+namespace wolf {
+WOLF_REGISTER_TREE_MANAGER(TreeManagerSlidingWindowDualRate);
+WOLF_REGISTER_TREE_MANAGER_AUTO(TreeManagerSlidingWindowDualRate);
+} // namespace wolf
+

test/gtest_tree_manager_sliding_window.cpp

@@ -24,7 +24,7 @@ TEST(TreeManagerSlidingWindow, make_shared)
 
     P->setTreeManager(GM);
 
-    ASSERT_EQ(P->getTreeManager(), GM);
+    EXPECT_EQ(P->getTreeManager(), GM);
 }
 
 TEST(TreeManagerSlidingWindow, createParams)
@@ -35,12 +35,12 @@ TEST(TreeManagerSlidingWindow, createParams)
 
     auto GM = TreeManagerSlidingWindow::create("tree_manager", ParamsGM);
 
-    ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(GM) != nullptr);
+    EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(GM) != nullptr);
 
     P->setTreeManager(GM);
 
-    ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(P->getTreeManager()) != nullptr);
-    ASSERT_EQ(P->getTreeManager(), GM);
+    EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(P->getTreeManager()) != nullptr);
+    EXPECT_EQ(P->getTreeManager(), GM);
 }
 
 TEST(TreeManagerSlidingWindow, createParamServer)
@@ -52,12 +52,12 @@ TEST(TreeManagerSlidingWindow, createParamServer)
 
     auto GM = TreeManagerSlidingWindow::create("tree_manager", server);
 
-    ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(GM) != nullptr);
+    EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(GM) != nullptr);
 
     P->setTreeManager(GM);
 
-    ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(P->getTreeManager()) != nullptr);
-    ASSERT_EQ(P->getTreeManager(), GM);
+    EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(P->getTreeManager()) != nullptr);
+    EXPECT_EQ(P->getTreeManager(), GM);
 }
 
 TEST(TreeManagerSlidingWindow, autoConf)
@@ -67,7 +67,7 @@ TEST(TreeManagerSlidingWindow, autoConf)
 
     ProblemPtr P = Problem::autoSetup(server);
 
-    ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(P->getTreeManager()) != nullptr);
+    EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(P->getTreeManager()) != nullptr);
 }
 
 TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
@@ -80,7 +80,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
 
     // FRAME 1 ----------------------------------------------------------
     auto F1 = P->getTrajectory()->getLastFrame();
-    ASSERT_TRUE(F1 != nullptr);
+    EXPECT_TRUE(F1 != nullptr);
 
     Vector7d state = F1->getStateVector();
     Vector7d zero_disp(state);
@@ -100,7 +100,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
     auto c12 = FactorBase::emplace<FactorOdom3d>(f12, f12, F1, nullptr, false);
 
     // Check no frame removed
-    ASSERT_FALSE(F1->isRemoving());
+    EXPECT_FALSE(F1->isRemoving());
 
     // FRAME 3 ----------------------------------------------------------
     auto F3 = P->emplaceKeyFrame(TimeStamp(3), "PO", 3,    state);
@@ -116,7 +116,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
     auto c23 = FactorBase::emplace<FactorOdom3d>(f23, f23, F2, nullptr, false);
 
     // Check no frame removed
-    ASSERT_FALSE(F1->isRemoving());
+    EXPECT_FALSE(F1->isRemoving());
 
     // FRAME 4 ----------------------------------------------------------
     auto F4 = P->emplaceKeyFrame(TimeStamp(4), "PO", 3,    state);
@@ -132,11 +132,11 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
     auto c34 = FactorBase::emplace<FactorOdom3d>(f34, f34, F3, nullptr, false);
 
     // Checks
-    ASSERT_TRUE(F1->isRemoving());
-    ASSERT_TRUE(c12->isRemoving());
-    ASSERT_TRUE(C12->isRemoving()); //Virally removed
-    ASSERT_TRUE(f12->isRemoving()); //Virally removed
-    ASSERT_TRUE(F2->isFixed()); //Fixed
+    EXPECT_TRUE(F1->isRemoving());
+    EXPECT_TRUE(c12->isRemoving());
+    EXPECT_TRUE(C12->isRemoving()); //Virally removed
+    EXPECT_TRUE(f12->isRemoving()); //Virally removed
+    EXPECT_TRUE(F2->isFixed()); //Fixed
 
     // FRAME 5 ----------------------------------------------------------
     auto F5 = P->emplaceKeyFrame(TimeStamp(5), "PO", 3,    state);
@@ -152,18 +152,18 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
     auto c45 = FactorBase::emplace<FactorOdom3d>(f45, f45, F4, nullptr, false);
 
     // Checks
-    ASSERT_TRUE(F1->isRemoving());
-    ASSERT_TRUE(c12->isRemoving());
-    ASSERT_TRUE(C12->isRemoving()); //Virally removed
-    ASSERT_TRUE(f12->isRemoving()); //Virally removed
-    ASSERT_TRUE(F2->isRemoving());
-    ASSERT_TRUE(c2->isRemoving());
-    ASSERT_TRUE(C2->isRemoving()); //Virally removed
-    ASSERT_TRUE(f2->isRemoving()); //Virally removed
-    ASSERT_TRUE(c23->isRemoving());
-    ASSERT_TRUE(C23->isRemoving()); //Virally removed
-    ASSERT_TRUE(f23->isRemoving()); //Virally removed
-    ASSERT_TRUE(F3->isFixed()); //Fixed
+    EXPECT_TRUE(F1->isRemoving());
+    EXPECT_TRUE(c12->isRemoving());
+    EXPECT_TRUE(C12->isRemoving()); //Virally removed
+    EXPECT_TRUE(f12->isRemoving()); //Virally removed
+    EXPECT_TRUE(F2->isRemoving());
+    EXPECT_TRUE(c2->isRemoving());
+    EXPECT_TRUE(C2->isRemoving()); //Virally removed
+    EXPECT_TRUE(f2->isRemoving()); //Virally removed
+    EXPECT_TRUE(c23->isRemoving());
+    EXPECT_TRUE(C23->isRemoving()); //Virally removed
+    EXPECT_TRUE(f23->isRemoving()); //Virally removed
+    EXPECT_TRUE(F3->isFixed()); //Fixed
 }
 
 TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
@@ -176,7 +176,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
 
     // FRAME 1 (prior) ----------------------------------------------------------
     auto F1 = P->getTrajectory()->getLastFrame();
-    ASSERT_TRUE(F1 != nullptr);
+    EXPECT_TRUE(F1 != nullptr);
 
     Vector7d state = F1->getStateVector();
     Vector7d zero_disp(state);
@@ -196,7 +196,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
     auto c12 = FactorBase::emplace<FactorOdom3d>(f12, f12, F1, nullptr, false);
 
     // Check no frame removed
-    ASSERT_FALSE(F1->isRemoving());
+    EXPECT_FALSE(F1->isRemoving());
 
     // FRAME 3 ----------------------------------------------------------
     auto F3 = P->emplaceKeyFrame(TimeStamp(3), "PO", 3,    state);
@@ -212,7 +212,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
     auto c23 = FactorBase::emplace<FactorOdom3d>(f23, f23, F2, nullptr, false);
 
     // Check no frame removed
-    ASSERT_FALSE(F1->isRemoving());
+    EXPECT_FALSE(F1->isRemoving());
 
     // FRAME 4 ----------------------------------------------------------
     auto F4 = P->emplaceKeyFrame(TimeStamp(4), "PO", 3,    state);
@@ -228,11 +228,11 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
     auto c34 = FactorBase::emplace<FactorOdom3d>(f34, f34, F3, nullptr, false);
 
     // Checks
-    ASSERT_TRUE(F1->isRemoving());
-    ASSERT_TRUE(c12->isRemoving());
-    ASSERT_FALSE(C12->isRemoving()); //Not virally removed
-    ASSERT_FALSE(f12->isRemoving()); //Not virally removed
-    ASSERT_FALSE(F2->isFixed()); //Not fixed
+    EXPECT_TRUE(F1->isRemoving());
+    EXPECT_TRUE(c12->isRemoving());
+    EXPECT_FALSE(C12->isRemoving()); //Not virally removed
+    EXPECT_FALSE(f12->isRemoving()); //Not virally removed
+    EXPECT_FALSE(F2->isFixed()); //Not fixed
 
     // FRAME 5 ----------------------------------------------------------
     auto F5 = P->emplaceKeyFrame(TimeStamp(5), "PO", 3,    state);
@@ -248,18 +248,18 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
     auto c45 = FactorBase::emplace<FactorOdom3d>(f45, f45, F4, nullptr, false);
 
     // Checks
-    ASSERT_TRUE(F1->isRemoving());
-    ASSERT_TRUE(c12->isRemoving());
-    ASSERT_TRUE(C12->isRemoving());
-    ASSERT_TRUE(f12->isRemoving());
-    ASSERT_TRUE(F2->isRemoving());
-    ASSERT_TRUE(c2->isRemoving());
-    ASSERT_TRUE(C2->isRemoving());
-    ASSERT_TRUE(f2->isRemoving());
-    ASSERT_TRUE(c23->isRemoving());
-    ASSERT_FALSE(C23->isRemoving()); //Not virally removed
-    ASSERT_FALSE(f23->isRemoving()); //Not virally removed
-    ASSERT_FALSE(F3->isFixed()); //Not fixed
+    EXPECT_TRUE(F1->isRemoving());
+    EXPECT_TRUE(c12->isRemoving());
+    EXPECT_TRUE(C12->isRemoving());
+    EXPECT_TRUE(f12->isRemoving());
+    EXPECT_TRUE(F2->isRemoving());
+    EXPECT_TRUE(c2->isRemoving());
+    EXPECT_TRUE(C2->isRemoving());
+    EXPECT_TRUE(f2->isRemoving());
+    EXPECT_TRUE(c23->isRemoving());
+    EXPECT_FALSE(C23->isRemoving()); //Not virally removed
+    EXPECT_FALSE(f23->isRemoving()); //Not virally removed
+    EXPECT_FALSE(F3->isFixed()); //Not fixed
 }
 
 int main(int argc, char **argv)

test/yaml/params_tree_manager_sliding_window1.yaml

@@ -15,8 +15,8 @@ config:
       time_tolerance: 0.1
     tree_manager:
       type: "TreeManagerSlidingWindow"
-      n_key_frames: 3
-      fix_first_key_frame: true
+      n_frames: 3
+      fix_first_frame: true
       viral_remove_empty_parent: true
   sensors: 
     -

test/yaml/params_tree_manager_sliding_window2.yaml

@@ -15,8 +15,8 @@ config:
       time_tolerance: 0.1
     tree_manager:
       type: "TreeManagerSlidingWindow"
-      n_key_frames: 3
-      fix_first_key_frame: false
+      n_frames: 3
+      fix_first_frame: false
       viral_remove_empty_parent: false
   sensors: 
     -