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/processor/processor_motion.cpp b/src/processor/processor_motion.cpp index 3a770b6f10dac4cfd05494725674b32c1f783eea..d18ae7c312f4e19a98ed1ae9522b2b06aad2fd41 100644 --- a/src/processor/processor_motion.cpp +++ b/src/processor/processor_motion.cpp @@ -65,7 +65,6 @@ void ProcessorMotion::mergeCaptures(CaptureMotionConstPtr cap_prev, cap_target->getBuffer().insert(cap_target->getBuffer().begin(), cap_prev->getBuffer().begin(), cap_prev->getBuffer().end()); - cap_target->getBuffer().print(); // change origin cap_target->setOriginCapture(cap_prev->getOriginCapture()); 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..2009cd89edf486af2efa892f58615a1e0fa05b0f --- /dev/null +++ b/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp @@ -0,0 +1,86 @@ +#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) + { + WOLF_DEBUG("TreeManagerSlidingWindow removing the oldest of recent frames"); + 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) + { + WOLF_DEBUG("TreeManagerSlidingWindow merging capture ", cap_prev->id(), " with ", cap_next->id()); + 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/CMakeLists.txt b/test/CMakeLists.txt index b05f5636dd989dc18fed2a63d8b4b92047cffa55..4363e38a4d4df67a73ce92086b7a3ae071c4a561 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -267,6 +267,10 @@ ENDIF(Ceres_FOUND) wolf_add_gtest(gtest_tree_manager_sliding_window gtest_tree_manager_sliding_window.cpp) target_link_libraries(gtest_tree_manager_sliding_window ${PLUGIN_NAME}) +# TreeManagerSlidingWindowDualRate class test +wolf_add_gtest(gtest_tree_manager_sliding_window_dual_rate gtest_tree_manager_sliding_window_dual_rate.cpp) +target_link_libraries(gtest_tree_manager_sliding_window_dual_rate ${PLUGIN_NAME}) + # yaml conversions wolf_add_gtest(gtest_yaml_conversions gtest_yaml_conversions.cpp) target_link_libraries(gtest_yaml_conversions ${PLUGIN_NAME}) diff --git a/test/gtest_tree_manager_sliding_window.cpp b/test/gtest_tree_manager_sliding_window.cpp index b8ff8240e7e2c7e97fb4f56aeadb72d2b51a0027..0c5e7c2aec97277425289f3f28a9ba3d61dece1c 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()); // Factor removed because involves removed frame + 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,19 @@ 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()); // Factor removed because involves removed frame + EXPECT_TRUE(C12->isRemoving()); // Virally removed + EXPECT_TRUE(f12->isRemoving()); // Virally removed + EXPECT_TRUE(F2->isRemoving()); + EXPECT_TRUE(c2->isRemoving()); // Factor removed because involves removed frame + EXPECT_TRUE(C2->isRemoving()); // Virally removed + EXPECT_TRUE(f2->isRemoving()); // Virally removed + 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 } TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral) @@ -176,7 +177,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 +197,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 +213,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 +229,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()); // 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 // FRAME 5 ---------------------------------------------------------- auto F5 = P->emplaceKeyFrame(TimeStamp(5), "PO", 3, state); @@ -248,18 +249,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()); // Factor removed because involves removed frame + 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()); // 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 } 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 new file mode 100644 index 0000000000000000000000000000000000000000..ae4112bc3c27d88e103694ffd0824f4b7b35de2d --- /dev/null +++ b/test/gtest_tree_manager_sliding_window_dual_rate.cpp @@ -0,0 +1,1066 @@ +#include "core/utils/utils_gtest.h" + + +#include "core/problem/problem.h" +#include "core/tree_manager/tree_manager_sliding_window_dual_rate.h" +#include "core/yaml/parser_yaml.h" +#include "core/capture/capture_void.h" +#include "core/capture/capture_odom_3d.h" +#include "core/feature/feature_base.h" +#include "core/factor/factor_odom_3d.h" +#include "core/factor/factor_pose_3d.h" +#include "core/solver/factory_solver.h" + +using namespace wolf; +using namespace Eigen; + +std::string wolf_root = _WOLF_ROOT_DIR; + +TEST(TreeManagerSlidingWindowDualRate, make_shared) +{ + ProblemPtr P = Problem::create("PO", 2); + + auto ParamsGM = std::make_shared<ParamsTreeManagerSlidingWindowDualRate>(); + + auto GM = std::make_shared<TreeManagerSlidingWindowDualRate>(ParamsGM); + + P->setTreeManager(GM); + + EXPECT_EQ(P->getTreeManager(), GM); +} + +TEST(TreeManagerSlidingWindowDualRate, createParams) +{ + ProblemPtr P = Problem::create("PO", 2); + + auto ParamsGM = std::make_shared<ParamsTreeManagerSlidingWindowDualRate>(); + + auto GM = TreeManagerSlidingWindowDualRate::create("tree_manager", ParamsGM); + + EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(GM) != nullptr); + + P->setTreeManager(GM); + + EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(P->getTreeManager()) != nullptr); + EXPECT_EQ(P->getTreeManager(), GM); +} + +TEST(TreeManagerSlidingWindowDualRate, createParamServer) +{ + ProblemPtr P = Problem::create("PO", 2); + + ParserYaml parser = ParserYaml("test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml", wolf_root); + ParamsServer server = ParamsServer(parser.getParams()); + + auto GM = TreeManagerSlidingWindowDualRate::create("tree_manager", server); + + EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(GM) != nullptr); + + P->setTreeManager(GM); + + EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(P->getTreeManager()) != nullptr); + EXPECT_EQ(P->getTreeManager(), GM); +} + +TEST(TreeManagerSlidingWindowDualRate, autoConf) +{ + ParserYaml parser = ParserYaml("test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml", wolf_root); + ParamsServer server = ParamsServer(parser.getParams()); + + ProblemPtr P = Problem::autoSetup(server); + + EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(P->getTreeManager()) != nullptr); +} + +TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral) +{ + /* sliding window dual rate: + * n_frames: 5 + * n_frames_recent: 3 + * rate_old_frames: 2 + */ + + ParserYaml parser = ParserYaml("test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml", wolf_root); + ParamsServer server = ParamsServer(parser.getParams()); + + ProblemPtr P = Problem::autoSetup(server); + P->applyPriorOptions(0); + + /* FRAME 1 ---------------------------------------------------------- + * + * Sliding window: + * ( ) ( ) ( )( )(F1) + */ + auto F1 = P->getTrajectory()->getLastFrame(); + ASSERT_TRUE(F1 != nullptr); + ASSERT_FALSE(F1->getCaptureList().empty()); + auto C1 = F1->getCaptureList().front(); + ASSERT_FALSE(C1->getFeatureList().empty()); + auto f1 = C1->getFeatureList().front(); + ASSERT_FALSE(f1->getFactorList().empty()); + auto c1 = f1->getFactorList().front(); + + Vector7d state = F1->getStateVector(); + Vector7d zero_disp(state); + Matrix6d cov = Matrix6d::Identity(); + + // Check no frame removed + EXPECT_FALSE(F1->isRemoving()); + EXPECT_FALSE(c1->isRemoving()); + EXPECT_FALSE(C1->isRemoving()); + EXPECT_FALSE(f1->isRemoving()); + + EXPECT_FALSE(F1->isFixed()); + + /* FRAME 2 ---------------------------------------------------------- + * + * Sliding window: + * ( ) ( ) ( )(F1)(F2) + */ + auto F2 = P->emplaceKeyFrame(TimeStamp(2), "PO", 3, state); + P->keyFrameCallback(F2, nullptr, 0); + + // absolute factor + auto C2 = CaptureBase::emplace<CaptureVoid>(F2, TimeStamp(2), nullptr); + auto f2 = FeatureBase::emplace<FeatureBase>(C2, "absolute", state, cov); + auto c2 = FactorBase::emplace<FactorPose3d>(f2, f2, nullptr, false); + // displacement + auto C12 = CaptureBase::emplace<CaptureVoid>(F2, TimeStamp(2), nullptr); + auto f12 = FeatureBase::emplace<FeatureBase>(C12, "odom", zero_disp, cov); + auto c12 = FactorBase::emplace<FactorOdom3d>(f12, f12, F1, nullptr, false); + + // Check no frame removed + EXPECT_FALSE(F1->isRemoving()); + EXPECT_FALSE(c1->isRemoving()); + EXPECT_FALSE(C1->isRemoving()); + EXPECT_FALSE(f1->isRemoving()); + + EXPECT_FALSE(F2->isRemoving()); + EXPECT_FALSE(c2->isRemoving()); + EXPECT_FALSE(C2->isRemoving()); + EXPECT_FALSE(f2->isRemoving()); + EXPECT_FALSE(c12->isRemoving()); + EXPECT_FALSE(C12->isRemoving()); + EXPECT_FALSE(f12->isRemoving()); + + EXPECT_FALSE(F1->isFixed()); + EXPECT_FALSE(F2->isFixed()); + + /* FRAME 3 ---------------------------------------------------------- + * + * Sliding window: + * ( ) ( ) (F1)(F2)(F3) + */ + auto F3 = P->emplaceKeyFrame(TimeStamp(3), "PO", 3, state); + P->keyFrameCallback(F3, nullptr, 0); + + // absolute factor + auto C3 = CaptureBase::emplace<CaptureVoid>(F3, TimeStamp(3), nullptr); + auto f3 = FeatureBase::emplace<FeatureBase>(C3, "absolute", state, cov); + auto c3 = FactorBase::emplace<FactorPose3d>(f3, f3, nullptr, false); + // displacement + auto C23 = CaptureBase::emplace<CaptureVoid>(F3, TimeStamp(3), nullptr); + auto f23 = FeatureBase::emplace<FeatureBase>(C23, "odom", zero_disp, cov); + auto c23 = FactorBase::emplace<FactorOdom3d>(f23, f23, F2, nullptr, false); + + // Check no frame removed + EXPECT_FALSE(F1->isRemoving()); + EXPECT_FALSE(c1->isRemoving()); + EXPECT_FALSE(C1->isRemoving()); + EXPECT_FALSE(f1->isRemoving()); + + EXPECT_FALSE(F2->isRemoving()); + EXPECT_FALSE(c2->isRemoving()); + EXPECT_FALSE(C2->isRemoving()); + EXPECT_FALSE(f2->isRemoving()); + EXPECT_FALSE(c12->isRemoving()); + EXPECT_FALSE(C12->isRemoving()); + EXPECT_FALSE(f12->isRemoving()); + + EXPECT_FALSE(F3->isRemoving()); + EXPECT_FALSE(c3->isRemoving()); + EXPECT_FALSE(C3->isRemoving()); + EXPECT_FALSE(f3->isRemoving()); + EXPECT_FALSE(c23->isRemoving()); + EXPECT_FALSE(C23->isRemoving()); + EXPECT_FALSE(f23->isRemoving()); + + EXPECT_FALSE(F1->isFixed()); + EXPECT_FALSE(F2->isFixed()); + EXPECT_FALSE(F3->isFixed()); + + /* FRAME 4 ---------------------------------------------------------- + * + * Sliding window: + * ( ) (F1)(F2)(F3)(F4) + */ + auto F4 = P->emplaceKeyFrame(TimeStamp(4), "PO", 3, state); + P->keyFrameCallback(F4, nullptr, 0); + + // absolute factor + auto C4 = CaptureBase::emplace<CaptureVoid>(F4, TimeStamp(4), nullptr); + auto f4 = FeatureBase::emplace<FeatureBase>(C4, "absolute", state, cov); + auto c4 = FactorBase::emplace<FactorPose3d>(f4, f4, nullptr, false); + // displacement + auto C34 = CaptureBase::emplace<CaptureVoid>(F4, TimeStamp(4), nullptr); + auto f34 = FeatureBase::emplace<FeatureBase>(C34, "odom", zero_disp, cov); + auto c34 = FactorBase::emplace<FactorOdom3d>(f34, f34, F3, nullptr, false); + + // Checks + EXPECT_FALSE(F1->isRemoving()); + EXPECT_FALSE(c1->isRemoving()); + EXPECT_FALSE(C1->isRemoving()); + EXPECT_FALSE(f1->isRemoving()); + + EXPECT_FALSE(F2->isRemoving()); + EXPECT_FALSE(c2->isRemoving()); + EXPECT_FALSE(C2->isRemoving()); + EXPECT_FALSE(f2->isRemoving()); + EXPECT_FALSE(c12->isRemoving()); + EXPECT_FALSE(C12->isRemoving()); + EXPECT_FALSE(f12->isRemoving()); + + EXPECT_FALSE(F3->isRemoving()); + EXPECT_FALSE(c3->isRemoving()); + EXPECT_FALSE(C3->isRemoving()); + EXPECT_FALSE(f3->isRemoving()); + EXPECT_FALSE(c23->isRemoving()); + EXPECT_FALSE(C23->isRemoving()); + EXPECT_FALSE(f23->isRemoving()); + + EXPECT_FALSE(F4->isRemoving()); + EXPECT_FALSE(c4->isRemoving()); + EXPECT_FALSE(C4->isRemoving()); + EXPECT_FALSE(f4->isRemoving()); + EXPECT_FALSE(c34->isRemoving()); + EXPECT_FALSE(C34->isRemoving()); + EXPECT_FALSE(f34->isRemoving()); + + EXPECT_FALSE(F1->isFixed()); + EXPECT_FALSE(F2->isFixed()); + EXPECT_FALSE(F3->isFixed()); + EXPECT_FALSE(F4->isFixed()); + + /* FRAME 5 ---------------------------------------------------------- + * + * Sliding window: + * ( ) (F1) (F3)(F4)(F5) + */ + auto F5 = P->emplaceKeyFrame(TimeStamp(5), "PO", 3, state); + P->keyFrameCallback(F5, nullptr, 0); + + // absolute factor + auto C5 = CaptureBase::emplace<CaptureVoid>(F5, TimeStamp(5), nullptr); + auto f5 = FeatureBase::emplace<FeatureBase>(C5, "absolute", state, cov); + auto c5 = FactorBase::emplace<FactorPose3d>(f5, f5, nullptr, false); + // displacement + auto C45 = CaptureBase::emplace<CaptureVoid>(F5, TimeStamp(5), nullptr); + auto f45 = FeatureBase::emplace<FeatureBase>(C45, "odom", zero_disp, cov); + auto c45 = FactorBase::emplace<FactorOdom3d>(f45, f45, F4, nullptr, false); + + // Checks + EXPECT_FALSE(F1->isRemoving()); + EXPECT_FALSE(c1->isRemoving()); + EXPECT_FALSE(C1->isRemoving()); + EXPECT_FALSE(f1->isRemoving()); + + EXPECT_TRUE(F2->isRemoving()); + EXPECT_TRUE(c2->isRemoving()); + EXPECT_TRUE(C2->isRemoving()); + EXPECT_TRUE(f2->isRemoving()); + EXPECT_TRUE(c12->isRemoving()); + EXPECT_TRUE(C12->isRemoving()); + EXPECT_TRUE(f12->isRemoving()); + + EXPECT_FALSE(F3->isRemoving()); + EXPECT_FALSE(c3->isRemoving()); + EXPECT_FALSE(C3->isRemoving()); + EXPECT_FALSE(f3->isRemoving()); + EXPECT_TRUE(c23->isRemoving()); // Factor removed because involves removed frame + EXPECT_TRUE(C23->isRemoving()); // Virally removed + EXPECT_TRUE(f23->isRemoving()); // Virally removed + + EXPECT_FALSE(F4->isRemoving()); + EXPECT_FALSE(c4->isRemoving()); + EXPECT_FALSE(C4->isRemoving()); + EXPECT_FALSE(f4->isRemoving()); + EXPECT_FALSE(c34->isRemoving()); + EXPECT_FALSE(C34->isRemoving()); + EXPECT_FALSE(f34->isRemoving()); + + EXPECT_FALSE(F5->isRemoving()); + EXPECT_FALSE(c5->isRemoving()); + EXPECT_FALSE(C5->isRemoving()); + EXPECT_FALSE(f5->isRemoving()); + EXPECT_FALSE(c45->isRemoving()); + EXPECT_FALSE(C45->isRemoving()); + EXPECT_FALSE(f45->isRemoving()); + + EXPECT_FALSE(F1->isFixed()); + EXPECT_FALSE(F3->isFixed()); + EXPECT_FALSE(F4->isFixed()); + EXPECT_FALSE(F5->isFixed()); + + /* FRAME 6 ---------------------------------------------------------- + * + * Sliding window: + * (F1) (F3)(F4)(F5)(F6) + */ + auto F6 = P->emplaceKeyFrame(TimeStamp(6), "PO", 3, state); + P->keyFrameCallback(F6, nullptr, 0); + + // absolute factor + auto C6 = CaptureBase::emplace<CaptureVoid>(F6, TimeStamp(6), nullptr); + auto f6 = FeatureBase::emplace<FeatureBase>(C6, "absolute", state, cov); + auto c6 = FactorBase::emplace<FactorPose3d>(f6, f6, nullptr, false); + // displacement + auto C56 = CaptureBase::emplace<CaptureVoid>(F6, TimeStamp(6), nullptr); + auto f56 = FeatureBase::emplace<FeatureBase>(C56, "odom", zero_disp, cov); + auto c56 = FactorBase::emplace<FactorOdom3d>(f56, f56, F5, nullptr, false); + + // Checks + EXPECT_FALSE(F1->isRemoving()); + EXPECT_FALSE(c1->isRemoving()); + EXPECT_FALSE(C1->isRemoving()); + EXPECT_FALSE(f1->isRemoving()); + + EXPECT_TRUE(F2->isRemoving()); + EXPECT_TRUE(c2->isRemoving()); + EXPECT_TRUE(C2->isRemoving()); + EXPECT_TRUE(f2->isRemoving()); + EXPECT_TRUE(c12->isRemoving()); + EXPECT_TRUE(C12->isRemoving()); + EXPECT_TRUE(f12->isRemoving()); + + EXPECT_FALSE(F3->isRemoving()); + EXPECT_FALSE(c3->isRemoving()); + EXPECT_FALSE(C3->isRemoving()); + EXPECT_FALSE(f3->isRemoving()); + EXPECT_TRUE(c23->isRemoving()); // Factor removed because involves removed frame + EXPECT_TRUE(C23->isRemoving()); // Virally removed + EXPECT_TRUE(f23->isRemoving()); // Virally removed + + EXPECT_FALSE(F4->isRemoving()); + EXPECT_FALSE(c4->isRemoving()); + EXPECT_FALSE(C4->isRemoving()); + EXPECT_FALSE(f4->isRemoving()); + EXPECT_FALSE(c34->isRemoving()); + EXPECT_FALSE(C34->isRemoving()); + EXPECT_FALSE(f34->isRemoving()); + + EXPECT_FALSE(F5->isRemoving()); + EXPECT_FALSE(c5->isRemoving()); + EXPECT_FALSE(C5->isRemoving()); + EXPECT_FALSE(f5->isRemoving()); + EXPECT_FALSE(c45->isRemoving()); + EXPECT_FALSE(C45->isRemoving()); + EXPECT_FALSE(f45->isRemoving()); + + EXPECT_FALSE(F6->isRemoving()); + EXPECT_FALSE(c6->isRemoving()); + EXPECT_FALSE(C6->isRemoving()); + EXPECT_FALSE(f6->isRemoving()); + EXPECT_FALSE(c56->isRemoving()); + EXPECT_FALSE(C56->isRemoving()); + EXPECT_FALSE(f56->isRemoving()); + + EXPECT_FALSE(F1->isFixed()); + EXPECT_FALSE(F3->isFixed()); + EXPECT_FALSE(F4->isFixed()); + EXPECT_FALSE(F5->isFixed()); + EXPECT_FALSE(F6->isFixed()); + + /* FRAME 7 ---------------------------------------------------------- + * + * Sliding window: + * (F1) (F3) (F5)(F6)(F7) + */ + auto F7 = P->emplaceKeyFrame(TimeStamp(7), "PO", 3, state); + P->keyFrameCallback(F7, nullptr, 0); + + // absolute factor + auto C7 = CaptureBase::emplace<CaptureVoid>(F7, TimeStamp(7), nullptr); + auto f7 = FeatureBase::emplace<FeatureBase>(C7, "absolute", state, cov); + auto c7 = FactorBase::emplace<FactorPose3d>(f7, f7, nullptr, false); + // displacement + auto C67 = CaptureBase::emplace<CaptureVoid>(F7, TimeStamp(7), nullptr); + auto f67 = FeatureBase::emplace<FeatureBase>(C67, "odom", zero_disp, cov); + auto c67 = FactorBase::emplace<FactorOdom3d>(f67, f67, F6, nullptr, false); + + // Checks + EXPECT_FALSE(F1->isRemoving()); + EXPECT_FALSE(c1->isRemoving()); + EXPECT_FALSE(C1->isRemoving()); + EXPECT_FALSE(f1->isRemoving()); + + EXPECT_TRUE(F2->isRemoving()); + EXPECT_TRUE(c2->isRemoving()); + EXPECT_TRUE(C2->isRemoving()); + EXPECT_TRUE(f2->isRemoving()); + EXPECT_TRUE(c12->isRemoving()); + EXPECT_TRUE(C12->isRemoving()); + EXPECT_TRUE(f12->isRemoving()); + + EXPECT_FALSE(F3->isRemoving()); + EXPECT_FALSE(c3->isRemoving()); + EXPECT_FALSE(C3->isRemoving()); + EXPECT_FALSE(f3->isRemoving()); + EXPECT_TRUE(c23->isRemoving()); // Factor removed because involves removed frame + EXPECT_TRUE(C23->isRemoving()); // Virally removed + EXPECT_TRUE(f23->isRemoving()); // Virally removed + + EXPECT_TRUE(F4->isRemoving()); + EXPECT_TRUE(c4->isRemoving()); + EXPECT_TRUE(C4->isRemoving()); + EXPECT_TRUE(f4->isRemoving()); + EXPECT_TRUE(c34->isRemoving()); + EXPECT_TRUE(C34->isRemoving()); + EXPECT_TRUE(f34->isRemoving()); + + EXPECT_FALSE(F5->isRemoving()); + EXPECT_FALSE(c5->isRemoving()); + EXPECT_FALSE(C5->isRemoving()); + EXPECT_FALSE(f5->isRemoving()); + EXPECT_TRUE(c45->isRemoving()); // Factor removed because involves removed frame + EXPECT_TRUE(C45->isRemoving()); // Virally removed + EXPECT_TRUE(f45->isRemoving()); // Virally removed + + EXPECT_FALSE(F6->isRemoving()); + EXPECT_FALSE(c6->isRemoving()); + EXPECT_FALSE(C6->isRemoving()); + EXPECT_FALSE(f6->isRemoving()); + EXPECT_FALSE(c56->isRemoving()); + EXPECT_FALSE(C56->isRemoving()); + EXPECT_FALSE(f56->isRemoving()); + + EXPECT_FALSE(F7->isRemoving()); + EXPECT_FALSE(c7->isRemoving()); + EXPECT_FALSE(C7->isRemoving()); + EXPECT_FALSE(f7->isRemoving()); + EXPECT_FALSE(c67->isRemoving()); + EXPECT_FALSE(C67->isRemoving()); + EXPECT_FALSE(f67->isRemoving()); + + EXPECT_FALSE(F1->isFixed()); + EXPECT_FALSE(F3->isFixed()); + EXPECT_FALSE(F5->isFixed()); + EXPECT_FALSE(F6->isFixed()); + EXPECT_FALSE(F7->isFixed()); + + /* FRAME 8 ---------------------------------------------------------- + * + * Sliding window: + * (F3) (F5)(F6)(F7)(F8) + */ + auto F8 = P->emplaceKeyFrame(TimeStamp(8), "PO", 3, state); + P->keyFrameCallback(F8, nullptr, 0); + + // absolute factor + auto C8 = CaptureBase::emplace<CaptureVoid>(F8, TimeStamp(8), nullptr); + auto f8 = FeatureBase::emplace<FeatureBase>(C8, "absolute", state, cov); + auto c8 = FactorBase::emplace<FactorPose3d>(f8, f8, nullptr, false); + // displacement + auto C78 = CaptureBase::emplace<CaptureVoid>(F8, TimeStamp(8), nullptr); + auto f78 = FeatureBase::emplace<FeatureBase>(C78, "odom", zero_disp, cov); + auto c78 = FactorBase::emplace<FactorOdom3d>(f78, f78, F7, nullptr, false); + + // Checks + EXPECT_TRUE(F1->isRemoving()); // First frame removed + EXPECT_TRUE(c1->isRemoving()); + EXPECT_TRUE(C1->isRemoving()); + EXPECT_TRUE(f1->isRemoving()); + + EXPECT_TRUE(F2->isRemoving()); + EXPECT_TRUE(c2->isRemoving()); + EXPECT_TRUE(C2->isRemoving()); + EXPECT_TRUE(f2->isRemoving()); + EXPECT_TRUE(c12->isRemoving()); + EXPECT_TRUE(C12->isRemoving()); + EXPECT_TRUE(f12->isRemoving()); + + EXPECT_FALSE(F3->isRemoving()); + EXPECT_FALSE(c3->isRemoving()); + EXPECT_FALSE(C3->isRemoving()); + EXPECT_FALSE(f3->isRemoving()); + EXPECT_TRUE(c23->isRemoving()); // Factor removed because involves removed frame + EXPECT_TRUE(C23->isRemoving()); // Virally removed + EXPECT_TRUE(f23->isRemoving()); // Virally removed + + EXPECT_TRUE(F4->isRemoving()); + EXPECT_TRUE(c4->isRemoving()); + EXPECT_TRUE(C4->isRemoving()); + EXPECT_TRUE(f4->isRemoving()); + EXPECT_TRUE(c34->isRemoving()); + EXPECT_TRUE(C34->isRemoving()); + EXPECT_TRUE(f34->isRemoving()); + + EXPECT_FALSE(F5->isRemoving()); + EXPECT_FALSE(c5->isRemoving()); + EXPECT_FALSE(C5->isRemoving()); + EXPECT_FALSE(f5->isRemoving()); + EXPECT_TRUE(c45->isRemoving()); // Factor removed because involves removed frame + EXPECT_TRUE(C45->isRemoving()); // Virally removed + EXPECT_TRUE(f45->isRemoving()); // Virally removed + + EXPECT_FALSE(F6->isRemoving()); + EXPECT_FALSE(c6->isRemoving()); + EXPECT_FALSE(C6->isRemoving()); + EXPECT_FALSE(f6->isRemoving()); + EXPECT_FALSE(c56->isRemoving()); + EXPECT_FALSE(C56->isRemoving()); + EXPECT_FALSE(f56->isRemoving()); + + EXPECT_FALSE(F7->isRemoving()); + EXPECT_FALSE(c7->isRemoving()); + EXPECT_FALSE(C7->isRemoving()); + EXPECT_FALSE(f7->isRemoving()); + EXPECT_FALSE(c67->isRemoving()); + EXPECT_FALSE(C67->isRemoving()); + EXPECT_FALSE(f67->isRemoving()); + + EXPECT_FALSE(F8->isRemoving()); + EXPECT_FALSE(c8->isRemoving()); + EXPECT_FALSE(C8->isRemoving()); + EXPECT_FALSE(f8->isRemoving()); + EXPECT_FALSE(c78->isRemoving()); + EXPECT_FALSE(C78->isRemoving()); + EXPECT_FALSE(f78->isRemoving()); + + EXPECT_TRUE(F3->isFixed()); + EXPECT_FALSE(F5->isFixed()); + EXPECT_FALSE(F6->isFixed()); + EXPECT_FALSE(F7->isFixed()); + EXPECT_FALSE(F8->isFixed()); +} + +TEST(TreeManagerSlidingWindowDualRate, slidingWindowNoFixNoViral) +{ + /* sliding window dual rate: + * n_frames: 5 + * n_frames_recent: 3 + * rate_old_frames: 2 + */ + + ParserYaml parser = ParserYaml("test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml", wolf_root); + ParamsServer server = ParamsServer(parser.getParams()); + + ProblemPtr P = Problem::autoSetup(server); + P->applyPriorOptions(0); + + /* FRAME 1 ---------------------------------------------------------- + * + * Sliding window: + * ( ) ( ) ( )( )(F1) + */ + auto F1 = P->getTrajectory()->getLastFrame(); + ASSERT_TRUE(F1 != nullptr); + ASSERT_FALSE(F1->getCaptureList().empty()); + auto C1 = F1->getCaptureList().front(); + ASSERT_FALSE(C1->getFeatureList().empty()); + auto f1 = C1->getFeatureList().front(); + ASSERT_FALSE(f1->getFactorList().empty()); + auto c1 = f1->getFactorList().front(); + + Vector7d state = F1->getStateVector(); + Vector7d zero_disp(state); + Matrix6d cov = Matrix6d::Identity(); + + // Check no frame removed + EXPECT_FALSE(F1->isRemoving()); + EXPECT_FALSE(c1->isRemoving()); + EXPECT_FALSE(C1->isRemoving()); + EXPECT_FALSE(f1->isRemoving()); + + EXPECT_FALSE(F1->isFixed()); + + /* FRAME 2 ---------------------------------------------------------- + * + * Sliding window: + * ( ) ( ) ( )(F1)(F2) + */ + auto F2 = P->emplaceKeyFrame(TimeStamp(2), "PO", 3, state); + P->keyFrameCallback(F2, nullptr, 0); + + // absolute factor + auto C2 = CaptureBase::emplace<CaptureVoid>(F2, TimeStamp(2), nullptr); + auto f2 = FeatureBase::emplace<FeatureBase>(C2, "absolute", state, cov); + auto c2 = FactorBase::emplace<FactorPose3d>(f2, f2, nullptr, false); + // displacement + auto C12 = CaptureBase::emplace<CaptureVoid>(F2, TimeStamp(2), nullptr); + auto f12 = FeatureBase::emplace<FeatureBase>(C12, "odom", zero_disp, cov); + auto c12 = FactorBase::emplace<FactorOdom3d>(f12, f12, F1, nullptr, false); + + // Check no frame removed + EXPECT_FALSE(F1->isRemoving()); + EXPECT_FALSE(c1->isRemoving()); + EXPECT_FALSE(C1->isRemoving()); + EXPECT_FALSE(f1->isRemoving()); + + EXPECT_FALSE(F2->isRemoving()); + EXPECT_FALSE(c2->isRemoving()); + EXPECT_FALSE(C2->isRemoving()); + EXPECT_FALSE(f2->isRemoving()); + EXPECT_FALSE(c12->isRemoving()); + EXPECT_FALSE(C12->isRemoving()); + EXPECT_FALSE(f12->isRemoving()); + + EXPECT_FALSE(F1->isFixed()); + EXPECT_FALSE(F2->isFixed()); + + /* FRAME 3 ---------------------------------------------------------- + * + * Sliding window: + * ( ) ( ) (F1)(F2)(F3) + */ + auto F3 = P->emplaceKeyFrame(TimeStamp(3), "PO", 3, state); + P->keyFrameCallback(F3, nullptr, 0); + + // absolute factor + auto C3 = CaptureBase::emplace<CaptureVoid>(F3, TimeStamp(3), nullptr); + auto f3 = FeatureBase::emplace<FeatureBase>(C3, "absolute", state, cov); + auto c3 = FactorBase::emplace<FactorPose3d>(f3, f3, nullptr, false); + // displacement + auto C23 = CaptureBase::emplace<CaptureVoid>(F3, TimeStamp(3), nullptr); + auto f23 = FeatureBase::emplace<FeatureBase>(C23, "odom", zero_disp, cov); + auto c23 = FactorBase::emplace<FactorOdom3d>(f23, f23, F2, nullptr, false); + + // Check no frame removed + EXPECT_FALSE(F1->isRemoving()); + EXPECT_FALSE(c1->isRemoving()); + EXPECT_FALSE(C1->isRemoving()); + EXPECT_FALSE(f1->isRemoving()); + + EXPECT_FALSE(F2->isRemoving()); + EXPECT_FALSE(c2->isRemoving()); + EXPECT_FALSE(C2->isRemoving()); + EXPECT_FALSE(f2->isRemoving()); + EXPECT_FALSE(c12->isRemoving()); + EXPECT_FALSE(C12->isRemoving()); + EXPECT_FALSE(f12->isRemoving()); + + EXPECT_FALSE(F3->isRemoving()); + EXPECT_FALSE(c3->isRemoving()); + EXPECT_FALSE(C3->isRemoving()); + EXPECT_FALSE(f3->isRemoving()); + EXPECT_FALSE(c23->isRemoving()); + EXPECT_FALSE(C23->isRemoving()); + EXPECT_FALSE(f23->isRemoving()); + + EXPECT_FALSE(F1->isFixed()); + EXPECT_FALSE(F2->isFixed()); + EXPECT_FALSE(F3->isFixed()); + + /* FRAME 4 ---------------------------------------------------------- + * + * Sliding window: + * ( ) (F1)(F2)(F3)(F4) + */ + auto F4 = P->emplaceKeyFrame(TimeStamp(4), "PO", 3, state); + P->keyFrameCallback(F4, nullptr, 0); + + // absolute factor + auto C4 = CaptureBase::emplace<CaptureVoid>(F4, TimeStamp(4), nullptr); + auto f4 = FeatureBase::emplace<FeatureBase>(C4, "absolute", state, cov); + auto c4 = FactorBase::emplace<FactorPose3d>(f4, f4, nullptr, false); + // displacement + auto C34 = CaptureBase::emplace<CaptureVoid>(F4, TimeStamp(4), nullptr); + auto f34 = FeatureBase::emplace<FeatureBase>(C34, "odom", zero_disp, cov); + auto c34 = FactorBase::emplace<FactorOdom3d>(f34, f34, F3, nullptr, false); + + // Checks + EXPECT_FALSE(F1->isRemoving()); + EXPECT_FALSE(c1->isRemoving()); + EXPECT_FALSE(C1->isRemoving()); + EXPECT_FALSE(f1->isRemoving()); + + EXPECT_FALSE(F2->isRemoving()); + EXPECT_FALSE(c2->isRemoving()); + EXPECT_FALSE(C2->isRemoving()); + EXPECT_FALSE(f2->isRemoving()); + EXPECT_FALSE(c12->isRemoving()); + EXPECT_FALSE(C12->isRemoving()); + EXPECT_FALSE(f12->isRemoving()); + + EXPECT_FALSE(F3->isRemoving()); + EXPECT_FALSE(c3->isRemoving()); + EXPECT_FALSE(C3->isRemoving()); + EXPECT_FALSE(f3->isRemoving()); + EXPECT_FALSE(c23->isRemoving()); + EXPECT_FALSE(C23->isRemoving()); + EXPECT_FALSE(f23->isRemoving()); + + EXPECT_FALSE(F4->isRemoving()); + EXPECT_FALSE(c4->isRemoving()); + EXPECT_FALSE(C4->isRemoving()); + EXPECT_FALSE(f4->isRemoving()); + EXPECT_FALSE(c34->isRemoving()); + EXPECT_FALSE(C34->isRemoving()); + EXPECT_FALSE(f34->isRemoving()); + + EXPECT_FALSE(F1->isFixed()); + EXPECT_FALSE(F2->isFixed()); + EXPECT_FALSE(F3->isFixed()); + EXPECT_FALSE(F4->isFixed()); + + /* FRAME 5 ---------------------------------------------------------- + * + * Sliding window: + * ( ) (F1) (F3)(F4)(F5) + */ + auto F5 = P->emplaceKeyFrame(TimeStamp(5), "PO", 3, state); + P->keyFrameCallback(F5, nullptr, 0); + + // absolute factor + auto C5 = CaptureBase::emplace<CaptureVoid>(F5, TimeStamp(5), nullptr); + auto f5 = FeatureBase::emplace<FeatureBase>(C5, "absolute", state, cov); + auto c5 = FactorBase::emplace<FactorPose3d>(f5, f5, nullptr, false); + // displacement + auto C45 = CaptureBase::emplace<CaptureVoid>(F5, TimeStamp(5), nullptr); + auto f45 = FeatureBase::emplace<FeatureBase>(C45, "odom", zero_disp, cov); + auto c45 = FactorBase::emplace<FactorOdom3d>(f45, f45, F4, nullptr, false); + + // Checks + EXPECT_FALSE(F1->isRemoving()); + EXPECT_FALSE(c1->isRemoving()); + EXPECT_FALSE(C1->isRemoving()); + EXPECT_FALSE(f1->isRemoving()); + + EXPECT_TRUE(F2->isRemoving()); + EXPECT_TRUE(c2->isRemoving()); + EXPECT_TRUE(C2->isRemoving()); + EXPECT_TRUE(f2->isRemoving()); + EXPECT_TRUE(c12->isRemoving()); + EXPECT_TRUE(C12->isRemoving()); + EXPECT_TRUE(f12->isRemoving()); + + EXPECT_FALSE(F3->isRemoving()); + EXPECT_FALSE(c3->isRemoving()); + EXPECT_FALSE(C3->isRemoving()); + EXPECT_FALSE(f3->isRemoving()); + 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(F4->isRemoving()); + EXPECT_FALSE(c4->isRemoving()); + EXPECT_FALSE(C4->isRemoving()); + EXPECT_FALSE(f4->isRemoving()); + EXPECT_FALSE(c34->isRemoving()); + EXPECT_FALSE(C34->isRemoving()); + EXPECT_FALSE(f34->isRemoving()); + + EXPECT_FALSE(F5->isRemoving()); + EXPECT_FALSE(c5->isRemoving()); + EXPECT_FALSE(C5->isRemoving()); + EXPECT_FALSE(f5->isRemoving()); + EXPECT_FALSE(c45->isRemoving()); + EXPECT_FALSE(C45->isRemoving()); + EXPECT_FALSE(f45->isRemoving()); + + EXPECT_FALSE(F1->isFixed()); + EXPECT_FALSE(F3->isFixed()); + EXPECT_FALSE(F4->isFixed()); + EXPECT_FALSE(F5->isFixed()); + + /* FRAME 6 ---------------------------------------------------------- + * + * Sliding window: + * (F1) (F3)(F4)(F5)(F6) + */ + auto F6 = P->emplaceKeyFrame(TimeStamp(6), "PO", 3, state); + P->keyFrameCallback(F6, nullptr, 0); + + // absolute factor + auto C6 = CaptureBase::emplace<CaptureVoid>(F6, TimeStamp(6), nullptr); + auto f6 = FeatureBase::emplace<FeatureBase>(C6, "absolute", state, cov); + auto c6 = FactorBase::emplace<FactorPose3d>(f6, f6, nullptr, false); + // displacement + auto C56 = CaptureBase::emplace<CaptureVoid>(F6, TimeStamp(6), nullptr); + auto f56 = FeatureBase::emplace<FeatureBase>(C56, "odom", zero_disp, cov); + auto c56 = FactorBase::emplace<FactorOdom3d>(f56, f56, F5, nullptr, false); + + // Checks + EXPECT_FALSE(F1->isRemoving()); + EXPECT_FALSE(c1->isRemoving()); + EXPECT_FALSE(C1->isRemoving()); + EXPECT_FALSE(f1->isRemoving()); + + EXPECT_TRUE(F2->isRemoving()); + EXPECT_TRUE(c2->isRemoving()); + EXPECT_TRUE(C2->isRemoving()); + EXPECT_TRUE(f2->isRemoving()); + EXPECT_TRUE(c12->isRemoving()); + EXPECT_TRUE(C12->isRemoving()); + EXPECT_TRUE(f12->isRemoving()); + + EXPECT_FALSE(F3->isRemoving()); + EXPECT_FALSE(c3->isRemoving()); + EXPECT_FALSE(C3->isRemoving()); + EXPECT_FALSE(f3->isRemoving()); + 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(F4->isRemoving()); + EXPECT_FALSE(c4->isRemoving()); + EXPECT_FALSE(C4->isRemoving()); + EXPECT_FALSE(f4->isRemoving()); + EXPECT_FALSE(c34->isRemoving()); + EXPECT_FALSE(C34->isRemoving()); + EXPECT_FALSE(f34->isRemoving()); + + EXPECT_FALSE(F5->isRemoving()); + EXPECT_FALSE(c5->isRemoving()); + EXPECT_FALSE(C5->isRemoving()); + EXPECT_FALSE(f5->isRemoving()); + EXPECT_FALSE(c45->isRemoving()); + EXPECT_FALSE(C45->isRemoving()); + EXPECT_FALSE(f45->isRemoving()); + + EXPECT_FALSE(F6->isRemoving()); + EXPECT_FALSE(c6->isRemoving()); + EXPECT_FALSE(C6->isRemoving()); + EXPECT_FALSE(f6->isRemoving()); + EXPECT_FALSE(c56->isRemoving()); + EXPECT_FALSE(C56->isRemoving()); + EXPECT_FALSE(f56->isRemoving()); + + EXPECT_FALSE(F1->isFixed()); + EXPECT_FALSE(F3->isFixed()); + EXPECT_FALSE(F4->isFixed()); + EXPECT_FALSE(F5->isFixed()); + EXPECT_FALSE(F6->isFixed()); + + /* FRAME 7 ---------------------------------------------------------- + * + * Sliding window: + * (F1) (F3) (F5)(F6)(F7) + */ + auto F7 = P->emplaceKeyFrame(TimeStamp(7), "PO", 3, state); + P->keyFrameCallback(F7, nullptr, 0); + + // absolute factor + auto C7 = CaptureBase::emplace<CaptureVoid>(F7, TimeStamp(7), nullptr); + auto f7 = FeatureBase::emplace<FeatureBase>(C7, "absolute", state, cov); + auto c7 = FactorBase::emplace<FactorPose3d>(f7, f7, nullptr, false); + // displacement + auto C67 = CaptureBase::emplace<CaptureVoid>(F7, TimeStamp(7), nullptr); + auto f67 = FeatureBase::emplace<FeatureBase>(C67, "odom", zero_disp, cov); + auto c67 = FactorBase::emplace<FactorOdom3d>(f67, f67, F6, nullptr, false); + + // Checks + EXPECT_FALSE(F1->isRemoving()); + EXPECT_FALSE(c1->isRemoving()); + EXPECT_FALSE(C1->isRemoving()); + EXPECT_FALSE(f1->isRemoving()); + + EXPECT_TRUE(F2->isRemoving()); + EXPECT_TRUE(c2->isRemoving()); + EXPECT_TRUE(C2->isRemoving()); + EXPECT_TRUE(f2->isRemoving()); + EXPECT_TRUE(c12->isRemoving()); + EXPECT_TRUE(C12->isRemoving()); + EXPECT_TRUE(f12->isRemoving()); + + EXPECT_FALSE(F3->isRemoving()); + EXPECT_FALSE(c3->isRemoving()); + EXPECT_FALSE(C3->isRemoving()); + EXPECT_FALSE(f3->isRemoving()); + 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_TRUE(F4->isRemoving()); + EXPECT_TRUE(c4->isRemoving()); + EXPECT_TRUE(C4->isRemoving()); + EXPECT_TRUE(f4->isRemoving()); + EXPECT_TRUE(c34->isRemoving()); + EXPECT_TRUE(C34->isRemoving()); + EXPECT_TRUE(f34->isRemoving()); + + EXPECT_FALSE(F5->isRemoving()); + EXPECT_FALSE(c5->isRemoving()); + EXPECT_FALSE(C5->isRemoving()); + EXPECT_FALSE(f5->isRemoving()); + EXPECT_TRUE(c45->isRemoving()); // Factor removed because involves removed frame + EXPECT_FALSE(C45->isRemoving()); // Not virally removed + EXPECT_FALSE(f45->isRemoving()); // Not virally removed + + EXPECT_FALSE(F6->isRemoving()); + EXPECT_FALSE(c6->isRemoving()); + EXPECT_FALSE(C6->isRemoving()); + EXPECT_FALSE(f6->isRemoving()); + EXPECT_FALSE(c56->isRemoving()); + EXPECT_FALSE(C56->isRemoving()); + EXPECT_FALSE(f56->isRemoving()); + + EXPECT_FALSE(F7->isRemoving()); + EXPECT_FALSE(c7->isRemoving()); + EXPECT_FALSE(C7->isRemoving()); + EXPECT_FALSE(f7->isRemoving()); + EXPECT_FALSE(c67->isRemoving()); + EXPECT_FALSE(C67->isRemoving()); + EXPECT_FALSE(f67->isRemoving()); + + EXPECT_FALSE(F1->isFixed()); + EXPECT_FALSE(F3->isFixed()); + EXPECT_FALSE(F5->isFixed()); + EXPECT_FALSE(F6->isFixed()); + EXPECT_FALSE(F7->isFixed()); + + /* FRAME 8 ---------------------------------------------------------- + * + * Sliding window: + * (F3) (F5)(F6)(F7)(F8) + */ + auto F8 = P->emplaceKeyFrame(TimeStamp(8), "PO", 3, state); + P->keyFrameCallback(F8, nullptr, 0); + + // absolute factor + auto C8 = CaptureBase::emplace<CaptureVoid>(F8, TimeStamp(8), nullptr); + auto f8 = FeatureBase::emplace<FeatureBase>(C8, "absolute", state, cov); + auto c8 = FactorBase::emplace<FactorPose3d>(f8, f8, nullptr, false); + // displacement + auto C78 = CaptureBase::emplace<CaptureVoid>(F8, TimeStamp(8), nullptr); + auto f78 = FeatureBase::emplace<FeatureBase>(C78, "odom", zero_disp, cov); + auto c78 = FactorBase::emplace<FactorOdom3d>(f78, f78, F7, nullptr, false); + + // Checks + EXPECT_TRUE(F1->isRemoving()); // First frame removed + EXPECT_TRUE(c1->isRemoving()); + EXPECT_TRUE(C1->isRemoving()); + EXPECT_TRUE(f1->isRemoving()); + + EXPECT_TRUE(F2->isRemoving()); + EXPECT_TRUE(c2->isRemoving()); + EXPECT_TRUE(C2->isRemoving()); + EXPECT_TRUE(f2->isRemoving()); + EXPECT_TRUE(c12->isRemoving()); + EXPECT_TRUE(C12->isRemoving()); + EXPECT_TRUE(f12->isRemoving()); + + EXPECT_FALSE(F3->isRemoving()); + EXPECT_FALSE(c3->isRemoving()); + EXPECT_FALSE(C3->isRemoving()); + EXPECT_FALSE(f3->isRemoving()); + 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_TRUE(F4->isRemoving()); + EXPECT_TRUE(c4->isRemoving()); + EXPECT_TRUE(C4->isRemoving()); + EXPECT_TRUE(f4->isRemoving()); + EXPECT_TRUE(c34->isRemoving()); + EXPECT_TRUE(C34->isRemoving()); + EXPECT_TRUE(f34->isRemoving()); + + EXPECT_FALSE(F5->isRemoving()); + EXPECT_FALSE(c5->isRemoving()); + EXPECT_FALSE(C5->isRemoving()); + EXPECT_FALSE(f5->isRemoving()); + EXPECT_TRUE(c45->isRemoving()); // Factor removed because involves removed frame + EXPECT_FALSE(C45->isRemoving()); // Not virally removed + EXPECT_FALSE(f45->isRemoving()); // Not virally removed + + EXPECT_FALSE(F6->isRemoving()); + EXPECT_FALSE(c6->isRemoving()); + EXPECT_FALSE(C6->isRemoving()); + EXPECT_FALSE(f6->isRemoving()); + EXPECT_FALSE(c56->isRemoving()); + EXPECT_FALSE(C56->isRemoving()); + EXPECT_FALSE(f56->isRemoving()); + + EXPECT_FALSE(F7->isRemoving()); + EXPECT_FALSE(c7->isRemoving()); + EXPECT_FALSE(C7->isRemoving()); + EXPECT_FALSE(f7->isRemoving()); + EXPECT_FALSE(c67->isRemoving()); + EXPECT_FALSE(C67->isRemoving()); + EXPECT_FALSE(f67->isRemoving()); + + EXPECT_FALSE(F8->isRemoving()); + EXPECT_FALSE(c8->isRemoving()); + EXPECT_FALSE(C8->isRemoving()); + EXPECT_FALSE(f8->isRemoving()); + EXPECT_FALSE(c78->isRemoving()); + EXPECT_FALSE(C78->isRemoving()); + EXPECT_FALSE(f78->isRemoving()); + + EXPECT_FALSE(F3->isFixed()); + EXPECT_FALSE(F5->isFixed()); + EXPECT_FALSE(F6->isFixed()); + EXPECT_FALSE(F7->isFixed()); + EXPECT_FALSE(F8->isFixed()); +} + +TEST(TreeManagerSlidingWindowDualRate, slidingWindowWithProcessor) +{ + // SLIDING WINDOW DUAL RATE + ParserYaml parser = ParserYaml("test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml", wolf_root); + ParamsServer server = ParamsServer(parser.getParams()); + ProblemPtr problem = Problem::autoSetup(server); + SolverManagerPtr solver = FactorySolver::create("SolverCeres", problem, server); + + // BASELINE + ParserYaml parser_bl = ParserYaml("test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml", wolf_root); + ParamsServer server_bl = ParamsServer(parser_bl.getParams()); + ProblemPtr problem_bl = Problem::autoSetup(server_bl); + SolverManagerPtr solver_bl = FactorySolver::create("SolverCeres", problem_bl, server); + + // aux variables + Vector7d data; + Matrix6d data_cov = Matrix6d::Identity(); + TimeStamp t(0.0); + double dt = 1; + CaptureMotionPtr capture = std::make_shared<CaptureOdom3d>(t, + problem->getSensor("odom"), + data, + data_cov); + CaptureMotionPtr capture_bl = std::make_shared<CaptureOdom3d>(t, + problem_bl->getSensor("odom"), + data, + data_cov); + + // START MOTION CAPTURES + for (int i = 0; i<20; i++) + { + t += dt; + WOLF_INFO("-------------------------- t: ", t); + + // random movement + data.setRandom(); data.tail<4>().normalize(); + + // sliding window process + capture->setTimeStamp(t); + capture->setData(data); + capture->process(); + + // baseline process + capture_bl->setTimeStamp(t); + capture_bl->setData(data); + capture_bl->process(); + + WOLF_INFO("state: ", problem->getState().vector("PO").transpose()); + + ASSERT_MATRIX_APPROX(problem->getState().vector("PO").transpose(), + problem_bl->getState().vector("PO").transpose(), + 1e-12); + + // Solve + solver->solve(); + solver_bl->solve(); + + ASSERT_MATRIX_APPROX(problem->getState().vector("PO").transpose(), + problem_bl->getState().vector("PO").transpose(), + 1e-12); + + ASSERT_TRUE(problem->check()); + } +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/yaml/params_tree_manager_sliding_window1.yaml b/test/yaml/params_tree_manager_sliding_window1.yaml index 96ad94f72af7e8de1b9200a8e5ea44e3bc952a85..22242febd640b616bebe4ab0410867b0e373a5be 100644 --- a/test/yaml/params_tree_manager_sliding_window1.yaml +++ b/test/yaml/params_tree_manager_sliding_window1.yaml @@ -4,8 +4,6 @@ config: dimension: 3 prior: mode: "factor" - # state: [0,0,0,0,0,0,1] - # cov: [[6,6],.1,0,0,0,0,0, 0,.1,0,0,0,0, 0,0,.1,0,0,0, 0,0,0,.1,0,0, 0,0,0,0,.1,0, 0,0,0,0,0,.1] $state: P: [0,0,0] O: [0,0,0,1] @@ -15,8 +13,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..add5eff760050fecbde63c827bb75e77c531f86e 100644 --- a/test/yaml/params_tree_manager_sliding_window2.yaml +++ b/test/yaml/params_tree_manager_sliding_window2.yaml @@ -4,8 +4,6 @@ config: dimension: 3 prior: mode: "factor" - # state: [0,0,0,0,0,0,1] - # cov: [[6,6],.1,0,0,0,0,0, 0,.1,0,0,0,0, 0,0,.1,0,0,0, 0,0,0,.1,0,0, 0,0,0,0,.1,0, 0,0,0,0,0,.1] $state: P: [0,0,0] O: [0,0,0,1] @@ -15,8 +13,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: - diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml new file mode 100644 index 0000000000000000000000000000000000000000..32ef665e0a6d92b94e9342d22c5e4ca9952613f1 --- /dev/null +++ b/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml @@ -0,0 +1,20 @@ +config: + problem: + frame_structure: "PO" + dimension: 3 + prior: + mode: "factor" + $state: + P: [0,0,0] + O: [0,0,0,1] + $sigma: + P: [0.31, 0.31, 0.31] + O: [0.31, 0.31, 0.31] + time_tolerance: 0.1 + tree_manager: + type: "TreeManagerSlidingWindowDualRate" + n_frames: 5 + n_frames_recent: 3 + rate_old_frames: 2 + fix_first_frame: true + 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 new file mode 100644 index 0000000000000000000000000000000000000000..6b4795fb3ad70c77b02c9cffc74b964abfe64141 --- /dev/null +++ b/test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml @@ -0,0 +1,20 @@ +config: + problem: + frame_structure: "PO" + dimension: 3 + prior: + mode: "factor" + $state: + P: [0,0,0] + O: [0,0,0,1] + $sigma: + P: [0.31, 0.31, 0.31] + O: [0.31, 0.31, 0.31] + time_tolerance: 0.1 + tree_manager: + type: "TreeManagerSlidingWindowDualRate" + n_frames: 5 + n_frames_recent: 3 + rate_old_frames: 2 + fix_first_frame: false + 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 new file mode 100644 index 0000000000000000000000000000000000000000..b327504aa414e4b36e6163175b20b0ba4a4f79b9 --- /dev/null +++ b/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml @@ -0,0 +1,54 @@ +config: + solver: + period: 1 + verbose: 2 + update_immediately: false + max_num_iterations: 10 + problem: + frame_structure: "PO" + dimension: 3 + prior: + mode: "factor" + $state: + P: [0,0,0] + O: [0,0,0,1] + $sigma: + P: [0.31, 0.31, 0.31] + O: [0.31, 0.31, 0.31] + time_tolerance: 0.1 + tree_manager: + type: "TreeManagerSlidingWindowDualRate" + n_frames: 5 + n_frames_recent: 3 + rate_old_frames: 2 + fix_first_frame: true + viral_remove_empty_parent: true + sensors: + - + type: "SensorOdom3d" + name: "odom" + plugin: "core" + k_disp_to_disp: 0.1 + k_disp_to_rot: 0.1 + k_rot_to_rot: 0.1 + min_disp_var: 0.1 + min_rot_var: 0.1 + extrinsic: + pose: [1,2,3,0,0,0,1] + processors: + - + type: "ProcessorOdom3d" + name: "my_proc_odom3d" + sensor_name: "odom" + plugin: "core" + apply_loss_function: false + time_tolerance: 0.01 # seconds + keyframe_vote: + voting_active: true + voting_aux_active: false + max_time_span: 0.2 # seconds + max_buff_length: 10 # motion deltas + dist_traveled: 10 # meters + angle_turned: 3.1 # radians (1 rad approx 57 deg, approx 60 deg) + + unmeasured_perturbation_std: 0.00111 diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml new file mode 100644 index 0000000000000000000000000000000000000000..eba780939b0e4521ba7a5d571ed47d63749adc6e --- /dev/null +++ b/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml @@ -0,0 +1,49 @@ +config: + solver: + period: 1 + verbose: 2 + update_immediately: false + max_num_iterations: 10 + problem: + frame_structure: "PO" + dimension: 3 + prior: + mode: "factor" + $state: + P: [0,0,0] + O: [0,0,0,1] + $sigma: + P: [0.31, 0.31, 0.31] + O: [0.31, 0.31, 0.31] + time_tolerance: 0.1 + tree_manager: + type: "None" + sensors: + - + type: "SensorOdom3d" + name: "odom" + plugin: "core" + k_disp_to_disp: 0.1 + k_disp_to_rot: 0.1 + k_rot_to_rot: 0.1 + min_disp_var: 0.1 + min_rot_var: 0.1 + extrinsic: + pose: [1,2,3,0,0,0,1] + processors: + - + type: "ProcessorOdom3d" + name: "my_proc_odom3d" + sensor_name: "odom" + plugin: "core" + apply_loss_function: false + time_tolerance: 0.01 # seconds + keyframe_vote: + voting_active: true + voting_aux_active: false + max_time_span: 0.2 # seconds + max_buff_length: 10 # motion deltas + dist_traveled: 10 # meters + angle_turned: 3.1 # radians (1 rad approx 57 deg, approx 60 deg) + + unmeasured_perturbation_std: 0.00111