diff --git a/include/core/tree_manager/tree_manager_base.h b/include/core/tree_manager/tree_manager_base.h
index 8cd42c07e543be9f6f690bf88581ee6b5e481ed0..53ef9825ab029933b8c58e497e792d4f101c0406 100644
--- a/include/core/tree_manager/tree_manager_base.h
+++ b/include/core/tree_manager/tree_manager_base.h
@@ -5,6 +5,7 @@
#include "core/common/wolf.h"
#include "core/common/node_base.h"
#include "core/common/params_base.h"
+#include "core/problem/problem.h"
namespace wolf
{
@@ -45,7 +46,7 @@ static TreeManagerBasePtr create(const std::string& _unique_name,
struct ParamsTreeManagerBase : public ParamsBase
{
- std::string prefix = "problem/tree_manager/";
+ std::string prefix = "problem/tree_manager";
ParamsTreeManagerBase() = default;
ParamsTreeManagerBase(std::string _unique_name, const ParamsServer& _server):
ParamsBase(_unique_name, _server)
diff --git a/include/core/tree_manager/tree_manager_sliding_window.h b/include/core/tree_manager/tree_manager_sliding_window.h
index 229ff57900a1cc8bbb3ee8e136328175f6375af3..8b2deec3b4fe8de046e1dd6c65c646be1a773727 100644
--- a/include/core/tree_manager/tree_manager_sliding_window.h
+++ b/include/core/tree_manager/tree_manager_sliding_window.h
@@ -15,18 +15,21 @@ 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_key_frames = _server.getParam<unsigned int>(prefix + "/n_key_frames");
+ fix_first_key_frame = _server.getParam<bool> (prefix + "/fix_first_key_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";
+ 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"
+ + "viral_remove_empty_parent: " + std::to_string(viral_remove_empty_parent) + "\n";
}
unsigned int n_key_frames;
bool fix_first_key_frame;
+ bool viral_remove_empty_parent;
};
class TreeManagerSlidingWindow : public TreeManagerBase
diff --git a/src/tree_manager/tree_manager_sliding_window.cpp b/src/tree_manager/tree_manager_sliding_window.cpp
index 52d91021af6d492dfc7dea960ee5c2106bfa15c4..710cd3566cd477e2dd8f9c02c0a71070c20e07d5 100644
--- a/src/tree_manager/tree_manager_sliding_window.cpp
+++ b/src/tree_manager/tree_manager_sliding_window.cpp
@@ -5,8 +5,6 @@ namespace wolf
void TreeManagerSlidingWindow::keyFrameCallback(FrameBasePtr _key_frame)
{
- WOLF_INFO("TreeManagerSlidingWindow: keyFrameCallback!");
-
int n_kf(0);
FrameBasePtr first_KF(nullptr), second_KF(nullptr);
for (auto frm : getProblem()->getTrajectory()->getFrameList())
@@ -24,9 +22,13 @@ void TreeManagerSlidingWindow::keyFrameCallback(FrameBasePtr _key_frame)
// remove first KF if too many KF
if (n_kf > params_sw_->n_key_frames)
{
- first_KF->remove();
+ WOLF_INFO("TreeManagerSlidingWindow removing first frame");
+ first_KF->remove(params_sw_->viral_remove_empty_parent);
if (params_sw_->fix_first_key_frame)
+ {
+ WOLF_INFO("TreeManagerSlidingWindow fixing new first frame");
second_KF->fix();
+ }
}
}
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 98d3c91cda3f6725fe9c458557ddfb4bd6ec2e9e..83e0aa1df36f4018463fc2bb16ac57f96e329def 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -233,6 +233,10 @@ target_link_libraries(gtest_processor_tracker_landmark_dummy ${PROJECT_NAME} dum
wolf_add_gtest(gtest_sensor_diff_drive gtest_sensor_diff_drive.cpp)
target_link_libraries(gtest_sensor_diff_drive ${PROJECT_NAME})
+# TreeManagerSlidingWindow class test
+wolf_add_gtest(gtest_tree_manager_sliding_window gtest_tree_manager_sliding_window.cpp)
+target_link_libraries(gtest_tree_manager_sliding_window ${PROJECT_NAME})
+
# yaml conversions
wolf_add_gtest(gtest_yaml_conversions gtest_yaml_conversions.cpp)
target_link_libraries(gtest_yaml_conversions ${PROJECT_NAME})
diff --git a/test/gtest_tree_manager_sliding_window.cpp b/test/gtest_tree_manager_sliding_window.cpp
index c48b993490027545b5184cc6a271a82a53fe37fe..14b05976faa99160657c303e028c1f651872f7ac 100644
--- a/test/gtest_tree_manager_sliding_window.cpp
+++ b/test/gtest_tree_manager_sliding_window.cpp
@@ -4,10 +4,10 @@
#include "core/problem/problem.h"
#include "core/tree_manager/tree_manager_sliding_window.h"
#include "core/yaml/parser_yaml.hpp"
-#include"core/capture/capture_void.h"
-#include"core/feature/feature_base.h"
-#include"core/factor/factor_odom_3d.h"
-#include"core/factor/factor_pose_3d.h"
+#include "core/capture/capture_void.h"
+#include "core/feature/feature_base.h"
+#include "core/factor/factor_odom_3d.h"
+#include "core/factor/factor_pose_3d.h"
using namespace wolf;
using namespace Eigen;
@@ -70,7 +70,7 @@ TEST(TreeManagerSlidingWindow, autoConf)
ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(P->getTreeManager()) != nullptr);
}
-TEST(TreeManagerSlidingWindow, slidingWindowFix)
+TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
{
ParserYAML parser = ParserYAML("test/yaml/params_tree_manager_sliding_window1.yaml", wolf_root);
ParamsServer server = ParamsServer(parser.getParams());
@@ -83,57 +83,181 @@ TEST(TreeManagerSlidingWindow, slidingWindowFix)
Vector7d state = F1->getState();
Vector7d zero_disp(state);
+ Matrix6d cov = Matrix6d::Identity();
// FRAME 2 ----------------------------------------------------------
- auto F2 = P->emplaceFrame("PO", 3, KEY, state, TimeStamp(1));
+ auto F2 = P->emplaceFrame("PO", 3, KEY, state, TimeStamp(2));
P->keyFrameCallback(F2, nullptr, 0);
// absolute factor
- auto C2 = CaptureBase::emplace<CaptureVoid>(F2);
- auto f2 = FeatureBase::emplace<FeatureBase>(C2, state);
+ 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);
- auto f12 = FeatureBase::emplace<FeatureBase>(C12, zero_disp);
- auto c12 = FactorBase::emplace<FactorOdom3d>(f12, f12, F2, nullptr, false);
+ 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
ASSERT_FALSE(F1->isRemoving());
// FRAME 3 ----------------------------------------------------------
- auto F3 = P->emplaceFrame("PO", 3, KEY, state, TimeStamp(1));
+ auto F3 = P->emplaceFrame("PO", 3, KEY, state, TimeStamp(3));
P->keyFrameCallback(F3, nullptr, 0);
// absolute factor
- auto C3 = CaptureBase::emplace<CaptureVoid>(F3);
- auto f3 = FeatureBase::emplace<FeatureBase>(C3, state);
+ 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);
- auto f23 = FeatureBase::emplace<FeatureBase>(C23, zero_disp);
- auto c23 = FactorBase::emplace<FactorOdom3d>(f23, f23, F3, nullptr, false);
+ 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
ASSERT_FALSE(F1->isRemoving());
// FRAME 4 ----------------------------------------------------------
- auto F4 = P->emplaceFrame("PO", 3, KEY, state, TimeStamp(1));
+ auto F4 = P->emplaceFrame("PO", 3, KEY, state, TimeStamp(4));
P->keyFrameCallback(F4, nullptr, 0);
// absolute factor
- auto C4 = CaptureBase::emplace<CaptureVoid>(F4);
- auto f4 = FeatureBase::emplace<FeatureBase>(C4, state);
+ 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);
- auto f34 = FeatureBase::emplace<FeatureBase>(C34, zero_disp);
- auto c34 = FactorBase::emplace<FactorOdom3d>(f34, f34, F4, nullptr, false);
+ 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);
- // Check frame F1 (prior) removed
+ // 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
+ // FRAME 5 ----------------------------------------------------------
+ auto F5 = P->emplaceFrame("PO", 3, KEY, state, TimeStamp(5));
+ P->keyFrameCallback(F5, nullptr, 0);
- ASSERT_EQ(GM->n_KF_, 1);
+ // 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
+ 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
+}
+
+TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
+{
+ ParserYAML parser = ParserYAML("test/yaml/params_tree_manager_sliding_window2.yaml", wolf_root);
+ ParamsServer server = ParamsServer(parser.getParams());
+
+ ProblemPtr P = Problem::autoSetup(server);
+
+ // FRAME 1 ----------------------------------------------------------
+ auto F1 = P->getTrajectory()->getLastKeyFrame();
+ ASSERT_TRUE(F1 != nullptr);
+
+ Vector7d state = F1->getState();
+ Vector7d zero_disp(state);
+ Matrix6d cov = Matrix6d::Identity();
+
+ // FRAME 2 ----------------------------------------------------------
+ auto F2 = P->emplaceFrame("PO", 3, KEY, state, TimeStamp(2));
+ 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
+ ASSERT_FALSE(F1->isRemoving());
+
+ // FRAME 3 ----------------------------------------------------------
+ auto F3 = P->emplaceFrame("PO", 3, KEY, state, TimeStamp(3));
+ 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
+ ASSERT_FALSE(F1->isRemoving());
+
+ // FRAME 4 ----------------------------------------------------------
+ auto F4 = P->emplaceFrame("PO", 3, KEY, state, TimeStamp(4));
+ 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
+ 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
+
+ // FRAME 5 ----------------------------------------------------------
+ auto F5 = P->emplaceFrame("PO", 3, KEY, state, TimeStamp(5));
+ 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
+ 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
}
int main(int argc, char **argv)
diff --git a/test/yaml/params_tree_manager_sliding_window1.yaml b/test/yaml/params_tree_manager_sliding_window1.yaml
index db88862872617570a07902460f1e91fab6342b89..5cbb9e5a787b4ad2e70f9cf7f283ab1747e1279b 100644
--- a/test/yaml/params_tree_manager_sliding_window1.yaml
+++ b/test/yaml/params_tree_manager_sliding_window1.yaml
@@ -10,7 +10,8 @@ config:
tree_manager:
type: "TreeManagerSlidingWindow"
n_key_frames: 3
- fix_first_key_frame: false
+ fix_first_key_frame: true
+ viral_remove_empty_parent: true
sensors:
-
type: "SensorOdom3d"
diff --git a/test/yaml/params_tree_manager_sliding_window2.yaml b/test/yaml/params_tree_manager_sliding_window2.yaml
index e0bb4e191f5bc38a352d79aaa91d6d70be8dc1a9..01f41eea97c7cca943d4aa12a143736668c2673f 100644
--- a/test/yaml/params_tree_manager_sliding_window2.yaml
+++ b/test/yaml/params_tree_manager_sliding_window2.yaml
@@ -10,7 +10,8 @@ config:
tree_manager:
type: "TreeManagerSlidingWindow"
n_key_frames: 3
- fix_first_key_frame: true
+ fix_first_key_frame: false
+ viral_remove_empty_parent: false
sensors:
-
type: "SensorOdom3d"