sliding window with n_fix_first_frames working

include/core/tree_manager/tree_manager_sliding_window.h

@@ -16,19 +16,21 @@ struct ParamsTreeManagerSlidingWindow : public ParamsTreeManagerBase
             ParamsTreeManagerBase(_unique_name, _server)
         {
             n_frames                    = _server.getParam<unsigned int>(prefix + "/n_frames");
-            fix_first_frame             = _server.getParam<bool>        (prefix + "/fix_first_frame");
+            n_fix_first_frames          = _server.getParam<unsigned int>(prefix + "/n_fix_first_frames");
             viral_remove_empty_parent   = _server.getParam<bool>        (prefix + "/viral_remove_empty_parent");
+            if (n_frames <= n_fix_first_frames)
+                throw std::runtime_error("TreeManagerSlidingWindow: Wrong parameter value. 'n_fix_first_frames' should be lower than 'n_frames'!");
         }
         std::string print() const override
         {
             return  ParamsTreeManagerBase::print()                                            + "\n"
                         + "n_frames: "                  + std::to_string(n_frames)                  + "\n"
-                        + "fix_first_frame: "           + std::to_string(fix_first_frame)           + "\n"
+                        + "fix_first_frame: "           + std::to_string(n_fix_first_frames)        + "\n"
                         + "viral_remove_empty_parent: " + std::to_string(viral_remove_empty_parent) + "\n";
         }
 
         unsigned int n_frames;
-        bool fix_first_frame;
+        unsigned int n_fix_first_frames;
         bool viral_remove_empty_parent;
 };
 

include/core/tree_manager/tree_manager_sliding_window_dual_rate.h

@@ -38,7 +38,7 @@ class TreeManagerSlidingWindowDualRate : public TreeManagerSlidingWindow
 
         ~TreeManagerSlidingWindowDualRate() override{}
 
-        void keyFrameCallback(FrameBasePtr _key_frame) override;
+        void keyFrameCallback(FrameBasePtr _frame) override;
 
     protected:
         ParamsTreeManagerSlidingWindowDualRatePtr params_swdr_;

src/tree_manager/tree_manager_sliding_window.cpp

