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_dual_rate.cpp b/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp
index 31585d3c7bdee10ac723a087cfbde74fdb03e956..2009cd89edf486af2efa892f58615a1e0fa05b0f 100644
--- a/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp
+++ b/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp
@@ -25,6 +25,7 @@ void TreeManagerSlidingWindowDualRate::keyFrameCallback(FrameBasePtr _key_frame)
// 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(),
@@ -43,9 +44,11 @@ void TreeManagerSlidingWindowDualRate::keyFrameCallback(FrameBasePtr _key_frame)
// 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
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 177209846280f0d8a626298126d69b1d68d55ed4..0c5e7c2aec97277425289f3f28a9ba3d61dece1c 100644
--- a/test/gtest_tree_manager_sliding_window.cpp
+++ b/test/gtest_tree_manager_sliding_window.cpp
@@ -133,9 +133,9 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
// Checks
EXPECT_TRUE(F1->isRemoving());
- EXPECT_TRUE(c12->isRemoving());
- EXPECT_TRUE(C12->isRemoving()); //Virally removed
- EXPECT_TRUE(f12->isRemoving()); //Virally removed
+ 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 ----------------------------------------------------------
@@ -153,16 +153,17 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
// Checks
EXPECT_TRUE(F1->isRemoving());
- EXPECT_TRUE(c12->isRemoving());
- EXPECT_TRUE(C12->isRemoving()); //Virally removed
- EXPECT_TRUE(f12->isRemoving()); //Virally removed
+ 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());
- 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(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
}
@@ -229,7 +230,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
// Checks
EXPECT_TRUE(F1->isRemoving());
- EXPECT_TRUE(c12->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
@@ -249,14 +250,14 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
// Checks
EXPECT_TRUE(F1->isRemoving());
- EXPECT_TRUE(c12->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());
+ 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
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 2a8613dede994fe72b7fea8a5f1506c71981bbb0..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]
diff --git a/test/yaml/params_tree_manager_sliding_window2.yaml b/test/yaml/params_tree_manager_sliding_window2.yaml
index fda0a8a558ac3f9dec60ec869bc5d9c7d68ca6a5..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]
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