diff --git a/CMakeLists.txt b/CMakeLists.txt index b84a4aa218a28e25b767d517f055f0c901bf8e8d..976709646ff5d2d052630537fe6178a4251c1653 100644 --- a/CMakeLists.txt +++ b/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 diff --git a/include/core/tree_manager/tree_manager_sliding_window.h b/include/core/tree_manager/tree_manager_sliding_window.h index 0ff5f31ed2734b7840518bf18dfc32073dac4cec..ce44fe003a8dfa58bb9a938e804af3e26fe576df 100644 --- a/include/core/tree_manager/tree_manager_sliding_window.h +++ b/include/core/tree_manager/tree_manager_sliding_window.h @@ -1,7 +1,7 @@ #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_; 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 new file mode 100644 index 0000000000000000000000000000000000000000..e9fcbd1332fd2d4eca3f7afa04c2aee167ad1980 --- /dev/null +++ b/include/core/tree_manager/tree_manager_sliding_window_dual_rate.h @@ -0,0 +1,51 @@ +#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_ */ diff --git a/src/tree_manager/tree_manager_sliding_window.cpp b/src/tree_manager/tree_manager_sliding_window.cpp index 623c7bec91214b55f7d2d8f2034dbba3e01c5ccb..3c64b2ee87d2e26318788c4eddd547606edb6cb0 100644 --- a/src/tree_manager/tree_manager_sliding_window.cpp +++ b/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); } } diff --git a/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp b/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp new file mode 100644 index 0000000000000000000000000000000000000000..31585d3c7bdee10ac723a087cfbde74fdb03e956 --- /dev/null +++ b/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp @@ -0,0 +1,83 @@ +#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 + diff --git a/test/gtest_tree_manager_sliding_window.cpp b/test/gtest_tree_manager_sliding_window.cpp index b8ff8240e7e2c7e97fb4f56aeadb72d2b51a0027..177209846280f0d8a626298126d69b1d68d55ed4 100644 --- a/test/gtest_tree_manager_sliding_window.cpp +++ b/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) diff --git a/test/yaml/params_tree_manager_sliding_window1.yaml b/test/yaml/params_tree_manager_sliding_window1.yaml index 96ad94f72af7e8de1b9200a8e5ea44e3bc952a85..2a8613dede994fe72b7fea8a5f1506c71981bbb0 100644 --- a/test/yaml/params_tree_manager_sliding_window1.yaml +++ b/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: - diff --git a/test/yaml/params_tree_manager_sliding_window2.yaml b/test/yaml/params_tree_manager_sliding_window2.yaml index 9ae33a2af5ab0695bd44948d33d2f9aa104cbc74..fda0a8a558ac3f9dec60ec869bc5d9c7d68ca6a5 100644 --- a/test/yaml/params_tree_manager_sliding_window2.yaml +++ b/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: -