@@ -5,18 +5,34 @@ namespace wolf
 
 void TreeManagerSlidingWindow::keyFrameCallback(FrameBasePtr _frame)
 {
-    int n_f = getProblem()->getTrajectory()->getFrameMap().size(); // in trajectory there are only key frames
+    int n_f = getProblem()->getTrajectory()->getFrameMap().size();
+    bool remove_first = (n_f > params_sw_->n_frames);
+    int n_fix = (n_f >= params_sw_->n_frames ?
+                 params_sw_->n_fix_first_frames :
+                 n_f - (params_sw_->n_frames - params_sw_->n_fix_first_frames));
 
-    // remove first frame if too many frames
-    if (n_f > params_sw_->n_frames)
+    auto frame = (remove_first ?
+                  getProblem()->getTrajectory()->getFirstFrame()->getNextFrame() :
+                  getProblem()->getTrajectory()->getFirstFrame());
+    int fixed_frames = 0;
+
+    // Fix n_fix first frames
+    while (fixed_frames < n_fix)
     {
-        if (params_sw_->fix_first_frame)
+        if (not frame)
+            break;
+        if (not frame->isFixed())
         {
-            WOLF_DEBUG("TreeManagerSlidingWindow fixing new first frame");
-            auto second_frame = std::next(getProblem()->getTrajectory()->begin())->second;
-            if (second_frame)
-                second_frame->fix();
+            WOLF_DEBUG("TreeManagerSlidingWindow fixing frame ", frame->id());
+            frame->fix();
         }
+        frame = frame->getNextFrame();
+        fixed_frames++;
+    }
+
+    // Remove first frame
+    if (remove_first)
+    {
         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

@@ -13,7 +13,7 @@ TreeManagerSlidingWindowDualRate::TreeManagerSlidingWindowDualRate(ParamsTreeMan
     NodeBase::setType("TreeManagerSlidingWindowDualRate");
 }
 
-void TreeManagerSlidingWindowDualRate::keyFrameCallback(FrameBasePtr _key_frame)
+void TreeManagerSlidingWindowDualRate::keyFrameCallback(FrameBasePtr _frame)
 {
     int n_f = getProblem()->getTrajectory()->getFrameMap().size(); // in trajectory there are only key frames
 
@@ -21,7 +21,6 @@ void TreeManagerSlidingWindowDualRate::keyFrameCallback(FrameBasePtr _key_frame)
     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)
     {
@@ -48,25 +47,14 @@ void TreeManagerSlidingWindowDualRate::keyFrameCallback(FrameBasePtr _key_frame)
                 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);
-    }
+    // Call tree manager sliding window
+    // It will remove oldest frame if tfirst recent frame has been kept
+    TreeManagerSlidingWindow::keyFrameCallback(_frame);
 
     // iterate counter
     count_frames_++;

test/gtest_tree_manager_sliding_window.cpp

@@ -72,6 +72,9 @@ TEST(TreeManagerSlidingWindow, autoConf)
 
 TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
 {
+    // window size: 3
+    // first 2 frames fixed
+
     ParserYaml parser = ParserYaml("test/yaml/params_tree_manager_sliding_window1.yaml", wolf_root);
     ParamsServer server = ParamsServer(parser.getParams());
 
@@ -101,6 +104,9 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
 
     // Check no frame removed
     EXPECT_FALSE(F1->isRemoving());
+    // Check F1 fixed
+    EXPECT_TRUE(F1->isFixed());
+    EXPECT_FALSE(F2->isFixed());
 
     // FRAME 3 ----------------------------------------------------------
     auto F3 = P->emplaceFrame(TimeStamp(3), "PO", 3,    state);
@@ -117,6 +123,10 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
 
     // Check no frame removed
     EXPECT_FALSE(F1->isRemoving());
+    // Check F1 and F2 fixed
+    EXPECT_TRUE(F1->isFixed());
+    EXPECT_TRUE(F2->isFixed());
+    EXPECT_FALSE(F3->isFixed());
 
     // FRAME 4 ----------------------------------------------------------
     auto F4 = P->emplaceFrame(TimeStamp(4), "PO", 3,    state);
@@ -131,12 +141,15 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
     auto f34 = FeatureBase::emplace<FeatureBase>(C34, "odom", zero_disp, cov);
     auto c34 = FactorBase::emplace<FactorOdom3d>(f34, f34, F3, nullptr, false);
 
-    // Checks
+    // Check F1 (virally) removed
     EXPECT_TRUE(F1->isRemoving());
     EXPECT_TRUE(c12->isRemoving()); // Factor removed because involves removed frame
     EXPECT_TRUE(C12->isRemoving()); // Virally removed
     EXPECT_TRUE(f12->isRemoving()); // Virally removed
-    EXPECT_TRUE(F2->isFixed()); //Fixed
+    // Check F2 and F3 fixed
+    EXPECT_TRUE(F2->isFixed());
+    EXPECT_TRUE(F3->isFixed());
+    EXPECT_FALSE(F4->isFixed());
 
     // FRAME 5 ----------------------------------------------------------
     auto F5 = P->emplaceFrame(TimeStamp(5), "PO", 3,    state);
@@ -151,7 +164,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
     auto f45 = FeatureBase::emplace<FeatureBase>(C45, "odom", zero_disp, cov);
     auto c45 = FactorBase::emplace<FactorOdom3d>(f45, f45, F4, nullptr, false);
 
-    // Checks
+    // Check F1 and F2 (virally) removed
     EXPECT_TRUE(F1->isRemoving());
     EXPECT_TRUE(c12->isRemoving()); // Factor removed because involves removed frame
     EXPECT_TRUE(C12->isRemoving()); // Virally removed
@@ -163,8 +176,10 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
     EXPECT_TRUE(c23->isRemoving()); // Factor removed because involves removed frame
     EXPECT_TRUE(C23->isRemoving()); // Virally removed
     EXPECT_TRUE(f23->isRemoving()); // Virally removed
-
-    EXPECT_TRUE(F3->isFixed()); //Fixed
+    // Check F3 and F4 fixed
+    EXPECT_TRUE(F3->isFixed());
+    EXPECT_TRUE(F4->isFixed());
+    EXPECT_FALSE(F5->isFixed());
 }
 
 TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
@@ -198,6 +213,9 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
 
     // Check no frame removed
     EXPECT_FALSE(F1->isRemoving());
+    // Check no frames fixed
+    EXPECT_FALSE(F1->isFixed());
+    EXPECT_FALSE(F2->isFixed());
 
     // FRAME 3 ----------------------------------------------------------
     auto F3 = P->emplaceFrame(TimeStamp(3), "PO", 3,    state);
@@ -214,6 +232,10 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
 
     // Check no frame removed
     EXPECT_FALSE(F1->isRemoving());
+    // Check no frames fixed
+    EXPECT_FALSE(F1->isFixed());
+    EXPECT_FALSE(F2->isFixed());
+    EXPECT_FALSE(F3->isFixed());
 
     // FRAME 4 ----------------------------------------------------------
     auto F4 = P->emplaceFrame(TimeStamp(4), "PO", 3,    state);
@@ -233,7 +255,10 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
     EXPECT_TRUE(c12->isRemoving()); // Factor removed because involves removed frame
     EXPECT_FALSE(C12->isRemoving()); //Not virally removed
     EXPECT_FALSE(f12->isRemoving()); //Not virally removed
-    EXPECT_FALSE(F2->isFixed()); //Not fixed
+    // Check no frames fixed
+    EXPECT_FALSE(F2->isFixed());
+    EXPECT_FALSE(F3->isFixed());
+    EXPECT_FALSE(F4->isFixed());
 
     // FRAME 5 ----------------------------------------------------------
     auto F5 = P->emplaceFrame(TimeStamp(5), "PO", 3,    state);
@@ -260,7 +285,10 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
     EXPECT_TRUE(c23->isRemoving()); // Factor removed because involves removed frame
     EXPECT_FALSE(C23->isRemoving()); //Not virally removed
     EXPECT_FALSE(f23->isRemoving()); //Not virally removed
-    EXPECT_FALSE(F3->isFixed()); //Not fixed
+    // Check no frames fixed
+    EXPECT_FALSE(F3->isFixed());
+    EXPECT_FALSE(F4->isFixed());
+    EXPECT_FALSE(F5->isFixed());
 }
 
 int main(int argc, char **argv)

