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
Hide whitespace changes
Inline Side-by-side
Showing
with 172 additions and 36 deletions
include/core/tree_manager/tree_manager_base.h
+ 2
− 1
View file @ 47562670
...@@ -5,6 +5,7 @@ ...@@ -5,6 +5,7 @@
#include "core/common/wolf.h" #include "core/common/wolf.h"
#include "core/common/node_base.h" #include "core/common/node_base.h"
#include "core/common/params_base.h" #include "core/common/params_base.h"
#include "core/problem/problem.h"
namespace wolf namespace wolf
{ {
...@@ -45,7 +46,7 @@ static TreeManagerBasePtr create(const std::string& _unique_name, ...@@ -45,7 +46,7 @@ static TreeManagerBasePtr create(const std::string& _unique_name,
struct ParamsTreeManagerBase : public ParamsBase struct ParamsTreeManagerBase : public ParamsBase
{ {
std::string prefix = "problem/tree_manager/"; std::string prefix = "problem/tree_manager";
ParamsTreeManagerBase() = default; ParamsTreeManagerBase() = default;
ParamsTreeManagerBase(std::string _unique_name, const ParamsServer& _server): ParamsTreeManagerBase(std::string _unique_name, const ParamsServer& _server):
ParamsBase(_unique_name, _server) ParamsBase(_unique_name, _server)
......
include/core/tree_manager/tree_manager_sliding_window.h
+ 8
− 5
View file @ 47562670
...@@ -15,18 +15,21 @@ struct ParamsTreeManagerSlidingWindow : public ParamsTreeManagerBase ...@@ -15,18 +15,21 @@ struct ParamsTreeManagerSlidingWindow : public ParamsTreeManagerBase
ParamsTreeManagerSlidingWindow(std::string _unique_name, const wolf::ParamsServer & _server) : ParamsTreeManagerSlidingWindow(std::string _unique_name, const wolf::ParamsServer & _server) :
ParamsTreeManagerBase(_unique_name, _server) ParamsTreeManagerBase(_unique_name, _server)
{ {
n_key_frames = _server.getParam<unsigned int>(prefix + "/n_key_frames"); n_key_frames = _server.getParam<unsigned int>(prefix + "/n_key_frames");
fix_first_key_frame = _server.getParam<bool> (prefix + "/fix_first_key_frame"); 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 std::string print() const
{ {
return "\n" + ParamsTreeManagerBase::print() + "\n" return "\n" + ParamsTreeManagerBase::print() + "\n"
+ "n_key_frames: " + std::to_string(n_key_frames) + "\n" + "n_key_frames: " + std::to_string(n_key_frames) + "\n"
+ "fix_first_key_frame: " + std::to_string(fix_first_key_frame) + "\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; unsigned int n_key_frames;
bool fix_first_key_frame; bool fix_first_key_frame;
bool viral_remove_empty_parent;
}; };
class TreeManagerSlidingWindow : public TreeManagerBase class TreeManagerSlidingWindow : public TreeManagerBase
......
src/tree_manager/tree_manager_sliding_window.cpp
+ 5
− 3
View file @ 47562670
...@@ -5,8 +5,6 @@ namespace wolf ...@@ -5,8 +5,6 @@ namespace wolf
void TreeManagerSlidingWindow::keyFrameCallback(FrameBasePtr _key_frame) void TreeManagerSlidingWindow::keyFrameCallback(FrameBasePtr _key_frame)
{ {
WOLF_INFO("TreeManagerSlidingWindow: keyFrameCallback!");
int n_kf(0); int n_kf(0);
FrameBasePtr first_KF(nullptr), second_KF(nullptr); FrameBasePtr first_KF(nullptr), second_KF(nullptr);
for (auto frm : getProblem()->getTrajectory()->getFrameList()) for (auto frm : getProblem()->getTrajectory()->getFrameList())
...@@ -24,9 +22,13 @@ void TreeManagerSlidingWindow::keyFrameCallback(FrameBasePtr _key_frame) ...@@ -24,9 +22,13 @@ void TreeManagerSlidingWindow::keyFrameCallback(FrameBasePtr _key_frame)
// remove first KF if too many KF // remove first KF if too many KF
if (n_kf > params_sw_->n_key_frames) 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) if (params_sw_->fix_first_key_frame)
{
WOLF_INFO("TreeManagerSlidingWindow fixing new first frame");
second_KF->fix(); second_KF->fix();
}
} }
} }
......
test/CMakeLists.txt
+ 4
− 0
View file @ 47562670
...@@ -233,6 +233,10 @@ target_link_libraries(gtest_processor_tracker_landmark_dummy ${PROJECT_NAME} dum ...@@ -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) wolf_add_gtest(gtest_sensor_diff_drive gtest_sensor_diff_drive.cpp)
target_link_libraries(gtest_sensor_diff_drive ${PROJECT_NAME}) 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 # yaml conversions
wolf_add_gtest(gtest_yaml_conversions gtest_yaml_conversions.cpp) wolf_add_gtest(gtest_yaml_conversions gtest_yaml_conversions.cpp)
target_link_libraries(gtest_yaml_conversions ${PROJECT_NAME}) target_link_libraries(gtest_yaml_conversions ${PROJECT_NAME})
......
test/gtest_tree_manager_sliding_window.cpp
+ 149
− 25
View file @ 47562670
...@@ -4,10 +4,10 @@ ...@@ -4,10 +4,10 @@
#include "core/problem/problem.h" #include "core/problem/problem.h"
#include "core/tree_manager/tree_manager_sliding_window.h" #include "core/tree_manager/tree_manager_sliding_window.h"
#include "core/yaml/parser_yaml.hpp" #include "core/yaml/parser_yaml.hpp"
#include"core/capture/capture_void.h" #include "core/capture/capture_void.h"
#include"core/feature/feature_base.h" #include "core/feature/feature_base.h"
#include"core/factor/factor_odom_3d.h" #include "core/factor/factor_odom_3d.h"
#include"core/factor/factor_pose_3d.h" #include "core/factor/factor_pose_3d.h"
using namespace wolf; using namespace wolf;
using namespace Eigen; using namespace Eigen;
...@@ -70,7 +70,7 @@ TEST(TreeManagerSlidingWindow, autoConf) ...@@ -70,7 +70,7 @@ TEST(TreeManagerSlidingWindow, autoConf)
ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(P->getTreeManager()) != nullptr); 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); ParserYAML parser = ParserYAML("test/yaml/params_tree_manager_sliding_window1.yaml", wolf_root);
ParamsServer server = ParamsServer(parser.getParams()); ParamsServer server = ParamsServer(parser.getParams());
...@@ -83,57 +83,181 @@ TEST(TreeManagerSlidingWindow, slidingWindowFix) ...@@ -83,57 +83,181 @@ TEST(TreeManagerSlidingWindow, slidingWindowFix)
Vector7d state = F1->getState(); Vector7d state = F1->getState();
Vector7d zero_disp(state); Vector7d zero_disp(state);
Matrix6d cov = Matrix6d::Identity();
// FRAME 2 ---------------------------------------------------------- // 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); P->keyFrameCallback(F2, nullptr, 0);
// absolute factor // absolute factor
auto C2 = CaptureBase::emplace<CaptureVoid>(F2); auto C2 = CaptureBase::emplace<CaptureVoid>(F2, TimeStamp(2), nullptr);
auto f2 = FeatureBase::emplace<FeatureBase>(C2, state); auto f2 = FeatureBase::emplace<FeatureBase>(C2, "absolute", state, cov);
auto c2 = FactorBase::emplace<FactorPose3d>(f2, f2, nullptr, false); auto c2 = FactorBase::emplace<FactorPose3d>(f2, f2, nullptr, false);
// displacement // displacement
auto C12 = CaptureBase::emplace<CaptureVoid>(F2); auto C12 = CaptureBase::emplace<CaptureVoid>(F2, TimeStamp(2), nullptr);
auto f12 = FeatureBase::emplace<FeatureBase>(C12, zero_disp); auto f12 = FeatureBase::emplace<FeatureBase>(C12, "odom", zero_disp, cov);
auto c12 = FactorBase::emplace<FactorOdom3d>(f12, f12, F2, nullptr, false); auto c12 = FactorBase::emplace<FactorOdom3d>(f12, f12, F1, nullptr, false);
// Check no frame removed // Check no frame removed
ASSERT_FALSE(F1->isRemoving()); ASSERT_FALSE(F1->isRemoving());
// FRAME 3 ---------------------------------------------------------- // 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); P->keyFrameCallback(F3, nullptr, 0);
// absolute factor // absolute factor
auto C3 = CaptureBase::emplace<CaptureVoid>(F3); auto C3 = CaptureBase::emplace<CaptureVoid>(F3, TimeStamp(3), nullptr);
auto f3 = FeatureBase::emplace<FeatureBase>(C3, state); auto f3 = FeatureBase::emplace<FeatureBase>(C3, "absolute", state, cov);
auto c3 = FactorBase::emplace<FactorPose3d>(f3, f3, nullptr, false); auto c3 = FactorBase::emplace<FactorPose3d>(f3, f3, nullptr, false);
// displacement // displacement
auto C23 = CaptureBase::emplace<CaptureVoid>(F3); auto C23 = CaptureBase::emplace<CaptureVoid>(F3, TimeStamp(3), nullptr);
auto f23 = FeatureBase::emplace<FeatureBase>(C23, zero_disp); auto f23 = FeatureBase::emplace<FeatureBase>(C23, "odom", zero_disp, cov);
auto c23 = FactorBase::emplace<FactorOdom3d>(f23, f23, F3, nullptr, false); auto c23 = FactorBase::emplace<FactorOdom3d>(f23, f23, F2, nullptr, false);
// Check no frame removed // Check no frame removed
ASSERT_FALSE(F1->isRemoving()); ASSERT_FALSE(F1->isRemoving());
// FRAME 4 ---------------------------------------------------------- // 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); P->keyFrameCallback(F4, nullptr, 0);
// absolute factor // absolute factor
auto C4 = CaptureBase::emplace<CaptureVoid>(F4); auto C4 = CaptureBase::emplace<CaptureVoid>(F4, TimeStamp(4), nullptr);
auto f4 = FeatureBase::emplace<FeatureBase>(C4, state); auto f4 = FeatureBase::emplace<FeatureBase>(C4, "absolute", state, cov);
auto c4 = FactorBase::emplace<FactorPose3d>(f4, f4, nullptr, false); auto c4 = FactorBase::emplace<FactorPose3d>(f4, f4, nullptr, false);
// displacement // displacement
auto C34 = CaptureBase::emplace<CaptureVoid>(F4); auto C34 = CaptureBase::emplace<CaptureVoid>(F4, TimeStamp(4), nullptr);
auto f34 = FeatureBase::emplace<FeatureBase>(C34, zero_disp); auto f34 = FeatureBase::emplace<FeatureBase>(C34, "odom", zero_disp, cov);
auto c34 = FactorBase::emplace<FactorOdom3d>(f34, f34, F4, nullptr, false); auto c34 = FactorBase::emplace<FactorOdom3d>(f34, f34, F3, nullptr, false);
// Check frame F1 (prior) removed // Checks
ASSERT_TRUE(F1->isRemoving()); 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) int main(int argc, char **argv)
......
test/yaml/params_tree_manager_sliding_window1.yaml
+ 2
− 1
View file @ 47562670
...@@ -10,7 +10,8 @@ config: ...@@ -10,7 +10,8 @@ config:
tree_manager: tree_manager:
type: "TreeManagerSlidingWindow" type: "TreeManagerSlidingWindow"
n_key_frames: 3 n_key_frames: 3
fix_first_key_frame: false fix_first_key_frame: true
viral_remove_empty_parent: true
sensors: sensors:
- -
type: "SensorOdom3d" type: "SensorOdom3d"
......
test/yaml/params_tree_manager_sliding_window2.yaml
+ 2
− 1
View file @ 47562670
...@@ -10,7 +10,8 @@ config: ...@@ -10,7 +10,8 @@ config:
tree_manager: tree_manager:
type: "TreeManagerSlidingWindow" type: "TreeManagerSlidingWindow"
n_key_frames: 3 n_key_frames: 3
fix_first_key_frame: true fix_first_key_frame: false
viral_remove_empty_parent: false
sensors: sensors:
- -
type: "SensorOdom3d" type: "SensorOdom3d"
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment