Skip to content
Snippets Groups Projects
Commit 47562670 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

sliding window working and exhaustive test

parent 97df889d
No related branches found
No related tags found
1 merge request!349Resolve "New GraphManager class"
Pipeline #5101 passed
......@@ -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)
......
......@@ -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
......
......@@ -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();
}
}
}
......
......@@ -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})
......
......@@ -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)
......
......@@ -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"
......
......@@ -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"
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment