From f0e6b1c8a9c6c5063dc06b2b4cb68406f3ae4cef Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu> Date: Mon, 1 Feb 2021 15:20:13 +0100 Subject: [PATCH] sliding window with n_fix_first_frames working --- .../tree_manager_sliding_window.h | 8 ++-- .../tree_manager_sliding_window_dual_rate.h | 2 +- .../tree_manager_sliding_window.cpp | 32 ++++++++++---- .../tree_manager_sliding_window_dual_rate.cpp | 20 ++------- test/gtest_tree_manager_sliding_window.cpp | 42 +++++++++++++++---- ..._tree_manager_sliding_window_dual_rate.cpp | 34 +++++++++++---- .../params_tree_manager_sliding_window1.yaml | 2 +- .../params_tree_manager_sliding_window2.yaml | 2 +- ...ree_manager_sliding_window_dual_rate1.yaml | 2 +- ...ree_manager_sliding_window_dual_rate2.yaml | 2 +- ...ree_manager_sliding_window_dual_rate3.yaml | 2 +- 11 files changed, 99 insertions(+), 49 deletions(-) diff --git a/include/core/tree_manager/tree_manager_sliding_window.h b/include/core/tree_manager/tree_manager_sliding_window.h index c2d06be8d..706a4092c 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 e810c11ba..f0e20f4b5 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 3c64b2ee8..7d10c3167 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 2009cd89e..54fadc44d 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 e8ffeb407..486ef3df9 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 fa1414aaf..dbc9f67c3 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 b83698dd0..704e8530e 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 0b9097d81..701fc6fbf 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 32ef665e0..a7f0f7434 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 6b4795fb3..cae3df67f 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 518d78ec1..47d81f409 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: - -- GitLab