diff --git a/include/core/tree_manager/tree_manager_sliding_window.h b/include/core/tree_manager/tree_manager_sliding_window.h index c2d06be8d17f1c2075b225b86ab7e09cf06a7aad..706a4092c5b0a46f520f57129906898db7c2525c 100644 --- a/include/core/tree_manager/tree_manager_sliding_window.h +++ b/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; }; diff --git a/include/core/tree_manager/tree_manager_sliding_window_dual_rate.h b/include/core/tree_manager/tree_manager_sliding_window_dual_rate.h index e810c11ba34699bbe9bc1537b5995d2670049862..f0e20f4b5eec08e6f322424f7353ca124b448e90 100644 --- a/include/core/tree_manager/tree_manager_sliding_window_dual_rate.h +++ b/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_; diff --git a/src/tree_manager/tree_manager_sliding_window.cpp b/src/tree_manager/tree_manager_sliding_window.cpp index 3c64b2ee87d2e26318788c4eddd547606edb6cb0..7d10c3167b5fe23aa279808e743bc4f8b4abb3da 100644 --- a/src/tree_manager/tree_manager_sliding_window.cpp +++ b/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); } diff --git a/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp b/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp index 2009cd89edf486af2efa892f58615a1e0fa05b0f..54fadc44d8ac752f8d522374e9ccdb8812241585 100644 --- a/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp +++ b/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_++; diff --git a/test/gtest_tree_manager_sliding_window.cpp b/test/gtest_tree_manager_sliding_window.cpp index e8ffeb407df4df15c00715b21a3e86d554b38da8..486ef3df9fccd76d266765a0486b1c09dbbc65d6 100644 --- a/test/gtest_tree_manager_sliding_window.cpp +++ b/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) diff --git a/test/gtest_tree_manager_sliding_window_dual_rate.cpp b/test/gtest_tree_manager_sliding_window_dual_rate.cpp index fa1414aafe81283cbc931b20498e3b737b13b533..dbc9f67c3aef8911b4568548d99f1722768ff598 100644 --- a/test/gtest_tree_manager_sliding_window_dual_rate.cpp +++ b/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 */ diff --git a/test/yaml/params_tree_manager_sliding_window1.yaml b/test/yaml/params_tree_manager_sliding_window1.yaml index b83698dd000ab1171d77c25d7e92aaeaa04fd9e1..704e8530ed2c8b8eb99ac35c29c36f62d01a3fb4 100644 --- a/test/yaml/params_tree_manager_sliding_window1.yaml +++ b/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: - diff --git a/test/yaml/params_tree_manager_sliding_window2.yaml b/test/yaml/params_tree_manager_sliding_window2.yaml index 0b9097d819dced956057db4d09b0096bc8752dd2..701fc6fbfb71e35c800563224cf7a86cce6d846b 100644 --- a/test/yaml/params_tree_manager_sliding_window2.yaml +++ b/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: - diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml index 32ef665e0a6d92b94e9342d22c5e4ca9952613f1..a7f0f7434fb8a71c74e3aa3f15b8dc9f6ea7c067 100644 --- a/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml +++ b/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 diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml index 6b4795fb3ad70c77b02c9cffc74b964abfe64141..cae3df67f036430481cd936ea31a9d2b4c0bca9a 100644 --- a/test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml +++ b/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 diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml index 518d78ec1d6d1f4f929ef5c33b77729d860ecc9e..47d81f409a5ed09d37ba3b307c2fb23152759791 100644 --- a/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml +++ b/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: -