test/gtest_tree_manager_sliding_window_dual_rate.cpp

@@ -76,6 +76,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
 {
     /* sliding window dual rate:
      *     n_frames: 5
+     *     n_fix_first_frames: 2
      *     n_frames_recent: 3
      *     rate_old_frames: 2
      */
@@ -90,6 +91,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
      *
      * Sliding window:
      * (  )   (  )   (  )(  )(F1)
+     * fix    fix
      */
     auto F1 = P->getTrajectory()->getLastFrame();
     ASSERT_TRUE(F1 != nullptr);
@@ -109,13 +111,14 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
     EXPECT_FALSE(c1->isRemoving());
     EXPECT_FALSE(C1->isRemoving());
     EXPECT_FALSE(f1->isRemoving());
-
+    // Check no frame fixed
     EXPECT_FALSE(F1->isFixed());
 
     /* FRAME 2 ----------------------------------------------------------
      *
      * Sliding window:
      * (  )   (  )   (  )(F1)(F2)
+     * fix    fix
      */
     auto F2 = P->emplaceFrame(TimeStamp(2), "PO", 3, state);
     P->keyFrameCallback(F2, nullptr, 0);
@@ -142,7 +145,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
     EXPECT_FALSE(c12->isRemoving());
     EXPECT_FALSE(C12->isRemoving());
     EXPECT_FALSE(f12->isRemoving());
-
+    // Check no frame fixed
     EXPECT_FALSE(F1->isFixed());
     EXPECT_FALSE(F2->isFixed());
 
@@ -150,6 +153,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
      *
      * Sliding window:
      * (  )   (  )   (F1)(F2)(F3)
+     * fix    fix
      */
     auto F3 = P->emplaceFrame(TimeStamp(3), "PO", 3,    state);
     P->keyFrameCallback(F3, nullptr, 0);
@@ -185,6 +189,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
     EXPECT_FALSE(C23->isRemoving());
     EXPECT_FALSE(f23->isRemoving());
 
+    // Check no frame fixed
     EXPECT_FALSE(F1->isFixed());
     EXPECT_FALSE(F2->isFixed());
     EXPECT_FALSE(F3->isFixed());
@@ -193,6 +198,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
      *
      * Sliding window:
      * (  )   (F1)(F2)(F3)(F4)
+     * fix    fix
      */
     auto F4 = P->emplaceFrame(TimeStamp(4), "PO", 3,    state);
     P->keyFrameCallback(F4, nullptr, 0);
@@ -236,7 +242,8 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
     EXPECT_FALSE(C34->isRemoving());
     EXPECT_FALSE(f34->isRemoving());
 
-    EXPECT_FALSE(F1->isFixed());
+    // Check F1 fixed
+    EXPECT_TRUE(F1->isFixed());
     EXPECT_FALSE(F2->isFixed());
     EXPECT_FALSE(F3->isFixed());
     EXPECT_FALSE(F4->isFixed());
@@ -245,6 +252,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
      *
      * Sliding window:
      * (  )   (F1)   (F3)(F4)(F5)
+     * fix    fix
      */
     auto F5 = P->emplaceFrame(TimeStamp(5), "PO", 3,    state);
     P->keyFrameCallback(F5, nullptr, 0);
@@ -296,7 +304,8 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
     EXPECT_FALSE(C45->isRemoving());
     EXPECT_FALSE(f45->isRemoving());
 
-    EXPECT_FALSE(F1->isFixed());
+    // Check F1 fixed
+    EXPECT_TRUE(F1->isFixed());
     EXPECT_FALSE(F3->isFixed());
     EXPECT_FALSE(F4->isFixed());
     EXPECT_FALSE(F5->isFixed());
@@ -305,6 +314,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
      *
      * Sliding window:
      * (F1)   (F3)(F4)(F5)(F6)
+     * fix    fix
      */
     auto F6 = P->emplaceFrame(TimeStamp(6), "PO", 3,    state);
     P->keyFrameCallback(F6, nullptr, 0);
@@ -364,8 +374,9 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
     EXPECT_FALSE(C56->isRemoving());
     EXPECT_FALSE(f56->isRemoving());
 
-    EXPECT_FALSE(F1->isFixed());
-    EXPECT_FALSE(F3->isFixed());
+    // Check F1 and F3 fixed
+    EXPECT_TRUE(F1->isFixed());
+    EXPECT_TRUE(F3->isFixed());
     EXPECT_FALSE(F4->isFixed());
     EXPECT_FALSE(F5->isFixed());
     EXPECT_FALSE(F6->isFixed());
@@ -374,6 +385,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
      *
      * Sliding window:
      * (F1)   (F3)   (F5)(F6)(F7)
+     * fix    fix
      */
     auto F7 = P->emplaceFrame(TimeStamp(7), "PO", 3,    state);
     P->keyFrameCallback(F7, nullptr, 0);
@@ -441,8 +453,9 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
     EXPECT_FALSE(C67->isRemoving());
     EXPECT_FALSE(f67->isRemoving());
 
-    EXPECT_FALSE(F1->isFixed());
-    EXPECT_FALSE(F3->isFixed());
+    // Check F1 and F3 fixed
+    EXPECT_TRUE(F1->isFixed());
+    EXPECT_TRUE(F3->isFixed());
     EXPECT_FALSE(F5->isFixed());
     EXPECT_FALSE(F6->isFixed());
     EXPECT_FALSE(F7->isFixed());
@@ -451,6 +464,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
      *
      * Sliding window:
      * (F3)   (F5)(F6)(F7)(F8)
+     * fix    fix
      */
     auto F8 = P->emplaceFrame(TimeStamp(8), "PO", 3,    state);
     P->keyFrameCallback(F8, nullptr, 0);
@@ -526,8 +540,9 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
     EXPECT_FALSE(C78->isRemoving());
     EXPECT_FALSE(f78->isRemoving());
 
+    // Check F1 and F3 fixed
     EXPECT_TRUE(F3->isFixed());
-    EXPECT_FALSE(F5->isFixed());
+    EXPECT_TRUE(F5->isFixed());
     EXPECT_FALSE(F6->isFixed());
     EXPECT_FALSE(F7->isFixed());
     EXPECT_FALSE(F8->isFixed());
@@ -537,6 +552,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowNoFixNoViral)
 {
     /* sliding window dual rate:
      *     n_frames: 5
+     *     n_fix_first_frames: 0
      *     n_frames_recent: 3
      *     rate_old_frames: 2
      */

test/yaml/params_tree_manager_sliding_window1.yaml

@@ -14,7 +14,7 @@ config:
     tree_manager:
       type: "TreeManagerSlidingWindow"
       n_frames: 3
-      fix_first_frame: true
+      n_fix_first_frames: 2
       viral_remove_empty_parent: true
   sensors: 
     -

test/yaml/params_tree_manager_sliding_window2.yaml

@@ -14,7 +14,7 @@ config:
     tree_manager:
       type: "TreeManagerSlidingWindow"
       n_frames: 3
-      fix_first_frame: false
+      n_fix_first_frames: 0
       viral_remove_empty_parent: false
   sensors: 
     -

test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml

@@ -16,5 +16,5 @@ config:
       n_frames: 5
       n_frames_recent: 3
       rate_old_frames: 2
-      fix_first_frame: true
+      n_fix_first_frames: 2
       viral_remove_empty_parent: true

test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml

@@ -16,5 +16,5 @@ config:
       n_frames: 5
       n_frames_recent: 3
       rate_old_frames: 2
-      fix_first_frame: false
+      n_fix_first_frames: 0
       viral_remove_empty_parent: false

test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml

@@ -22,7 +22,7 @@ config:
       n_frames: 5
       n_frames_recent: 3
       rate_old_frames: 2
-      fix_first_frame: true
+      n_fix_first_frames: 2
       viral_remove_empty_parent: true
   sensors: 
     -