From 2846efc035f7d9910a8d768eeca06a530316d78a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Tue, 31 May 2022 16:36:20 +0200
Subject: [PATCH] all working and gtests all ok
---
CMakeLists.txt | 16 +-
demos/hello_wolf/hello_wolf_autoconf.cpp | 7 +-
demos/hello_wolf/processor_range_bearing.cpp | 1 -
include/core/processor/factory_processor.h | 47 ++--
include/core/processor/motion_provider.h | 7 +-
include/core/processor/processor_base.h | 70 +++---
.../processor/processor_fixed_wing_model.h | 6 +-
.../core/processor/processor_loop_closure.h | 2 +-
include/core/processor/processor_motion.h | 10 +-
include/core/processor/processor_odom_2d.h | 2 +-
include/core/processor/processor_tracker.h | 6 +-
include/core/sensor/sensor_base.h | 4 +-
include/core/sensor/sensor_motion_model.h | 31 +--
include/core/sensor/sensor_odom.h | 12 +-
include/core/sensor/sensor_pose.h | 6 +-
include/core/solver/solver_manager.h | 10 +-
include/core/state_block/prior.h | 2 -
.../core/tree_manager/factory_tree_manager.h | 31 ++-
include/core/tree_manager/tree_manager_base.h | 50 ++--
.../tree_manager_sliding_window.h | 10 +-
.../tree_manager_sliding_window_dual_rate.h | 12 +-
include/core/yaml/parser_yaml.h | 3 +-
src/problem/problem.cpp | 37 ++-
src/processor/processor_diff_drive.cpp | 1 -
src/processor/processor_fixed_wing_model.cpp | 7 -
src/processor/processor_odom_2d.cpp | 1 -
src/processor/processor_odom_3d.cpp | 1 -
src/processor/processor_pose.cpp | 1 -
src/sensor/sensor_motion_model.cpp | 31 ++-
src/sensor/sensor_pose.cpp | 2 -
.../tree_manager_sliding_window.cpp | 1 -
.../tree_manager_sliding_window_dual_rate.cpp | 1 -
src/yaml/parser_yaml.cpp | 15 +-
src/yaml/processor_odom_3d_yaml.cpp | 76 ------
src/yaml/sensor_odom_2d_yaml.cpp | 70 ------
src/yaml/sensor_odom_3d_yaml.cpp | 71 ------
src/yaml/sensor_pose_yaml.cpp | 68 ------
test/CMakeLists.txt | 31 ++-
test/dummy/processor_motion_provider_dummy.h | 1 -
.../dummy/processor_tracker_feature_dummy.cpp | 1 -
.../processor_tracker_landmark_dummy.cpp | 1 -
test/dummy/tree_manager_dummy.h | 7 +-
test/gtest_factor_diff_drive.cpp | 224 +++++++++---------
...actor_relative_pose_2d_with_extrinsics.cpp | 4 +-
...actor_relative_pose_3d_with_extrinsics.cpp | 22 +-
test/gtest_factory.cpp | 2 +-
test/gtest_param_prior.cpp | 64 +++--
test/gtest_param_server.cpp | 6 +-
test/gtest_parser_yaml.cpp | 12 +-
test/gtest_prior.cpp | 8 +-
test/gtest_problem.cpp | 6 +-
...sor_and_factor_pose_3d_with_extrinsics.cpp | 17 +-
test/gtest_processor_diff_drive.cpp | 106 ++++-----
test/gtest_processor_fixed_wing_model.cpp | 21 +-
test/gtest_processor_loop_closure.cpp | 18 +-
test/gtest_processor_motion.cpp | 4 +-
test/gtest_sensor_base.cpp | 18 +-
test/gtest_sensor_diff_drive.cpp | 4 +-
test/gtest_sensor_odom.cpp | 10 +-
test/gtest_sensor_pose.cpp | 8 +-
test/gtest_tree_manager.cpp | 73 +++++-
test/gtest_tree_manager_sliding_window.cpp | 74 +++++-
..._tree_manager_sliding_window_dual_rate.cpp | 78 +++++-
test/yaml/params1.yaml | 4 +-
test/yaml/params3.yaml | 2 +-
test/yaml/params_tree_manager1.yaml | 4 +-
.../params_tree_manager_sliding_window1.yaml | 19 +-
.../params_tree_manager_sliding_window2.yaml | 19 +-
...ree_manager_sliding_window_dual_rate1.yaml | 9 +-
...ree_manager_sliding_window_dual_rate3.yaml | 16 +-
...ger_sliding_window_dual_rate_baseline.yaml | 16 +-
test/yaml/processor_odom_3d.yaml | 7 +-
test/yaml/tree_manager_dummy.yaml | 3 +
test/yaml/tree_manager_sliding_window1.yaml | 5 +
test/yaml/tree_manager_sliding_window2.yaml | 5 +
...ree_manager_sliding_window_dual_rate1.yaml | 7 +
76 files changed, 807 insertions(+), 857 deletions(-)
delete mode 100644 src/yaml/processor_odom_3d_yaml.cpp
delete mode 100644 src/yaml/sensor_odom_2d_yaml.cpp
delete mode 100644 src/yaml/sensor_odom_3d_yaml.cpp
delete mode 100644 src/yaml/sensor_pose_yaml.cpp
create mode 100644 test/yaml/tree_manager_dummy.yaml
create mode 100644 test/yaml/tree_manager_sliding_window1.yaml
create mode 100644 test/yaml/tree_manager_sliding_window2.yaml
create mode 100644 test/yaml/tree_manager_sliding_window_dual_rate1.yaml
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 6cb77a634..a52e95f75 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -182,8 +182,8 @@ SET(HDRS_PROCESSOR
include/${PROJECT_NAME}/processor/motion_provider.h
include/${PROJECT_NAME}/processor/processor_base.h
include/${PROJECT_NAME}/processor/processor_diff_drive.h
- # include/${PROJECT_NAME}/processor/processor_fixed_wing_model.h
- # include/${PROJECT_NAME}/processor/processor_loop_closure.h
+ include/${PROJECT_NAME}/processor/processor_fixed_wing_model.h
+ include/${PROJECT_NAME}/processor/processor_loop_closure.h
include/${PROJECT_NAME}/processor/processor_motion.h
include/${PROJECT_NAME}/processor/processor_odom_2d.h
include/${PROJECT_NAME}/processor/processor_odom_3d.h
@@ -197,7 +197,7 @@ SET(HDRS_SENSOR
include/${PROJECT_NAME}/sensor/factory_sensor.h
include/${PROJECT_NAME}/sensor/sensor_base.h
include/${PROJECT_NAME}/sensor/sensor_diff_drive.h
- # include/${PROJECT_NAME}/sensor/sensor_motion_model.h
+ include/${PROJECT_NAME}/sensor/sensor_motion_model.h
include/${PROJECT_NAME}/sensor/sensor_odom.h
include/${PROJECT_NAME}/sensor/sensor_pose.h
)
@@ -290,8 +290,8 @@ SET(SRCS_PROCESSOR
src/processor/motion_provider.cpp
src/processor/processor_base.cpp
src/processor/processor_diff_drive.cpp
- # src/processor/processor_fixed_wing_model.cpp
- # src/processor/processor_loop_closure.cpp
+ src/processor/processor_fixed_wing_model.cpp
+ src/processor/processor_loop_closure.cpp
src/processor/processor_motion.cpp
src/processor/processor_odom_2d.cpp
src/processor/processor_odom_3d.cpp
@@ -304,7 +304,7 @@ SET(SRCS_PROCESSOR
SET(SRCS_SENSOR
src/sensor/sensor_base.cpp
src/sensor/sensor_diff_drive.cpp
- # src/sensor/sensor_motion_model.cpp
+ src/sensor/sensor_motion_model.cpp
src/sensor/sensor_odom.cpp
src/sensor/sensor_pose.cpp
)
@@ -336,10 +336,6 @@ SET(SRCS_UTILS
)
SET(SRCS_YAML
src/yaml/parser_yaml.cpp
- src/yaml/processor_odom_3d_yaml.cpp
- # src/yaml/sensor_odom_2d_yaml.cpp
- # src/yaml/sensor_odom_3d_yaml.cpp
- # src/yaml/sensor_pose_yaml.cpp
)
# ============ OPTIONALS ============
diff --git a/demos/hello_wolf/hello_wolf_autoconf.cpp b/demos/hello_wolf/hello_wolf_autoconf.cpp
index 39492e197..13e92f792 100644
--- a/demos/hello_wolf/hello_wolf_autoconf.cpp
+++ b/demos/hello_wolf/hello_wolf_autoconf.cpp
@@ -120,10 +120,10 @@ int main()
// Config file to parse. Here is where all the problem is defined:
std::string config_file = "demos/hello_wolf/yaml/hello_wolf_config.yaml";
- std::string wolf_path = std::string(_WOLF_ROOT_DIR);
+ std::string wolf_path = _WOLF_ROOT_DIR;
// parse file into params server: each param will be retrievable from this params server:
- ParserYaml parser = ParserYaml(config_file, wolf_path);
+ ParserYaml parser = ParserYaml(wolf_path + "/" + config_file);
ParamsServer server = ParamsServer(parser.getParams());
// Wolf problem: automatically build the left branch of the wolf tree from the contents of the params server:
ProblemPtr problem = Problem::autoSetup(server);
@@ -132,9 +132,6 @@ int main()
problem->print(4,0,1,0);
// Solver. Configure a Ceres solver
- // ceres::Solver::Options options;
- // options.max_num_iterations = 1000; // We depart far from solution, need a lot of iterations
- // SolverCeresPtr ceres = std::make_shared<SolverCeres>(problem, options);
SolverManagerPtr ceres = FactorySolver::create("SolverCeres", problem, server);
// recover sensor pointers and other stuff for later use (access by sensor name)
diff --git a/demos/hello_wolf/processor_range_bearing.cpp b/demos/hello_wolf/processor_range_bearing.cpp
index db0f5c12b..5cace0ba1 100644
--- a/demos/hello_wolf/processor_range_bearing.cpp
+++ b/demos/hello_wolf/processor_range_bearing.cpp
@@ -183,6 +183,5 @@ bool ProcessorRangeBearing::storeCapture(CaptureBasePtr _cap_ptr)
namespace wolf
{
WOLF_REGISTER_PROCESSOR(ProcessorRangeBearing)
-WOLF_REGISTER_PROCESSOR_AUTO(ProcessorRangeBearing)
} // namespace wolf
diff --git a/include/core/processor/factory_processor.h b/include/core/processor/factory_processor.h
index 74f01419e..5abc327f4 100644
--- a/include/core/processor/factory_processor.h
+++ b/include/core/processor/factory_processor.h
@@ -126,43 +126,50 @@ namespace wolf
*
*/
typedef Factory<ProcessorBase,
- const std::string&,
- const ParamsProcessorBasePtr> FactoryProcessor;
+ const std::string&,
+ const ParamsProcessorBasePtr> FactoryProcessor;
template<>
inline std::string FactoryProcessor::getClass() const
{
return "FactoryProcessor";
}
+typedef Factory<ProcessorBase,
+ const std::string&,
+ const ParamsServer&> FactoryProcessorServer;
+template<>
+inline std::string FactoryProcessorServer::getClass() const
+{
+ return "FactoryProcessorServer";
+}
+
+typedef Factory<ProcessorBase,
+ const std::string&,
+ const std::string&> FactoryProcessorYaml;
+template<>
+inline std::string FactoryProcessorYaml::getClass() const
+{
+ return "FactoryProcessorYaml";
+}
// ParamsProcessor factory
struct ParamsProcessorBase;
typedef Factory<ParamsProcessorBase,
- const std::string&> FactoryParamsProcessor;
+ const std::string&> FactoryParamsProcessor;
template<>
inline std::string FactoryParamsProcessor::getClass() const
{
return "FactoryParamsProcessor";
}
-#define WOLF_REGISTER_PROCESSOR(ProcessorType) \
- namespace{ const bool WOLF_UNUSED ProcessorType##Registered = \
- wolf::FactoryProcessor::registerCreator(#ProcessorType, ProcessorType::create); } \
-
-typedef Factory<ProcessorBase,
- const std::string&,
- const ParamsServer&> AutoConfFactoryProcessor;
-template<>
-inline std::string AutoConfFactoryProcessor::getClass() const
-{
- return "AutoConfFactoryProcessor";
-}
-
+#define WOLF_REGISTER_PROCESSOR(ProcessorType) \
+ namespace{ const bool WOLF_UNUSED ProcessorType##ServerRegistered = \
+ wolf::FactoryProcessorServer::registerCreator(#ProcessorType, ProcessorType::create); } \
+ namespace{ const bool WOLF_UNUSED ProcessorType##Registered = \
+ wolf::FactoryProcessor::registerCreator(#ProcessorType, ProcessorType::create); } \
+ namespace{ const bool WOLF_UNUSED ProcessorType##YamlRegistered = \
+ wolf::FactoryProcessorYaml::registerCreator(#ProcessorType, ProcessorType::create); } \
-#define WOLF_REGISTER_PROCESSOR_AUTO(ProcessorType) \
- namespace{ const bool WOLF_UNUSED ProcessorType##AutoConfRegistered = \
- wolf::AutoConfFactoryProcessor::registerCreator(#ProcessorType, ProcessorType::create); } \
} /* namespace wolf */
-
#endif /* PROCESSOR_FACTORY_H_ */
diff --git a/include/core/processor/motion_provider.h b/include/core/processor/motion_provider.h
index d7bd26d00..2f6a63573 100644
--- a/include/core/processor/motion_provider.h
+++ b/include/core/processor/motion_provider.h
@@ -47,12 +47,13 @@ struct ParamsMotionProvider
ParamsMotionProvider(std::string _unique_name, const ParamsServer& _server)
{
state_getter = _server.getParam<bool>("processor/" + _unique_name + "/state_getter");
- state_priority = _server.getParam<double>("processor/" + _unique_name + "/state_priority");
+ if (state_getter)
+ state_priority = _server.getParam<double>("processor/" + _unique_name + "/state_priority");
}
std::string print() const
{
- return "state_getter: " + std::to_string(state_getter) + "\n"
- + "state_priority: " + std::to_string(state_priority) + "\n";
+ return "state_getter: " + toString(state_getter) + "\n"
+ + "state_priority: " + toString(state_priority) + "\n";
}
};
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index b5c4cf7ce..931ab1bac 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -54,32 +54,44 @@ namespace wolf {
*
* ProcessorClass(const ParamsProcessorClassPtr _params);
*/
-#define WOLF_PROCESSOR_CREATE(ProcessorClass, ParamsProcessorClass) \
-static ProcessorBasePtr create(const std::string& _unique_name, \
- const ParamsServer& _server) \
-{ \
- auto params = std::make_shared<ParamsProcessorClass>(_unique_name, _server); \
- \
- auto processor = std::make_shared<ProcessorClass>(params); \
- \
- processor ->setName(_unique_name); \
- \
- return processor; \
-} \
-static ProcessorBasePtr create(const std::string& _unique_name, const ParamsProcessorBasePtr _params) \
-{ \
- auto params = std::static_pointer_cast<ParamsProcessorClass>(_params); \
- \
- auto processor = std::make_shared<ProcessorClass>(params); \
- \
- processor ->setName(_unique_name); \
- \
- return processor; \
-} \
-
-
-
-
+#define WOLF_PROCESSOR_CREATE(ProcessorClass, ParamsProcessorClass) \
+static ProcessorBasePtr create(const std::string& _unique_name, \
+ const ParamsServer& _server) \
+{ \
+ auto params = std::make_shared<ParamsProcessorClass>(_unique_name, _server); \
+ \
+ auto processor = std::make_shared<ProcessorClass>(params); \
+ \
+ processor ->setName(_unique_name); \
+ \
+ return processor; \
+} \
+static ProcessorBasePtr create(const std::string& _unique_name, \
+ const ParamsProcessorBasePtr _params) \
+{ \
+ auto params = std::static_pointer_cast<ParamsProcessorClass>(_params); \
+ \
+ auto processor = std::make_shared<ProcessorClass>(params); \
+ \
+ processor ->setName(_unique_name); \
+ \
+ return processor; \
+} \
+static ProcessorBasePtr create(const std::string& _unique_name, \
+ const std::string& _yaml_filepath) \
+{ \
+ auto parser = ParserYaml(_yaml_filepath, true); \
+ \
+ auto server = ParamsServer(parser.getParams(), "processor/" + _unique_name); \
+ \
+ auto params = std::make_shared<ParamsProcessorClass>(_unique_name, server); \
+ \
+ auto processor = std::make_shared<ProcessorClass>(params); \
+ \
+ processor ->setName(_unique_name); \
+ \
+ return processor; \
+} \
/** \brief Buffer for arbitrary type objects
@@ -224,9 +236,9 @@ struct ParamsProcessorBase : public ParamsBase
std::string print() const override
{
- return "voting_active: " + std::to_string(voting_active) + "\n"
- + "time_tolerance: " + std::to_string(time_tolerance) + "\n"
- + "apply_loss_function: " + std::to_string(apply_loss_function) + "\n";
+ return "voting_active: " + toString(voting_active) + "\n"
+ + "time_tolerance: " + toString(time_tolerance) + "\n"
+ + "apply_loss_function: " + toString(apply_loss_function) + "\n";
}
};
diff --git a/include/core/processor/processor_fixed_wing_model.h b/include/core/processor/processor_fixed_wing_model.h
index bccbc2317..b882750a5 100644
--- a/include/core/processor/processor_fixed_wing_model.h
+++ b/include/core/processor/processor_fixed_wing_model.h
@@ -55,9 +55,9 @@ struct ParamsProcessorFixedWingModel : public ParamsProcessorBase
std::string print() const override
{
return ParamsProcessorBase::print() + "\n"
- + "velocity_local: print not implemented\n"
- + "angle_stdev: " + std::to_string(angle_stdev) + "\n"
- + "min_vel_norm: " + std::to_string(min_vel_norm) + "\n";
+ + "velocity_local: "+ toString(velocity_local) + "\n"
+ + "angle_stdev: " + toString(angle_stdev) + "\n"
+ + "min_vel_norm: " + toString(min_vel_norm) + "\n";
}
};
diff --git a/include/core/processor/processor_loop_closure.h b/include/core/processor/processor_loop_closure.h
index b6e4d965b..cf602f451 100644
--- a/include/core/processor/processor_loop_closure.h
+++ b/include/core/processor/processor_loop_closure.h
@@ -43,7 +43,7 @@ struct ParamsProcessorLoopClosure : public ParamsProcessorBase
std::string print() const override
{
return "\n" + ParamsProcessorBase::print()
- + "max_loops: " + std::to_string(max_loops) + "\n";
+ + "max_loops: " + toString(max_loops) + "\n";
}
};
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index 7c0cf9b2c..3e34bf694 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -67,11 +67,11 @@ struct ParamsProcessorMotion : public ParamsProcessorBase, public ParamsMotionPr
{
return ParamsProcessorBase::print() + "\n" +
ParamsMotionProvider::print() + "\n"
- + "max_time_span: " + std::to_string(max_time_span) + "\n"
- + "max_buff_length: " + std::to_string(max_buff_length) + "\n"
- + "dist_traveled: " + std::to_string(dist_traveled) + "\n"
- + "angle_turned: " + std::to_string(angle_turned) + "\n"
- + "unmeasured_perturbation_std: " + std::to_string(unmeasured_perturbation_std) + "\n";
+ + "max_time_span: " + toString(max_time_span) + "\n"
+ + "max_buff_length: " + toString(max_buff_length) + "\n"
+ + "dist_traveled: " + toString(dist_traveled) + "\n"
+ + "angle_turned: " + toString(angle_turned) + "\n"
+ + "unmeasured_perturbation_std: " + toString(unmeasured_perturbation_std) + "\n";
}
};
diff --git a/include/core/processor/processor_odom_2d.h b/include/core/processor/processor_odom_2d.h
index aa9011ea6..51ada92bb 100644
--- a/include/core/processor/processor_odom_2d.h
+++ b/include/core/processor/processor_odom_2d.h
@@ -54,7 +54,7 @@ struct ParamsProcessorOdom2d : public ParamsProcessorMotion
std::string print() const override
{
return ParamsProcessorMotion::print() + "\n"
- + "cov_det: " + std::to_string(cov_det) + "\n";
+ + "cov_det: " + toString(cov_det) + "\n";
}
};
diff --git a/include/core/processor/processor_tracker.h b/include/core/processor/processor_tracker.h
index b0683aba0..5ea080d61 100644
--- a/include/core/processor/processor_tracker.h
+++ b/include/core/processor/processor_tracker.h
@@ -50,9 +50,9 @@ struct ParamsProcessorTracker : public ParamsProcessorBase
}
std::string print() const override
{
- return ParamsProcessorBase::print() + "\n"
- + "min_features_for_keyframe: " + std::to_string(min_features_for_keyframe) + "\n"
- + "max_new_features: " + std::to_string(max_new_features) + "\n";
+ return ParamsProcessorBase::print() + "\n"
+ + "min_features_for_keyframe: " + toString(min_features_for_keyframe) + "\n"
+ + "max_new_features: " + toString(max_new_features) + "\n";
}
};
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 8c56d51e5..2e778b40f 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -76,8 +76,10 @@ static SensorBasePtr create(const std::string& _unique_name,
SizeEigen _dim, \
const std::string& _yaml_filepath) \
{ \
- auto parser = ParserYaml(_yaml_filepath, "", true); \
+ auto parser = ParserYaml(_yaml_filepath, true); \
+ \
auto server = ParamsServer(parser.getParams(), "sensor/" + _unique_name); \
+ \
auto params = std::make_shared<ParamsSensorClass>(_unique_name, server); \
\
return std::make_shared<SensorClass>(_unique_name, _dim, params, server); \
diff --git a/include/core/sensor/sensor_motion_model.h b/include/core/sensor/sensor_motion_model.h
index 68da8c7d4..f56ff1d72 100644
--- a/include/core/sensor/sensor_motion_model.h
+++ b/include/core/sensor/sensor_motion_model.h
@@ -27,31 +27,26 @@
namespace wolf {
-
WOLF_PTR_TYPEDEFS(SensorMotionModel);
class SensorMotionModel : public SensorBase
{
public:
- SensorMotionModel();
+ SensorMotionModel(const std::string& _unique_name,
+ const SizeEigen& _dim,
+ ParamsSensorBasePtr _params,
+ const Priors& _priors);
+
+ SensorMotionModel(const std::string& _unique_name,
+ const SizeEigen& _dim,
+ ParamsSensorBasePtr _params,
+ const ParamsServer& _server);
+
+ WOLF_SENSOR_CREATE(SensorMotionModel, ParamsSensorBase);
+
~SensorMotionModel() override;
- static SensorBasePtr create(const std::string& _unique_name,
- const ParamsServer& _server)
- {
- auto sensor = std::make_shared<SensorMotionModel>();
- sensor ->setName(_unique_name);
- return sensor;
- }
-
- static SensorBasePtr create(const std::string& _unique_name,
- const Eigen::VectorXd& _extrinsics,
- const ParamsSensorBasePtr _intrinsics)
- {
- auto sensor = std::make_shared<SensorMotionModel>();
- sensor ->setName(_unique_name);
- return sensor;
- }
+ Eigen::MatrixXd computeNoiseCov(const Eigen::VectorXd & _data) const override;
};
diff --git a/include/core/sensor/sensor_odom.h b/include/core/sensor/sensor_odom.h
index 3bb0eff61..41590e1cf 100644
--- a/include/core/sensor/sensor_odom.h
+++ b/include/core/sensor/sensor_odom.h
@@ -51,12 +51,12 @@ struct ParamsSensorOdom : public ParamsSensorBase
~ParamsSensorOdom() override = default;
std::string print() const override
{
- return ParamsSensorBase::print() + "\n"
- + "k_disp_to_disp: " + std::to_string(k_disp_to_disp) + "\n"
- + "k_disp_to_rot: " + std::to_string(k_disp_to_rot) + "\n"
- + "k_rot_to_rot: " + std::to_string(k_rot_to_rot) + "\n"
- + "min_disp_var: " + std::to_string(min_disp_var) + "\n"
- + "min_rot_var: " + std::to_string(min_rot_var) + "\n";
+ return ParamsSensorBase::print() + "\n"
+ + "k_disp_to_disp: " + toString(k_disp_to_disp) + "\n"
+ + "k_disp_to_rot: " + toString(k_disp_to_rot) + "\n"
+ + "k_rot_to_rot: " + toString(k_rot_to_rot) + "\n"
+ + "min_disp_var: " + toString(min_disp_var) + "\n"
+ + "min_rot_var: " + toString(min_rot_var) + "\n";
}
};
diff --git a/include/core/sensor/sensor_pose.h b/include/core/sensor/sensor_pose.h
index 72f589183..344de02c9 100644
--- a/include/core/sensor/sensor_pose.h
+++ b/include/core/sensor/sensor_pose.h
@@ -51,9 +51,9 @@ struct ParamsSensorPose : public ParamsSensorBase
~ParamsSensorPose() override = default;
std::string print() const override
{
- return ParamsSensorBase::print() + "\n"
- + "std_p: " + std::to_string(std_p) + "\n"
- + "std_o: " + std::to_string(std_o) + "\n";
+ return ParamsSensorBase::print() + "\n"
+ + "std_p: " + toString(std_p) + "\n"
+ + "std_o: " + toString(std_o) + "\n";
}
};
diff --git a/include/core/solver/solver_manager.h b/include/core/solver/solver_manager.h
index c98804624..b8e99a5a9 100644
--- a/include/core/solver/solver_manager.h
+++ b/include/core/solver/solver_manager.h
@@ -259,11 +259,11 @@ struct ParamsSolver: public ParamsBase
}
std::string print() const override
{
- return "period: " + std::to_string(period) + "\n" +
- "verbose: " + std::to_string((int)verbose) + "\n" +
- "compute_cov: " + std::to_string(compute_cov) + "\n" +
- "cov_enum: " + std::to_string((int)cov_enum) + "\n" +
- "cov_period: " + std::to_string(cov_period) + "\n";
+ return "period: " + toString(period) + "\n" +
+ "verbose: " + toString((int)verbose) + "\n" +
+ "compute_cov: " + toString(compute_cov) + "\n" +
+ "cov_enum: " + toString((int)cov_enum) + "\n" +
+ "cov_period: " + toString(cov_period) + "\n";
}
~ParamsSolver() override = default;
diff --git a/include/core/state_block/prior.h b/include/core/state_block/prior.h
index 38bce67d8..38412c65d 100644
--- a/include/core/state_block/prior.h
+++ b/include/core/state_block/prior.h
@@ -26,8 +26,6 @@
#include <unordered_map>
#include <eigen3/Eigen/Dense>
-using std::to_string;
-
namespace wolf
{
class ParamsServer;
diff --git a/include/core/tree_manager/factory_tree_manager.h b/include/core/tree_manager/factory_tree_manager.h
index ae4782088..fe382c986 100644
--- a/include/core/tree_manager/factory_tree_manager.h
+++ b/include/core/tree_manager/factory_tree_manager.h
@@ -51,31 +51,36 @@ inline std::string FactoryParamsTreeManager::getClass() const
// TreeManager factory
typedef Factory<TreeManagerBase,
- const std::string&,
- const ParamsTreeManagerBasePtr> FactoryTreeManager;
+ ParamsTreeManagerBasePtr> FactoryTreeManager;
template<>
inline std::string FactoryTreeManager::getClass() const
{
return "FactoryTreeManager";
}
-#define WOLF_REGISTER_TREE_MANAGER(TreeManagerType) \
- namespace{ const bool WOLF_UNUSED TreeManagerType##Registered = \
- wolf::FactoryTreeManager::registerCreator(#TreeManagerType, TreeManagerType::create); } \
-
typedef Factory<TreeManagerBase,
- const std::string&,
- const ParamsServer&> AutoConfFactoryTreeManager;
+ const ParamsServer&> FactoryTreeManagerServer;
template<>
-inline std::string AutoConfFactoryTreeManager::getClass() const
+inline std::string FactoryTreeManagerServer::getClass() const
{
- return "AutoConfFactoryTreeManager";
+ return "FactoryTreeManagerServer";
}
+typedef Factory<TreeManagerBase,
+ const std::string&> FactoryTreeManagerYaml;
+template<>
+inline std::string FactoryTreeManagerYaml::getClass() const
+{
+ return "FactoryTreeManagerYaml";
+}
-#define WOLF_REGISTER_TREE_MANAGER_AUTO(TreeManagerType) \
- namespace{ const bool WOLF_UNUSED TreeManagerType##AutoConfRegistered = \
- wolf::AutoConfFactoryTreeManager::registerCreator(#TreeManagerType, TreeManagerType::create); } \
+#define WOLF_REGISTER_TREE_MANAGER(TreeManagerType) \
+ namespace{ const bool WOLF_UNUSED TreeManagerType##Registered = \
+ wolf::FactoryTreeManager::registerCreator(#TreeManagerType, TreeManagerType::create); } \
+ namespace{ const bool WOLF_UNUSED TreeManagerType##ServerRegistered = \
+ wolf::FactoryTreeManagerServer::registerCreator(#TreeManagerType, TreeManagerType::create); } \
+ namespace{ const bool WOLF_UNUSED TreeManagerType##YamlRegistered = \
+ wolf::FactoryTreeManagerYaml::registerCreator(#TreeManagerType, TreeManagerType::create); } \
} /* namespace wolf */
diff --git a/include/core/tree_manager/tree_manager_base.h b/include/core/tree_manager/tree_manager_base.h
index 8aa2ac43d..5d3d9c76a 100644
--- a/include/core/tree_manager/tree_manager_base.h
+++ b/include/core/tree_manager/tree_manager_base.h
@@ -41,36 +41,36 @@ namespace wolf
*
* TreeManagerClass(const ParamsTreeManagerPtr _params);
*/
-#define WOLF_TREE_MANAGER_CREATE(TreeManagerClass, ParamsTreeManagerClass) \
-static TreeManagerBasePtr create(const std::string& _unique_name, \
- const ParamsServer& _server) \
-{ \
- auto params = std::make_shared<ParamsTreeManagerClass>(_unique_name, _server); \
- \
- auto tree_manager = std::make_shared<TreeManagerClass>(params); \
- \
- tree_manager ->setName(_unique_name); \
- \
- return tree_manager; \
-} \
-static TreeManagerBasePtr create(const std::string& _unique_name, \
- const ParamsTreeManagerBasePtr _params) \
-{ \
- auto params = std::static_pointer_cast<ParamsTreeManagerClass>(_params); \
- \
- auto tree_manager = std::make_shared<TreeManagerClass>(params); \
- \
- tree_manager ->setName(_unique_name); \
- \
- return tree_manager; \
-} \
+#define WOLF_TREE_MANAGER_CREATE(TreeManagerClass, ParamsTreeManagerClass) \
+static TreeManagerBasePtr create(const ParamsServer& _server) \
+{ \
+ auto params = std::make_shared<ParamsTreeManagerClass>(_server); \
+ \
+ return std::make_shared<TreeManagerClass>(params); \
+} \
+static TreeManagerBasePtr create(ParamsTreeManagerBasePtr _params) \
+{ \
+ auto params = std::static_pointer_cast<ParamsTreeManagerClass>(_params); \
+ \
+ return std::make_shared<TreeManagerClass>(params); \
+} \
+static TreeManagerBasePtr create(const std::string& _yaml_filepath) \
+{ \
+ auto parser = ParserYaml(_yaml_filepath, true); \
+ \
+ auto server = ParamsServer(parser.getParams(), "problem/tree_manager/"); \
+ \
+ auto params = std::make_shared<ParamsTreeManagerClass>(server); \
+ \
+ return std::make_shared<TreeManagerClass>(params); \
+} \
struct ParamsTreeManagerBase : public ParamsBase
{
std::string prefix = "problem/tree_manager";
ParamsTreeManagerBase() = default;
- ParamsTreeManagerBase(std::string _unique_name, const ParamsServer& _server):
- ParamsBase(_unique_name, _server)
+ ParamsTreeManagerBase(const ParamsServer& _server):
+ ParamsBase("tree_manager", _server)
{
}
diff --git a/include/core/tree_manager/tree_manager_sliding_window.h b/include/core/tree_manager/tree_manager_sliding_window.h
index 46eedc0f2..65f650c62 100644
--- a/include/core/tree_manager/tree_manager_sliding_window.h
+++ b/include/core/tree_manager/tree_manager_sliding_window.h
@@ -33,8 +33,8 @@ WOLF_PTR_TYPEDEFS(TreeManagerSlidingWindow)
struct ParamsTreeManagerSlidingWindow : public ParamsTreeManagerBase
{
ParamsTreeManagerSlidingWindow() = default;
- ParamsTreeManagerSlidingWindow(std::string _unique_name, const wolf::ParamsServer & _server) :
- ParamsTreeManagerBase(_unique_name, _server)
+ ParamsTreeManagerSlidingWindow(const wolf::ParamsServer & _server) :
+ ParamsTreeManagerBase(_server)
{
n_frames = _server.getParam<unsigned int>(prefix + "/n_frames");
n_fix_first_frames = _server.getParam<unsigned int>(prefix + "/n_fix_first_frames");
@@ -45,9 +45,9 @@ struct ParamsTreeManagerSlidingWindow : public ParamsTreeManagerBase
std::string print() const override
{
return ParamsTreeManagerBase::print() + "\n"
- + "n_frames: " + std::to_string(n_frames) + "\n"
- + "n_fix_first_frames: " + std::to_string(n_fix_first_frames) + "\n"
- + "viral_remove_empty_parent: " + std::to_string(viral_remove_empty_parent) + "\n";
+ + "n_frames: " + toString(n_frames) + "\n"
+ + "n_fix_first_frames: " + toString(n_fix_first_frames) + "\n"
+ + "viral_remove_empty_parent: " + toString(viral_remove_empty_parent) + "\n";
}
unsigned int n_frames;
diff --git a/include/core/tree_manager/tree_manager_sliding_window_dual_rate.h b/include/core/tree_manager/tree_manager_sliding_window_dual_rate.h
index 6df971b5b..a4f9a0fa7 100644
--- a/include/core/tree_manager/tree_manager_sliding_window_dual_rate.h
+++ b/include/core/tree_manager/tree_manager_sliding_window_dual_rate.h
@@ -33,8 +33,8 @@ WOLF_PTR_TYPEDEFS(TreeManagerSlidingWindowDualRate)
struct ParamsTreeManagerSlidingWindowDualRate : public ParamsTreeManagerSlidingWindow
{
ParamsTreeManagerSlidingWindowDualRate() = default;
- ParamsTreeManagerSlidingWindowDualRate(std::string _unique_name, const wolf::ParamsServer & _server) :
- ParamsTreeManagerSlidingWindow(_unique_name, _server)
+ ParamsTreeManagerSlidingWindowDualRate(const wolf::ParamsServer & _server) :
+ ParamsTreeManagerSlidingWindow(_server)
{
n_frames_recent = _server.getParam<unsigned int>(prefix + "/n_frames_recent");
assert(n_frames_recent <= n_frames);
@@ -42,9 +42,9 @@ struct ParamsTreeManagerSlidingWindowDualRate : public ParamsTreeManagerSlidingW
}
std::string print() const override
{
- return ParamsTreeManagerBase::print() + "\n"
- + "n_frames_recent: " + std::to_string(n_frames_recent) + "\n"
- + "rate_old_frames: " + std::to_string(rate_old_frames) + "\n";
+ return ParamsTreeManagerBase::print() + "\n"
+ + "n_frames_recent: " + toString(n_frames_recent) + "\n"
+ + "rate_old_frames: " + toString(rate_old_frames) + "\n";
}
unsigned int n_frames_recent, rate_old_frames;
@@ -54,7 +54,7 @@ class TreeManagerSlidingWindowDualRate : public TreeManagerSlidingWindow
{
public:
TreeManagerSlidingWindowDualRate(ParamsTreeManagerSlidingWindowDualRatePtr _params);
- ;
+
WOLF_TREE_MANAGER_CREATE(TreeManagerSlidingWindowDualRate, ParamsTreeManagerSlidingWindowDualRate)
~TreeManagerSlidingWindowDualRate() override{}
diff --git a/include/core/yaml/parser_yaml.h b/include/core/yaml/parser_yaml.h
index d5f948961..e5ee7ed52 100644
--- a/include/core/yaml/parser_yaml.h
+++ b/include/core/yaml/parser_yaml.h
@@ -71,8 +71,7 @@ class ParserYaml {
YAML::Node loadYaml(std::string);
void insert_register(std::string, std::string);
public:
- ParserYaml(std::string _file, std::string _path_root = "",
- bool _freely_parse = false);
+ ParserYaml(std::string _file, bool _freely_parse = false);
~ParserYaml()
{
//
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 7e646b2b0..59299e8bd 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -101,19 +101,10 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
// Load plugins
auto loaders = std::vector<std::shared_ptr<Loader>>();
std::string plugins_path = _WOLF_LIB_DIR;
- if (plugins_path.back() != '/'){
- plugins_path += '/'; // only works for UNIX systems
- }
- /*try
+ if (plugins_path.back() != '/')
{
- plugins_path = _server.getParam<std::string>("plugins_path");
+ plugins_path += '/'; // only works for UNIX systems
}
- catch (MissingValueException& e)
- {
- WOLF_WARN(e.what());
- WOLF_WARN("Setting '/usr/local/lib/' as plugins path...");
- plugins_path="/usr/local/lib/";
- }*/
for (auto plugin_name : _server.getParam<std::vector<std::string>>("plugins"))
{
if (plugin_name == "core" or plugin_name == "wolf" or plugin_name == "") continue; // ignore plugin "core"
@@ -181,7 +172,7 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
std::string tree_manager_type = _server.getParam<std::string>("problem/tree_manager/type");
WOLF_TRACE("Tree Manager Type: ", tree_manager_type);
if (tree_manager_type != "None" and tree_manager_type != "none")
- problem->setTreeManager(AutoConfFactoryTreeManager::create(tree_manager_type, "tree manager", _server));
+ problem->setTreeManager(FactoryTreeManagerServer::create(tree_manager_type, _server));
// Set problem prior -- first keyframe
std::string prior_mode = _server.getParam<std::string>("problem/prior/mode");
@@ -286,12 +277,20 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
throw std::runtime_error("Cannot bind processor. Reason: Sensor \"" + _corresponding_sensor_name + "\" not found. Check sensor name, it must match in sensor and processor!");
if (_params_filename == "")
return installProcessor(_prc_type, _unique_processor_name, sen_ptr, nullptr);
- else
- {
- assert(file_exists(_params_filename) && "Cannot install processor: parameters' YAML file does not exist.");
- ParamsProcessorBasePtr prc_params = FactoryParamsProcessor::create(_prc_type, _params_filename);
- return installProcessor(_prc_type, _unique_processor_name, sen_ptr, prc_params);
- }
+ if (not file_exists(_params_filename))
+ throw std::runtime_error("Cannot install processor: parameters' YAML file does not exist: " + _params_filename);
+
+ ProcessorBasePtr prc_ptr = FactoryProcessorYaml::create(_prc_type, _unique_processor_name, _params_filename);
+
+ //Dimension check
+ int prc_dim = prc_ptr->getDim();
+ auto prb = this;
+ assert(((prc_dim == 0) or (prc_dim == prb->getDim())) && "Processor and Problem do not agree on dimension");
+
+ prc_ptr->configure(sen_ptr);
+ prc_ptr->link(sen_ptr);
+
+ return prc_ptr;
}
ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
@@ -303,7 +302,7 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
if (sen_ptr == nullptr)
throw std::runtime_error("Cannot bind processor. Reason: Sensor \"" + _corresponding_sensor_name + "\" not found. Check sensor name, it must match in sensor and processor!");
- ProcessorBasePtr prc_ptr = AutoConfFactoryProcessor::create(_prc_type, _unique_processor_name, _server);
+ ProcessorBasePtr prc_ptr = FactoryProcessorServer::create(_prc_type, _unique_processor_name, _server);
//Dimension check
int prc_dim = prc_ptr->getDim();
diff --git a/src/processor/processor_diff_drive.cpp b/src/processor/processor_diff_drive.cpp
index beb1c7a4b..3e1ea99bb 100644
--- a/src/processor/processor_diff_drive.cpp
+++ b/src/processor/processor_diff_drive.cpp
@@ -197,6 +197,5 @@ FactorBasePtr ProcessorDiffDrive::emplaceFactor(FeatureBasePtr _feature,
#include "core/processor/factory_processor.h"
namespace wolf {
WOLF_REGISTER_PROCESSOR(ProcessorDiffDrive);
-WOLF_REGISTER_PROCESSOR_AUTO(ProcessorDiffDrive);
} // namespace wolf
diff --git a/src/processor/processor_fixed_wing_model.cpp b/src/processor/processor_fixed_wing_model.cpp
index 4f67c52a8..25ec91bc9 100644
--- a/src/processor/processor_fixed_wing_model.cpp
+++ b/src/processor/processor_fixed_wing_model.cpp
@@ -19,12 +19,6 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-/*
- * processor_fix_wing_model.cpp
- *
- * Created on: Sep 6, 2021
- * Author: joanvallve
- */
#include "core/processor/processor_fixed_wing_model.h"
@@ -82,6 +76,5 @@ void ProcessorFixedWingModel::processKeyFrame(FrameBasePtr _keyframe_ptr)
#include "core/processor/factory_processor.h"
namespace wolf {
WOLF_REGISTER_PROCESSOR(ProcessorFixedWingModel);
-WOLF_REGISTER_PROCESSOR_AUTO(ProcessorFixedWingModel);
} // namespace wolf
diff --git a/src/processor/processor_odom_2d.cpp b/src/processor/processor_odom_2d.cpp
index 78ecb0fdd..ec6a42639 100644
--- a/src/processor/processor_odom_2d.cpp
+++ b/src/processor/processor_odom_2d.cpp
@@ -195,5 +195,4 @@ FeatureBasePtr ProcessorOdom2d::emplaceFeature(CaptureMotionPtr _capture_motion)
#include "core/processor/factory_processor.h"
namespace wolf {
WOLF_REGISTER_PROCESSOR(ProcessorOdom2d);
-WOLF_REGISTER_PROCESSOR_AUTO(ProcessorOdom2d);
} // namespace wolf
diff --git a/src/processor/processor_odom_3d.cpp b/src/processor/processor_odom_3d.cpp
index e96f3a7de..da78f58b3 100644
--- a/src/processor/processor_odom_3d.cpp
+++ b/src/processor/processor_odom_3d.cpp
@@ -257,6 +257,5 @@ void ProcessorOdom3d::setCalibration (const CaptureBasePtr _capture, const Vecto
#include "core/processor/factory_processor.h"
namespace wolf {
WOLF_REGISTER_PROCESSOR(ProcessorOdom3d);
-WOLF_REGISTER_PROCESSOR_AUTO(ProcessorOdom3d);
} // namespace wolf
diff --git a/src/processor/processor_pose.cpp b/src/processor/processor_pose.cpp
index 727a49d74..a9f53bf72 100644
--- a/src/processor/processor_pose.cpp
+++ b/src/processor/processor_pose.cpp
@@ -134,5 +134,4 @@ inline void ProcessorPose::processKeyFrame(FrameBasePtr _keyframe_ptr)
#include "core/processor/factory_processor.h"
namespace wolf {
WOLF_REGISTER_PROCESSOR(ProcessorPose);
-WOLF_REGISTER_PROCESSOR_AUTO(ProcessorPose);
} // namespace wolf
diff --git a/src/sensor/sensor_motion_model.cpp b/src/sensor/sensor_motion_model.cpp
index d2f400876..167d33528 100644
--- a/src/sensor/sensor_motion_model.cpp
+++ b/src/sensor/sensor_motion_model.cpp
@@ -23,16 +23,38 @@
namespace wolf {
+SensorMotionModel::SensorMotionModel(const std::string& _unique_name,
+ const SizeEigen& _dim,
+ ParamsSensorBasePtr _params,
+ const Priors& _priors) :
+ SensorBase("SensorMotionModel",
+ _unique_name,
+ _dim,
+ _params,
+ _priors)
+{
+}
-SensorMotionModel::SensorMotionModel() :
- SensorBase("SensorMotionModel", nullptr, nullptr, nullptr, 6)
+SensorMotionModel::SensorMotionModel(const std::string& _unique_name,
+ const SizeEigen& _dim,
+ ParamsSensorBasePtr _params,
+ const ParamsServer& _server) :
+ SensorBase("SensorMotionModel",
+ _unique_name,
+ _dim,
+ _params,
+ _server)
{
- //
}
SensorMotionModel::~SensorMotionModel()
{
- //
+}
+
+Eigen::MatrixXd SensorMotionModel::computeNoiseCov(const Eigen::VectorXd & _data) const
+{
+ throw std::runtime_error("SensorMotionModel::computeNoiseCov not implemented!");
+ return Eigen::MatrixXd(0,0);
}
} // namespace wolf
@@ -41,5 +63,4 @@ SensorMotionModel::~SensorMotionModel()
#include "core/sensor/factory_sensor.h"
namespace wolf {
WOLF_REGISTER_SENSOR(SensorMotionModel);
-WOLF_REGISTER_SENSOR_AUTO(SensorMotionModel);
}
diff --git a/src/sensor/sensor_pose.cpp b/src/sensor/sensor_pose.cpp
index 5be1e7473..4eb43f291 100644
--- a/src/sensor/sensor_pose.cpp
+++ b/src/sensor/sensor_pose.cpp
@@ -42,7 +42,6 @@ SensorPose::SensorPose(const std::string& _unique_name,
dim_(_dim),
params_pose_(_params)
{
- assert(dim_ == 2 or dim_ == 3);
}
SensorPose::SensorPose(const std::string& _unique_name,
@@ -57,7 +56,6 @@ SensorPose::SensorPose(const std::string& _unique_name,
dim_(_dim),
params_pose_(_params)
{
- assert(dim_ == 2 or dim_ == 3);
}
SensorPose::~SensorPose()
diff --git a/src/tree_manager/tree_manager_sliding_window.cpp b/src/tree_manager/tree_manager_sliding_window.cpp
index 1a0f44507..e7cdbb8fb 100644
--- a/src/tree_manager/tree_manager_sliding_window.cpp
+++ b/src/tree_manager/tree_manager_sliding_window.cpp
@@ -65,6 +65,5 @@ void TreeManagerSlidingWindow::keyFrameCallback(FrameBasePtr _frame)
#include "core/tree_manager/factory_tree_manager.h"
namespace wolf {
WOLF_REGISTER_TREE_MANAGER(TreeManagerSlidingWindow);
-WOLF_REGISTER_TREE_MANAGER_AUTO(TreeManagerSlidingWindow);
} // namespace wolf
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 85b9c4cf7..6ec268ddb 100644
--- a/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp
+++ b/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp
@@ -94,6 +94,5 @@ void TreeManagerSlidingWindowDualRate::keyFrameCallback(FrameBasePtr _frame)
#include "core/tree_manager/factory_tree_manager.h"
namespace wolf {
WOLF_REGISTER_TREE_MANAGER(TreeManagerSlidingWindowDualRate);
-WOLF_REGISTER_TREE_MANAGER_AUTO(TreeManagerSlidingWindowDualRate);
} // namespace wolf
diff --git a/src/yaml/parser_yaml.cpp b/src/yaml/parser_yaml.cpp
index ecc7aa088..5a415ca23 100644
--- a/src/yaml/parser_yaml.cpp
+++ b/src/yaml/parser_yaml.cpp
@@ -182,7 +182,7 @@ std::map<std::string, std::string> fetchAsMap(YAML::Node _n){
return false;
}
}
-ParserYaml::ParserYaml(std::string _file, std::string path_root, bool freely_parse)
+ParserYaml::ParserYaml(std::string _file, bool freely_parse)
{
params_ = std::map<std::string, std::string>();
active_name_ = "";
@@ -192,14 +192,11 @@ ParserYaml::ParserYaml(std::string _file, std::string path_root, bool freely_par
publisher_managers_ = std::vector<PublisherManager>();
parsing_file_ = std::stack<std::string>();
file_ = _file;
- if (path_root != "")
- {
- std::regex r(".*/ *$");
- if (not std::regex_match(path_root, r))
- path_root_ = path_root + "/";
- else
- path_root_ = path_root;
- }
+
+ std::size_t found = _file.find_last_of("/");
+ path_root_ = _file.substr(0,found+1);
+ file_ = _file.substr(found+1);
+
if (not freely_parse)
parse();
else
diff --git a/src/yaml/processor_odom_3d_yaml.cpp b/src/yaml/processor_odom_3d_yaml.cpp
deleted file mode 100644
index 771d19b74..000000000
--- a/src/yaml/processor_odom_3d_yaml.cpp
+++ /dev/null
@@ -1,76 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-/**
- * \file processor_odom_3d_yaml.cpp
- *
- * Created on: Oct 25, 2016
- * \author: jsola
- */
-
-// wolf yaml
-#include "core/yaml/yaml_conversion.h"
-
-// wolf
-#include "core/processor/processor_odom_3d.h"
-#include "core/processor/factory_processor.h"
-
-// yaml-cpp library
-#include <yaml-cpp/yaml.h>
-
-namespace wolf
-{
-
-namespace
-{
-static ParamsProcessorBasePtr createProcessorOdom3dParams(const std::string & _filename_dot_yaml)
-{
- YAML::Node config = YAML::LoadFile(_filename_dot_yaml);
-
- if (config["type"].as<std::string>() == "ProcessorOdom3d")
- {
- ParamsProcessorOdom3dPtr params = std::make_shared<ParamsProcessorOdom3d>();
-
- params->time_tolerance = config["time_tolerance"] .as<double>();
- params->unmeasured_perturbation_std = config["unmeasured_perturbation_std"] .as<double>();
-
- YAML::Node keyframe_vote = config["keyframe_vote"];
-
- params->voting_active = keyframe_vote["voting_active"] .as<bool>();
- params->max_time_span = keyframe_vote["max_time_span"] .as<double>();
- params->max_buff_length = keyframe_vote["max_buff_length"] .as<SizeEigen>();
- params->dist_traveled = keyframe_vote["dist_traveled"] .as<double>();
- params->angle_turned = keyframe_vote["angle_turned"] .as<double>();
-
- return params;
- }
-
- std::cout << "Bad configuration file. No processor type found." << std::endl;
- return nullptr;
-}
-
-// Register in the FactorySensor
-const bool WOLF_UNUSED registered_prc_odom_3d = FactoryParamsProcessor::registerCreator("ProcessorOdom3d", createProcessorOdom3dParams);
-
-} // namespace [unnamed]
-
-} // namespace wolf
-
diff --git a/src/yaml/sensor_odom_2d_yaml.cpp b/src/yaml/sensor_odom_2d_yaml.cpp
deleted file mode 100644
index dcb0cf0c1..000000000
--- a/src/yaml/sensor_odom_2d_yaml.cpp
+++ /dev/null
@@ -1,70 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-/**
- * \file sensor_odom_2d_yaml.cpp
- *
- * Created on: Aug 3, 2019
- * \author: jsola
- */
-
-
-
-// wolf yaml
-#include "core/yaml/yaml_conversion.h"
-
-// wolf
-#include "core/sensor/sensor_odom_2d.h"
-#include "core/sensor/factory_sensor.h"
-
-// yaml-cpp library
-#include <yaml-cpp/yaml.h>
-
-namespace wolf
-{
-
-namespace
-{
-static ParamsSensorBasePtr createParamsSensorOdom2d(const std::string & _filename_dot_yaml)
-{
- YAML::Node config = YAML::LoadFile(_filename_dot_yaml);
-
- if (config["type"].as<std::string>() == "SensorOdom2d")
- {
- auto params = std::make_shared<ParamsSensorOdom2d>();
-
- params->k_disp_to_disp = config["k_disp_to_disp"] .as<double>();
- params->k_rot_to_rot = config["k_rot_to_rot"] .as<double>();
-
- return params;
- }
-
- std::cout << "Bad configuration file. No sensor type found." << std::endl;
- return nullptr;
-}
-
-// Register in the FactorySensor
-const bool WOLF_UNUSED registered_odom_2d_intr = FactoryParamsSensor::registerCreator("SensorOdom2d", createParamsSensorOdom2d);
-
-} // namespace [unnamed]
-
-} // namespace wolf
-
diff --git a/src/yaml/sensor_odom_3d_yaml.cpp b/src/yaml/sensor_odom_3d_yaml.cpp
deleted file mode 100644
index 0cb47d441..000000000
--- a/src/yaml/sensor_odom_3d_yaml.cpp
+++ /dev/null
@@ -1,71 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-/**
- * \file sensor_odom_3d_yaml.cpp
- *
- * Created on: Oct 25, 2016
- * \author: jsola
- */
-
-// wolf yaml
-#include "core/yaml/yaml_conversion.h"
-
-// wolf
-#include "core/sensor/sensor_odom_3d.h"
-#include "core/sensor/factory_sensor.h"
-
-// yaml-cpp library
-#include <yaml-cpp/yaml.h>
-
-namespace wolf
-{
-
-namespace
-{
-static ParamsSensorBasePtr createParamsSensorOdom3d(const std::string & _filename_dot_yaml)
-{
- YAML::Node config = YAML::LoadFile(_filename_dot_yaml);
-
- if (config["type"].as<std::string>() == "SensorOdom3d")
- {
- ParamsSensorOdom3dPtr params = std::make_shared<ParamsSensorOdom3d>();
-
- params->k_disp_to_disp = config["k_disp_to_disp"] .as<double>();
- params->k_disp_to_rot = config["k_disp_to_rot"] .as<double>();
- params->k_rot_to_rot = config["k_rot_to_rot"] .as<double>();
- params->min_disp_var = config["min_disp_var"] .as<double>();
- params->min_rot_var = config["min_rot_var"] .as<double>();
-
- return params;
- }
-
- std::cout << "Bad configuration file. No sensor type found." << std::endl;
- return nullptr;
-}
-
-// Register in the FactorySensor
-const bool WOLF_UNUSED registered_odom_3d_intr = FactoryParamsSensor::registerCreator("SensorOdom3d", createParamsSensorOdom3d);
-
-} // namespace [unnamed]
-
-} // namespace wolf
-
diff --git a/src/yaml/sensor_pose_yaml.cpp b/src/yaml/sensor_pose_yaml.cpp
deleted file mode 100644
index bfbe151c4..000000000
--- a/src/yaml/sensor_pose_yaml.cpp
+++ /dev/null
@@ -1,68 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of WOLF
-// WOLF is free software: you can redistribute it and/or modify
-// it under the terms of the GNU Lesser General Public License as published by
-// the Free Software Foundation, either version 3 of the License, or
-// at your option) any later version.
-//
-// This program is distributed in the hope that it will be useful,
-// but WITHOUT ANY WARRANTY; without even the implied warranty of
-// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-// GNU Lesser General Public License for more details.
-//
-// You should have received a copy of the GNU Lesser General Public License
-// along with this program. If not, see <http://www.gnu.org/licenses/>.
-//
-//--------LICENSE_END--------
-/**
- * \file sensor_pose_yaml.cpp
- *
- * Created on: Feb 18, 2020
- * \author: mfourmy
- */
-
-// wolf yaml
-#include "core/yaml/yaml_conversion.h"
-
-// wolf
-#include "core/sensor/sensor_pose.h"
-#include "core/sensor/factory_sensor.h"
-
-// yaml-cpp library
-#include <yaml-cpp/yaml.h>
-
-namespace wolf
-{
-
-namespace
-{
-static ParamsSensorBasePtr createParamsSensorPose(const std::string & _filename_dot_yaml)
-{
- YAML::Node config = YAML::LoadFile(_filename_dot_yaml);
-
- if (config["type"].as<std::string>() == "SensorPose")
- {
- ParamsSensorPosePtr params = std::make_shared<ParamsSensorPose>();
-
- params->std_p = config["std_p"].as<double>();
- params->std_o = config["std_o"].as<double>();
-
- return params;
- }
-
- std::cout << "Bad configuration file. No sensor type found." << std::endl;
- return nullptr;
-}
-
-// Register in the FactorySensor
-const bool WOLF_UNUSED registered_odom_3d_intr = FactoryParamsSensor::registerCreator("SensorPose", createParamsSensorPose);
-
-} // namespace [unnamed]
-
-} // namespace wolf
-
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index c5693f17b..df10a34dc 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -100,7 +100,7 @@ wolf_add_gtest(gtest_processor_base gtest_processor_base.cpp)
target_link_libraries(gtest_processor_base PUBLIC dummy)
# ProcessorMotion class test
-# wolf_add_gtest(gtest_processor_motion gtest_processor_motion.cpp)
+wolf_add_gtest(gtest_processor_motion gtest_processor_motion.cpp)
# Rotation test
wolf_add_gtest(gtest_rotation gtest_rotation.cpp)
@@ -153,7 +153,7 @@ wolf_add_gtest(gtest_factor_autodiff_distance_3d gtest_factor_autodiff_distance_
wolf_add_gtest(gtest_factor_block_difference gtest_factor_block_difference.cpp)
# FactorDiffDrive class test
-# wolf_add_gtest(gtest_factor_diff_drive gtest_factor_diff_drive.cpp)
+wolf_add_gtest(gtest_factor_diff_drive gtest_factor_diff_drive.cpp)
# FactorOdom2dAutodiff class test
wolf_add_gtest(gtest_factor_odom_2d_autodiff gtest_factor_odom_2d_autodiff.cpp)
@@ -168,13 +168,13 @@ wolf_add_gtest(gtest_factor_pose_3d gtest_factor_pose_3d.cpp)
wolf_add_gtest(gtest_factor_relative_pose_2d gtest_factor_relative_pose_2d.cpp)
# FactorRelativePose2dWithExtrinsics class test
-# wolf_add_gtest(gtest_factor_relative_pose_2d_with_extrinsics gtest_factor_relative_pose_2d_with_extrinsics.cpp)
+wolf_add_gtest(gtest_factor_relative_pose_2d_with_extrinsics gtest_factor_relative_pose_2d_with_extrinsics.cpp)
# FactorRelativePose3d class test
wolf_add_gtest(gtest_factor_relative_pose_3d gtest_factor_relative_pose_3d.cpp)
# FactorRelativePose3dWithExtrinsics class test
-# wolf_add_gtest(gtest_factor_relative_pose_3d_with_extrinsics gtest_factor_relative_pose_3d_with_extrinsics.cpp)
+wolf_add_gtest(gtest_factor_relative_pose_3d_with_extrinsics gtest_factor_relative_pose_3d_with_extrinsics.cpp)
# FactorVelocityLocalDirection3d class test
wolf_add_gtest(gtest_factor_velocity_local_direction_3d gtest_factor_velocity_local_direction_3d.cpp)
@@ -186,19 +186,16 @@ wolf_add_gtest(gtest_make_posdef gtest_make_posdef.cpp)
wolf_add_gtest(gtest_map_yaml gtest_map_yaml.cpp)
# Parameter prior test
-# wolf_add_gtest(gtest_param_prior gtest_param_prior.cpp)
+wolf_add_gtest(gtest_param_prior gtest_param_prior.cpp)
# ProcessorFixedWingModel class test
-# wolf_add_gtest(gtest_processor_fixed_wing_model gtest_processor_fixed_wing_model.cpp)
+wolf_add_gtest(gtest_processor_fixed_wing_model gtest_processor_fixed_wing_model.cpp)
# ProcessorDiffDrive class test
-# wolf_add_gtest(gtest_processor_diff_drive gtest_processor_diff_drive.cpp)
+wolf_add_gtest(gtest_processor_diff_drive gtest_processor_diff_drive.cpp)
# ProcessorLoopClosure class test
-# wolf_add_gtest(gtest_processor_loop_closure gtest_processor_loop_closure.cpp)
-
-# ProcessorFrameNearestNeighborFilter class test
-# wolf_add_gtest(gtest_processor_frame_nearest_neighbor_filter_2d gtest_processor_frame_nearest_neighbor_filter_2d.cpp)
+wolf_add_gtest(gtest_processor_loop_closure gtest_processor_loop_closure.cpp)
# ProcessorMotion in 2d
wolf_add_gtest(gtest_odom_2d gtest_odom_2d.cpp)
@@ -207,7 +204,7 @@ wolf_add_gtest(gtest_odom_2d gtest_odom_2d.cpp)
wolf_add_gtest(gtest_processor_odom_3d gtest_processor_odom_3d.cpp)
# FactorPose3dWithExtrinsics class test
-# wolf_add_gtest(gtest_processor_and_factor_pose_3d_with_extrinsics gtest_processor_and_factor_pose_3d_with_extrinsics.cpp)
+wolf_add_gtest(gtest_processor_and_factor_pose_3d_with_extrinsics gtest_processor_and_factor_pose_3d_with_extrinsics.cpp)
# ProcessorTrackerFeatureDummy class test
wolf_add_gtest(gtest_processor_tracker_feature_dummy gtest_processor_tracker_feature_dummy.cpp)
@@ -228,17 +225,17 @@ wolf_add_gtest(gtest_sensor_pose gtest_sensor_pose.cpp)
IF (Ceres_FOUND)
# SolverCeres test
- # wolf_add_gtest(gtest_solver_ceres gtest_solver_ceres.cpp)
+ wolf_add_gtest(gtest_solver_ceres gtest_solver_ceres.cpp)
# SolverCeresMultithread test
- # wolf_add_gtest(gtest_solver_ceres_multithread gtest_solver_ceres_multithread.cpp)
+ wolf_add_gtest(gtest_solver_ceres_multithread gtest_solver_ceres_multithread.cpp)
ENDIF(Ceres_FOUND)
# TreeManagerSlidingWindow class test
-# wolf_add_gtest(gtest_tree_manager_sliding_window gtest_tree_manager_sliding_window.cpp)
+wolf_add_gtest(gtest_tree_manager_sliding_window gtest_tree_manager_sliding_window.cpp)
# TreeManagerSlidingWindowDualRate class test
-# wolf_add_gtest(gtest_tree_manager_sliding_window_dual_rate gtest_tree_manager_sliding_window_dual_rate.cpp)
+wolf_add_gtest(gtest_tree_manager_sliding_window_dual_rate gtest_tree_manager_sliding_window_dual_rate.cpp)
# yaml conversions
-# wolf_add_gtest(gtest_yaml_conversions gtest_yaml_conversions.cpp)
+wolf_add_gtest(gtest_yaml_conversions gtest_yaml_conversions.cpp)
diff --git a/test/dummy/processor_motion_provider_dummy.h b/test/dummy/processor_motion_provider_dummy.h
index c94013296..4e67ec2f7 100644
--- a/test/dummy/processor_motion_provider_dummy.h
+++ b/test/dummy/processor_motion_provider_dummy.h
@@ -91,7 +91,6 @@ class MotionProviderDummy : public ProcessorBase, public MotionProvider
#include "core/processor/factory_processor.h"
namespace wolf {
WOLF_REGISTER_PROCESSOR(MotionProviderDummy);
-WOLF_REGISTER_PROCESSOR_AUTO(MotionProviderDummy);
} // namespace wolf
#endif /* TEST_DUMMY_PROCESSOR_MOTION_PROVIDER_DUMMY_H_ */
diff --git a/test/dummy/processor_tracker_feature_dummy.cpp b/test/dummy/processor_tracker_feature_dummy.cpp
index ab58e3e0e..d839c4cbe 100644
--- a/test/dummy/processor_tracker_feature_dummy.cpp
+++ b/test/dummy/processor_tracker_feature_dummy.cpp
@@ -120,5 +120,4 @@ FactorBasePtr ProcessorTrackerFeatureDummy::emplaceFactor(FeatureBasePtr _featur
#include "core/processor/factory_processor.h"
namespace wolf {
WOLF_REGISTER_PROCESSOR(ProcessorTrackerFeatureDummy)
-WOLF_REGISTER_PROCESSOR_AUTO(ProcessorTrackerFeatureDummy)
} // namespace wolf
diff --git a/test/dummy/processor_tracker_landmark_dummy.cpp b/test/dummy/processor_tracker_landmark_dummy.cpp
index 54525c868..7c04d5397 100644
--- a/test/dummy/processor_tracker_landmark_dummy.cpp
+++ b/test/dummy/processor_tracker_landmark_dummy.cpp
@@ -143,6 +143,5 @@ FactorBasePtr ProcessorTrackerLandmarkDummy::emplaceFactor(FeatureBasePtr _featu
#include "core/processor/factory_processor.h"
namespace wolf {
WOLF_REGISTER_PROCESSOR(ProcessorTrackerLandmarkDummy)
-WOLF_REGISTER_PROCESSOR_AUTO(ProcessorTrackerLandmarkDummy)
} // namespace wolf
diff --git a/test/dummy/tree_manager_dummy.h b/test/dummy/tree_manager_dummy.h
index 1f115ace8..33a8f8fcf 100644
--- a/test/dummy/tree_manager_dummy.h
+++ b/test/dummy/tree_manager_dummy.h
@@ -31,8 +31,8 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsTreeManagerDummy)
struct ParamsTreeManagerDummy : public ParamsTreeManagerBase
{
ParamsTreeManagerDummy() = default;
- ParamsTreeManagerDummy(std::string _unique_name, const ParamsServer& _server):
- ParamsTreeManagerBase(_unique_name, _server)
+ ParamsTreeManagerDummy(const ParamsServer& _server):
+ ParamsTreeManagerBase(_server)
{
toy_param = _server.getParam<double>(prefix + "/toy_param");
}
@@ -44,7 +44,7 @@ struct ParamsTreeManagerDummy : public ParamsTreeManagerBase
std::string print() const override
{
return ParamsTreeManagerBase::print() + "\n"
- + "toy_param: " + std::to_string(toy_param) + "\n";
+ + "toy_param: " + toString(toy_param) + "\n";
}
};
@@ -75,7 +75,6 @@ class TreeManagerDummy : public TreeManagerBase
#include "core/tree_manager/factory_tree_manager.h"
namespace wolf {
WOLF_REGISTER_TREE_MANAGER(TreeManagerDummy)
-WOLF_REGISTER_TREE_MANAGER_AUTO(TreeManagerDummy)
} /* namespace wolf */
diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp
index c37bed03e..b8266a980 100644
--- a/test/gtest_factor_diff_drive.cpp
+++ b/test/gtest_factor_diff_drive.cpp
@@ -38,6 +38,9 @@
using namespace wolf;
using namespace Eigen;
+using namespace std;
+
+string wolf_root = _WOLF_ROOT_DIR;
WOLF_PTR_TYPEDEFS(ProcessorDiffDrivePublic);
@@ -54,31 +57,31 @@ class ProcessorDiffDrivePublic : public ProcessorDiffDrive
{
Base::configure(_sensor);
}
- void computeCurrentDelta(const Eigen::VectorXd& _data,
- const Eigen::MatrixXd& _data_cov,
- const Eigen::VectorXd& _calib,
- const double _dt,
- Eigen::VectorXd& _delta,
- Eigen::MatrixXd& _delta_cov,
- Eigen::MatrixXd& _jacobian_calib) const override
+ void computeCurrentDelta(const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _calib,
+ const double _dt,
+ VectorXd& _delta,
+ MatrixXd& _delta_cov,
+ MatrixXd& _jacobian_calib) const override
{
Base::computeCurrentDelta(_data, _data_cov, _calib, _dt, _delta, _delta_cov, _jacobian_calib);
}
- void deltaPlusDelta(const Eigen::VectorXd& _delta1,
- const Eigen::VectorXd& _delta2,
- const double _dt2,
- Eigen::VectorXd& _delta1_plus_delta2) const override
+ void deltaPlusDelta(const VectorXd& _delta1,
+ const VectorXd& _delta2,
+ const double _dt2,
+ VectorXd& _delta1_plus_delta2) const override
{
Base::deltaPlusDelta(_delta1, _delta2, _dt2, _delta1_plus_delta2);
}
- void deltaPlusDelta(const Eigen::VectorXd& _delta1,
- const Eigen::VectorXd& _delta2,
- const double _dt2,
- Eigen::VectorXd& _delta1_plus_delta2,
- Eigen::MatrixXd& _jacobian1,
- Eigen::MatrixXd& _jacobian2) const override
+ void deltaPlusDelta(const VectorXd& _delta1,
+ const VectorXd& _delta2,
+ const double _dt2,
+ VectorXd& _delta1_plus_delta2,
+ MatrixXd& _jacobian1,
+ MatrixXd& _jacobian2) const override
{
Base::deltaPlusDelta(_delta1, _delta2, _dt2, _delta1_plus_delta2, _jacobian1, _jacobian2);
}
@@ -88,19 +91,19 @@ class ProcessorDiffDrivePublic : public ProcessorDiffDrive
return Base::getCalibration(_capture);
}
- Eigen::VectorXd deltaZero() const override
+ VectorXd deltaZero() const override
{
return Base::deltaZero();
}
CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
- const SensorBasePtr& _sensor,
- const TimeStamp& _ts,
- const VectorXd& _data,
- const MatrixXd& _data_cov,
- const VectorXd& _calib,
- const VectorXd& _calib_preint,
- const CaptureBasePtr& _capture_origin) override
+ const SensorBasePtr& _sensor,
+ const TimeStamp& _ts,
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _calib,
+ const VectorXd& _calib_preint,
+ const CaptureBasePtr& _capture_origin) override
{
return Base::emplaceCapture(_frame_own, _sensor, _ts, _data, _data_cov, _calib, _calib_preint, _capture_origin);
}
@@ -128,17 +131,15 @@ class ProcessorDiffDrivePublic : public ProcessorDiffDrive
};
-
-
class FactorDiffDriveTest : public testing::Test
{
public:
ProblemPtr problem;
SolverCeresPtr solver;
TrajectoryBasePtr trajectory;
- ParamsSensorDiffDrivePtr intr;
+ ParamsSensorDiffDrivePtr params_sen;
SensorDiffDrivePtr sensor;
- ParamsProcessorDiffDrivePtr params;
+ ParamsProcessorDiffDrivePtr params_proc;
ProcessorDiffDrivePublicPtr processor;
FrameBasePtr F0, F1;
CaptureMotionPtr C0, C1;
@@ -157,36 +158,41 @@ class FactorDiffDriveTest : public testing::Test
problem = Problem::create("PO", 2);
trajectory = problem->getTrajectory();
- solver = std::make_shared<SolverCeres>(problem);
-
- // make a sensor first // DO NOT MODIFY THESE VALUES !!!
- intr = std::make_shared<ParamsSensorDiffDrive>();
- intr->radius_left = 1.0; // DO NOT MODIFY THESE VALUES !!!
- intr->radius_right = 1.0; // DO NOT MODIFY THESE VALUES !!!
- intr->wheel_separation = 1.0; // DO NOT MODIFY THESE VALUES !!!
- intr->ticks_per_wheel_revolution = 100; // DO NOT MODIFY THESE VALUES !!!
- Vector3d extr(0, 0, 0);
- auto sen = problem->installSensor("SensorDiffDrive", "sensor", extr, intr);
- sensor = std::static_pointer_cast<SensorDiffDrive>(sen);
+ solver = make_shared<SolverCeres>(problem);
+
+ // make a sensor first
+ params_sen = make_shared<ParamsSensorDiffDrive>();
+ params_sen->ticks_per_wheel_revolution = 100; // DO NOT MODIFY THESE VALUES !!!
+
+ Priors priors{{'P',Prior("P", Vector2d::Zero())}, //default "fix", not dynamic
+ {'O',Prior("O", Vector1d::Zero())}, //default "fix", not dynamic
+ {'I',Prior("StateBlock", Vector3d::Ones(), "initial_guess")}}; // not dynamic
+
+ sensor = static_pointer_cast<SensorDiffDrive>(problem->installSensor("SensorDiffDrive",
+ "sensor",
+ params_sen,
+ priors));
calib = sensor->getIntrinsic()->getState();
// params and processor
- params = std::make_shared<ParamsProcessorDiffDrive>();
- params->angle_turned = 1;
- params->dist_traveled = 2;
- params->max_buff_length = 3;
- auto prc = problem->installProcessor("ProcessorDiffDrive", "diff drive", sensor, params);
- processor = std::static_pointer_cast<ProcessorDiffDrivePublic>(prc);
+ params_proc = make_shared<ParamsProcessorDiffDrive>();
+ params_proc->angle_turned = 1;
+ params_proc->dist_traveled = 2;
+ params_proc->max_buff_length = 3;
+ processor = static_pointer_cast<ProcessorDiffDrivePublic>(problem->installProcessor("ProcessorDiffDrive",
+ "diff drive",
+ sensor,
+ params_proc));
// frames
F0 = FrameBase::emplace<FrameBase>(trajectory,
0.0,
"PO",
- std::list<VectorXd>{Vector2d(0,0), Vector1d(0)});
+ list<VectorXd>{Vector2d(0,0), Vector1d(0)});
F1 = FrameBase::emplace<FrameBase>(trajectory,
1.0,
"PO",
- std::list<VectorXd>{Vector2d(1,0), Vector1d(0)});
+ list<VectorXd>{Vector2d(1,0), Vector1d(0)});
// captures
C0 = CaptureBase::emplace<CaptureDiffDrive>(F0,
@@ -250,10 +256,10 @@ TEST_F(FactorDiffDriveTest, constructor)
ASSERT_EQ(c1_obj.getType(), "FactorDiffDrive");
// std: make_shared
- c1 = std::make_shared<FactorDiffDrive>(f1,
- C0,
- processor,
- false);
+ c1 = make_shared<FactorDiffDrive>(f1,
+ C0,
+ processor,
+ false);
ASSERT_NE(c1, nullptr);
ASSERT_EQ(c1->getType(), "FactorDiffDrive");
@@ -294,8 +300,8 @@ TEST_F(FactorDiffDriveTest, residual_right_turn_90_deg)
MatrixXd delta_cov(3,3);
double dt = 1.0;
- data1(0) = 0.50*intr->ticks_per_wheel_revolution;
- data1(1) = 0.25*intr->ticks_per_wheel_revolution;
+ data1(0) = 0.50*params_sen->ticks_per_wheel_revolution;
+ data1(1) = 0.25*params_sen->ticks_per_wheel_revolution;
processor->computeCurrentDelta(data1, data_cov, calib, dt, delta, delta_cov, J_delta_calib);
delta1 .setZero();
@@ -331,8 +337,8 @@ TEST_F(FactorDiffDriveTest, solve_F1)
MatrixXd delta_cov(3,3);
double dt = 1.0;
- data1(0) = 0.50*intr->ticks_per_wheel_revolution;
- data1(1) = 0.25*intr->ticks_per_wheel_revolution;
+ data1(0) = 0.50*params_sen->ticks_per_wheel_revolution;
+ data1(1) = 0.25*params_sen->ticks_per_wheel_revolution;
processor->computeCurrentDelta(data1, data_cov, calib, dt, delta, delta_cov, J_delta_calib);
delta1 .setZero();
@@ -359,7 +365,7 @@ TEST_F(FactorDiffDriveTest, solve_F1)
sensor->fixIntrinsics();
F1->perturb(0.1);
- std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+ string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
// WOLF_TRACE("\n", report);
problem->print(1,0,1,1);
@@ -377,8 +383,8 @@ TEST_F(FactorDiffDriveTest, solve_sensor_intrinsics)
MatrixXd delta_cov(3,3);
double dt = 1.0;
- data1(0) = 0.50*intr->ticks_per_wheel_revolution;
- data1(1) = 0.25*intr->ticks_per_wheel_revolution;
+ data1(0) = 0.50*params_sen->ticks_per_wheel_revolution;
+ data1(1) = 0.25*params_sen->ticks_per_wheel_revolution;
processor->computeCurrentDelta(data1, data_cov, calib, dt, delta, delta_cov, J_delta_calib);
delta1 .setZero();
@@ -406,7 +412,7 @@ TEST_F(FactorDiffDriveTest, solve_sensor_intrinsics)
sensor->unfixIntrinsics();
sensor->perturb(0.1);
- std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+ string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
// WOLF_TRACE("\n", report);
problem->print(1,0,1,1);
@@ -454,29 +460,32 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
*/
ProblemPtr problem = Problem::create("PO", 2);
- SolverCeresPtr solver = std::make_shared<SolverCeres>(problem);
-
- // make a sensor first // DO NOT MODIFY THESE VALUES !!!
- ParamsSensorDiffDrivePtr intr = std::make_shared<ParamsSensorDiffDrive>();
- intr->radius_left = 1.0; // DO NOT MODIFY THESE VALUES !!!
- intr->radius_right = 1.0; // DO NOT MODIFY THESE VALUES !!!
- intr->wheel_separation = 1.0; // DO NOT MODIFY THESE VALUES !!!
- intr->ticks_per_wheel_revolution = 100; // DO NOT MODIFY THESE VALUES !!!
- Vector3d extr(0, 0, 0);
- auto sen = problem->installSensor("SensorDiffDrive", "sensor", extr, intr);
- auto sensor = std::static_pointer_cast<SensorDiffDrive>(sen);
+ SolverCeresPtr solver = make_shared<SolverCeres>(problem);
+
+ // make a sensor first
+ ParamsSensorDiffDrivePtr params_sen = make_shared<ParamsSensorDiffDrive>();
+ params_sen->ticks_per_wheel_revolution = 100; // DO NOT MODIFY THESE VALUES !!!
+
+ Priors priors{{'P',Prior("P", Vector2d::Zero())}, //default "fix", not dynamic
+ {'O',Prior("O", Vector1d::Zero())}, //default "fix", not dynamic
+ {'I',Prior("StateBlock", Vector3d::Ones(), "initial_guess")}}; // not dynamic
+
+ auto sensor = static_pointer_cast<SensorDiffDrive>(problem->installSensor("SensorDiffDrive",
+ "sensor",
+ params_sen,
+ priors));
auto calib_preint = sensor->getIntrinsic()->getState();
Vector3d calib_gt(1,1,1);
// params and processor
- ParamsProcessorDiffDrivePtr params = std::make_shared<ParamsProcessorDiffDrive>();
- params->angle_turned = 99;
- params->dist_traveled = 99;
- params->max_buff_length = 99;
- params->voting_active = true;
- params->max_time_span = 1.5;
- auto prc = problem->installProcessor("ProcessorDiffDrive", "diff drive", sensor, params);
- auto processor = std::static_pointer_cast<ProcessorDiffDrivePublic>(prc);
+ ParamsProcessorDiffDrivePtr params_proc = make_shared<ParamsProcessorDiffDrive>();
+ params_proc->angle_turned = 99;
+ params_proc->dist_traveled = 99;
+ params_proc->max_buff_length = 99;
+ params_proc->voting_active = true;
+ params_proc->max_time_span = 1.5;
+ auto prc = problem->installProcessor("ProcessorDiffDrive", "diff drive", sensor, params_proc);
+ auto processor = static_pointer_cast<ProcessorDiffDrivePublic>(prc);
TimeStamp t(0.0);
double dt = 1.0;
@@ -495,10 +504,10 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
Vector3d data;
Matrix2d data_cov; data_cov.setIdentity();
- data(0) = 0.50*intr->ticks_per_wheel_revolution / N;
- data(1) = 0.25*intr->ticks_per_wheel_revolution / N;
+ data(0) = 0.50*params_sen->ticks_per_wheel_revolution / N;
+ data(1) = 0.25*params_sen->ticks_per_wheel_revolution / N;
- auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, nullptr);
+ auto C = make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, nullptr);
for (int n = 0; n < N; n++)
{
@@ -508,8 +517,8 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
}
// left turn 90 deg in N steps --> ends up in (3, -3, 0)
- data(0) = 0.25*intr->ticks_per_wheel_revolution / N;
- data(1) = 0.50*intr->ticks_per_wheel_revolution / N;
+ data(0) = 0.25*params_sen->ticks_per_wheel_revolution / N;
+ data(1) = 0.50*params_sen->ticks_per_wheel_revolution / N;
C->setData(data);
for (int n = 0; n < N; n++)
{
@@ -529,7 +538,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
Vector3d calib_pert = sensor->getIntrinsic()->getState();
WOLF_TRACE("\n ========== SOLVE =========");
- std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+ string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
WOLF_TRACE("\n", report);
WOLF_TRACE("x1 : ", problem->getState(N*dt).vector("PO").transpose());
@@ -543,7 +552,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
ASSERT_MATRIX_APPROX(problem->getState( N*dt).vector("PO") , x1 , 1e-6 );
ASSERT_MATRIX_APPROX(problem->getState(2*N*dt).vector("PO") , x2 , 1e-6 );
- std::cout << "\n\n" << std::endl;
+ cout << "\n\n" << endl;
}
@@ -587,29 +596,32 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
ProblemPtr problem = Problem::create("PO", 2);
- SolverCeresPtr solver = std::make_shared<SolverCeres>(problem);
-
- // make a sensor first // DO NOT MODIFY THESE VALUES !!!
- ParamsSensorDiffDrivePtr intr = std::make_shared<ParamsSensorDiffDrive>();
- intr->radius_left = 0.95;
- intr->radius_right = 0.98;
- intr->wheel_separation = 1.05;
- intr->ticks_per_wheel_revolution = 100; // DO NOT MODIFY THESE VALUES !!!
- Vector3d extr(0, 0, 0);
- auto sen = problem->installSensor("SensorDiffDrive", "sensor", extr, intr);
- auto sensor = std::static_pointer_cast<SensorDiffDrive>(sen);
+ SolverCeresPtr solver = make_shared<SolverCeres>(problem);
+
+ // make a sensor first
+ ParamsSensorDiffDrivePtr params_sen = make_shared<ParamsSensorDiffDrive>();
+ params_sen->ticks_per_wheel_revolution = 100; // DO NOT MODIFY THESE VALUES !!!
+
+ Priors priors{{'P',Prior("P", Vector2d::Zero())}, //default "fix", not dynamic
+ {'O',Prior("O", Vector1d::Zero())}, //default "fix", not dynamic
+ {'I',Prior("StateBlock", Vector3d::Ones(), "initial_guess")}}; // not dynamic
+
+ auto sensor = static_pointer_cast<SensorDiffDrive>(problem->installSensor("SensorDiffDrive",
+ "sensor",
+ params_sen,
+ priors));
auto calib_preint = sensor->getIntrinsic()->getState();
- Vector3d calib_gt(1.0, 1.0, 1.0); // ground truth calib
+ Vector3d calib_gt(1,1,1);
// params and processor
- ParamsProcessorDiffDrivePtr params = std::make_shared<ParamsProcessorDiffDrive>();
+ ParamsProcessorDiffDrivePtr params = make_shared<ParamsProcessorDiffDrive>();
params->angle_turned = 99;
params->dist_traveled = 99;
params->max_buff_length = 99;
params->voting_active = true;
params->max_time_span = 1.5;
auto prc = problem->installProcessor("ProcessorDiffDrive", "diff drive", sensor, params);
- auto processor = std::static_pointer_cast<ProcessorDiffDrivePublic>(prc);
+ auto processor = static_pointer_cast<ProcessorDiffDrivePublic>(prc);
TimeStamp t(0.0);
double dt = 1.0;
@@ -629,10 +641,10 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
Vector2d data;
Matrix2d data_cov; data_cov.setIdentity();
- data(0) = 0.50*intr->ticks_per_wheel_revolution / N;
- data(1) = 0.25*intr->ticks_per_wheel_revolution / N;
+ data(0) = 0.50*params_sen->ticks_per_wheel_revolution / N;
+ data(1) = 0.25*params_sen->ticks_per_wheel_revolution / N;
- auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, nullptr);
+ auto C = make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, nullptr);
for (int n = 0; n < N; n++)
{
@@ -642,8 +654,8 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
}
// left turn 90 deg in N steps --> ends up in (3, -3, 0)
- data(0) = 0.25*intr->ticks_per_wheel_revolution / N;
- data(1) = 0.50*intr->ticks_per_wheel_revolution / N;
+ data(0) = 0.25*params_sen->ticks_per_wheel_revolution / N;
+ data(1) = 0.50*params_sen->ticks_per_wheel_revolution / N;
C->setData(data);
@@ -664,7 +676,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
F2->fix();
WOLF_TRACE("\n ========== SOLVE =========");
- std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+ string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
WOLF_TRACE("\n", report);
WOLF_TRACE("x1 : ", problem->getState(N*dt).vector("PO").transpose());
diff --git a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
index ee2bc076d..ca8aba7d3 100644
--- a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
+++ b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
@@ -24,7 +24,7 @@
#include "core/ceres_wrapper/solver_ceres.h"
#include "core/factor/factor_relative_pose_2d_with_extrinsics.h"
#include "core/capture/capture_odom_2d.h"
-#include "core/sensor/sensor_odom_2d.h"
+#include "core/sensor/sensor_odom.h"
#include "core/processor/processor_odom_2d.h"
#include "core/math/rotations.h"
@@ -44,7 +44,7 @@ ProblemPtr problem_ptr = Problem::create("PO", 2);
SolverCeres solver(problem_ptr);
// Sensor
-auto sensor_odom2d = problem_ptr->installSensor("SensorOdom2d", "odom", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2d.yaml");
+auto sensor_odom2d = problem_ptr->installSensor("SensorOdom", "odom", wolf_root + "/test/yaml/sensor_odom_2d.yaml");
auto processor_odom2d = problem_ptr->installProcessor("ProcessorOdom2d", "odom", sensor_odom2d, std::make_shared<ParamsProcessorOdom2d>());
// Two frames
diff --git a/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
index 0f89a1100..ee0ff88d7 100644
--- a/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
+++ b/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
@@ -29,14 +29,13 @@
#include "core/ceres_wrapper/solver_ceres.h"
#include "core/capture/capture_pose.h"
#include "core/feature/feature_pose.h"
-
-
+#include "core/sensor/sensor_pose.h"
using namespace Eigen;
using namespace wolf;
using std::static_pointer_cast;
-
+string wolf_root = _WOLF_ROOT_DIR;
// Use the following in case you want to initialize tests with predefines variables or methods.
class FactorRelativePose3dWithExtrinsics_class : public testing::Test{
@@ -115,17 +114,12 @@ class FactorRelativePose3dWithExtrinsics_class : public testing::Test{
problem = Problem::create("PO", 3);
solver = std::make_shared<SolverCeres>(problem);
- // Create sensor to be able to initialize (a camera for instance)
- S = SensorBase::emplace<SensorBase>(problem->getHardware(), "SensorPose",
- std::make_shared<StateBlock>(pos_camera, true),
- std::make_shared<StateQuaternion>(vquat_camera, true),
- std::make_shared<StateBlock>(Vector4d::Zero(), false), // fixed
- Vector1d::Zero());
-
- // ParamsSensorCameraPtr params_camera = std::make_shared<ParamsSensorCamera>();
- // S = problem->installSensor("SensorCamera", "canonical", pose_camera, params_camera);
- // camera = std::static_pointer_cast<SensorCamera>(S);
-
+ // Create sensor to be able to initialize
+ S = problem->installSensor("SensorPose",
+ "sensor_pose_1",
+ std::make_shared<ParamsSensorPose>(),
+ Priors{{'P',Prior("P",pos_camera)},
+ {'O',Prior("O",vquat_camera)}});
// F1 is be origin KF
VectorComposite x0(pose_robot, "PO", {3,4});
diff --git a/test/gtest_factory.cpp b/test/gtest_factory.cpp
index f46ee7506..6527cfd9b 100644
--- a/test/gtest_factory.cpp
+++ b/test/gtest_factory.cpp
@@ -32,7 +32,7 @@ std::string wolf_root = _WOLF_ROOT_DIR;
TEST(TestFactory, DummyObjectFactory)
{
- ParserYaml parser = ParserYaml("test/yaml/params_basic.yaml", wolf_root);
+ ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/params_basic.yaml");
ParamsServer server = ParamsServer(parser.getParams());
auto object = FactoryDummyObject::create("DummyObjectDerived","ACoolDummyObject", server);
diff --git a/test/gtest_param_prior.cpp b/test/gtest_param_prior.cpp
index 194671976..a6b2177e1 100644
--- a/test/gtest_param_prior.cpp
+++ b/test/gtest_param_prior.cpp
@@ -29,70 +29,84 @@
#include "core/utils/utils_gtest.h"
#include "core/problem/problem.h"
-#include "core/sensor/sensor_odom_3d.h"
+#include "core/sensor/sensor_odom.h"
#include "core/ceres_wrapper/solver_ceres.h"
#include <iostream>
using namespace wolf;
+using namespace Eigen;
ProblemPtr problem_ptr = Problem::create("PO", 3);
SolverCeresPtr solver_ceres = std::make_shared<SolverCeres>(problem_ptr);
-Eigen::Vector7d initial_extrinsics((Eigen::Vector7d() << 1, 2, 3, 1, 0, 0, 0).finished());
-Eigen::Vector7d prior_extrinsics((Eigen::Vector7d() << 10, 20, 30, 0, 0, 0, 1).finished());
-Eigen::Vector7d prior2_extrinsics((Eigen::Vector7d() << 100, 200, 300, 0, 0, 0, 1).finished());
-
-SensorOdom3dPtr odom_sensor_ptr_ = std::static_pointer_cast<SensorOdom3d>(problem_ptr->installSensor("SensorOdom3d", "odometer", initial_extrinsics, std::make_shared<ParamsSensorOdom3d>()));
-
-TEST(ParameterPrior, initial_extrinsics)
+Vector3d initial_extrinsics_p((Vector3d() << 1, 2, 3).finished());
+Vector4d initial_extrinsics_o((Vector4d() << 1, 0, 0, 0).finished());
+Vector7d initial_extrinsics((Vector7d() << initial_extrinsics_p, initial_extrinsics_o).finished());
+Vector7d prior_extrinsics((Vector7d() << 10, 20, 30, 0, 0, 0, 1).finished());
+Vector7d prior2_extrinsics((Vector7d() << 100, 200, 300, 0, 0, 0, 1).finished());
+
+ParamsSensorOdomPtr params = std::make_shared<ParamsSensorOdom>();
+Priors priors{{'P',Prior("P",initial_extrinsics_p,"initial_guess")},
+ {'O',Prior("O",initial_extrinsics_o,"initial_guess")}};
+SensorBasePtr sensor_ptr_ = problem_ptr->installSensor("SensorOdom",
+ "odometer",
+ params,
+ priors);
+
+TEST(ParameterPrior, add_prior_p)
{
ASSERT_TRUE(problem_ptr->check(0));
- ASSERT_TRUE(odom_sensor_ptr_->getP());
- ASSERT_TRUE(odom_sensor_ptr_->getO());
- ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getP()->getState(),initial_extrinsics.head(3),1e-9);
- ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getO()->getState(),initial_extrinsics.tail(4),1e-9);
-}
+ ASSERT_TRUE(sensor_ptr_->getP());
+ ASSERT_MATRIX_APPROX(sensor_ptr_->getP()->getState(),initial_extrinsics_p,Constants::EPS);
-TEST(ParameterPrior, prior_p)
-{
- odom_sensor_ptr_->addPriorP(prior_extrinsics.head(3),Eigen::Matrix3d::Identity());
+ sensor_ptr_->addPriorP(prior_extrinsics.head(3),Matrix3d::Identity());
std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::BRIEF);
- ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getP()->getState(),prior_extrinsics.head(3),1e-6);
+ ASSERT_MATRIX_APPROX(sensor_ptr_->getP()->getState(),prior_extrinsics.head(3),1e-6);
}
-TEST(ParameterPrior, prior_o)
+TEST(ParameterPrior, add_prior_o)
{
- odom_sensor_ptr_->addPriorO(prior_extrinsics.tail(4),Eigen::Matrix3d::Identity());
+ ASSERT_TRUE(problem_ptr->check(0));
+ ASSERT_TRUE(sensor_ptr_->getO());
+ ASSERT_MATRIX_APPROX(sensor_ptr_->getO()->getState(),initial_extrinsics_o,Constants::EPS);
+
+ sensor_ptr_->addPriorO(prior_extrinsics.tail(4),Matrix3d::Identity());
std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::BRIEF);
- ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getO()->getState(),prior_extrinsics.tail(4),1e-6);
+ ASSERT_MATRIX_APPROX(sensor_ptr_->getO()->getState(),prior_extrinsics.tail(4),1e-6);
}
TEST(ParameterPrior, prior_p_overwrite)
{
- odom_sensor_ptr_->addPriorP(prior2_extrinsics.head(3),Eigen::Matrix3d::Identity());
+ ASSERT_TRUE(sensor_ptr_->getPriorFeature('P'));
+ ASSERT_FALSE(sensor_ptr_->getPriorFeatures().empty());
+
+ sensor_ptr_->addPriorP(prior2_extrinsics.head(3),Matrix3d::Identity());
+
+ ASSERT_TRUE(sensor_ptr_->getPriorFeature('P'));
+ ASSERT_FALSE(sensor_ptr_->getPriorFeatures().empty());
std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::BRIEF);
- ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getP()->getState(),prior2_extrinsics.head(3),1e-6);
+ ASSERT_MATRIX_APPROX(sensor_ptr_->getP()->getState(),prior2_extrinsics.head(3),1e-6);
}
TEST(ParameterPrior, prior_p_segment)
{
- odom_sensor_ptr_->addPriorP(prior_extrinsics.segment(1,2),Eigen::Matrix2d::Identity(),1,2);
+ sensor_ptr_->addPriorP(prior_extrinsics.segment(1,2),Matrix2d::Identity(),1);
std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::BRIEF);
- ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getP()->getState().segment(1,2),prior_extrinsics.segment(1,2),1e-6);
+ ASSERT_MATRIX_APPROX(sensor_ptr_->getP()->getState().segment(1,2),prior_extrinsics.segment(1,2),1e-6);
}
TEST(ParameterPrior, prior_o_segment)
{
// constraining segment of quaternion is not allowed
- ASSERT_DEATH(odom_sensor_ptr_->addPriorParameter('O', prior_extrinsics.segment(1,2),Eigen::Matrix2d::Identity(),1,2),"");
+ ASSERT_THROW(sensor_ptr_->addPriorParameter('O', prior_extrinsics.segment(1,2),Matrix2d::Identity(),1),std::runtime_error);
}
diff --git a/test/gtest_param_server.cpp b/test/gtest_param_server.cpp
index e8116c8ff..5cc0ca768 100644
--- a/test/gtest_param_server.cpp
+++ b/test/gtest_param_server.cpp
@@ -32,13 +32,13 @@ std::string wolf_root = _WOLF_ROOT_DIR;
TEST(ParamsServer, Default)
{
- ParserYaml parser = ParserYaml("test/yaml/params_basic.yaml", wolf_root);
+ ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/params_basic.yaml");
ParamsServer server = ParamsServer(parser.getParams());
}
TEST(ParamsServer, getParamsOk)
{
- ParserYaml parser = ParserYaml("test/yaml/params_basic.yaml", wolf_root);
+ ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/params_basic.yaml");
ParamsServer server = ParamsServer(parser.getParams());
EXPECT_EQ(server.getParam<int>("int_1"), -3);
@@ -68,7 +68,7 @@ TEST(ParamsServer, getParamsOk)
TEST(ParamsServer, getParamsWrong)
{
- ParserYaml parser = ParserYaml("test/yaml/params_basic.yaml", wolf_root);
+ ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/params_basic.yaml");
ParamsServer server = ParamsServer(parser.getParams());
EXPECT_ANY_THROW({ server.getParam<double>("should_not_exist"); });
diff --git a/test/gtest_parser_yaml.cpp b/test/gtest_parser_yaml.cpp
index bb37da59d..1d97c2a13 100644
--- a/test/gtest_parser_yaml.cpp
+++ b/test/gtest_parser_yaml.cpp
@@ -33,7 +33,7 @@ std::string processor_prefix = "processor/";
TEST(ParserYaml, RegularParse)
{
- auto parser = ParserYaml("test/yaml/params1.yaml", wolf_root);
+ auto parser = ParserYaml(wolf_root + "/test/yaml/params1.yaml");
auto params = parser.getParams();
// for(auto it : params)
// cout << it.first << " %% " << it.second << endl;
@@ -42,7 +42,7 @@ TEST(ParserYaml, RegularParse)
}
TEST(ParserYaml, ParseMap)
{
- auto parser = ParserYaml("test/yaml/params2.yaml", wolf_root);
+ auto parser = ParserYaml(wolf_root + "/test/yaml/params2.yaml");
auto params = parser.getParams();
// for(auto it : params)
// cout << it.first << " %% " << it.second << endl;
@@ -51,28 +51,28 @@ TEST(ParserYaml, ParseMap)
}
TEST(ParserYaml, FollowFile)
{
- auto parser = ParserYaml("test/yaml/params1.yaml", wolf_root);
+ auto parser = ParserYaml(wolf_root + "/test/yaml/params1.yaml");
auto params = parser.getParams();
EXPECT_EQ(params[processor_prefix + "my_proc_test/max_buff_length"], "100");
EXPECT_EQ(params[processor_prefix + "my_proc_test/voting_active"], "false");
}
TEST(ParserYaml, FollowOdom3d)
{
- auto parser = ParserYaml("test/yaml/params1.yaml", wolf_root);
+ auto parser = ParserYaml(wolf_root + "/test/yaml/params1.yaml");
auto params = parser.getParams();
EXPECT_EQ(params[processor_prefix + "my_proc_odom3d/keyframe_vote/max_buff_length"], "10");
EXPECT_EQ(params[processor_prefix + "my_proc_odom3d/keyframe_vote/max_time_span"], "0.2");
}
TEST(ParserYaml, JumpFile)
{
- auto parser = ParserYaml("test/yaml/params3.yaml", wolf_root);
+ auto parser = ParserYaml(wolf_root + "/test/yaml/params3.yaml");
auto params = parser.getParams();
EXPECT_EQ(params[processor_prefix + "my_proc_test/extern_params/max_buff_length"], "100");
EXPECT_EQ(params[processor_prefix + "my_proc_test/extern_params/voting_active"], "false");
}
TEST(ParserYaml, ProblemConfig)
{
- auto parser = ParserYaml("test/yaml/params2.yaml", wolf_root);
+ auto parser = ParserYaml(wolf_root + "/test/yaml/params2.yaml");
auto params = parser.getParams();
EXPECT_EQ(params["problem/frame_structure"], "POV");
EXPECT_EQ(params["problem/dimension"], "2");
diff --git a/test/gtest_prior.cpp b/test/gtest_prior.cpp
index 1985c80b6..fc2ed8b46 100644
--- a/test/gtest_prior.cpp
+++ b/test/gtest_prior.cpp
@@ -338,7 +338,7 @@ TEST(Prior, StateQuaternion)
TEST(Prior, ParamsServer)
{
- ParserYaml parser = ParserYaml("test/yaml/params_prior.yaml", wolf_root, true);
+ ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/params_prior.yaml", true);
ParamsServer server = ParamsServer(parser.getParams());
std::vector<std::string> keys({"P","O"});
@@ -363,7 +363,7 @@ TEST(Prior, ParamsServer)
if (not dynamic and drift)
continue;
- std::string prefix = key + "_" + to_string(dim) + "D_" + mode + (dynamic ? "_dynamic" : "") + (drift ? "_drift" : "");
+ std::string prefix = key + "_" + toString(dim) + "D_" + mode + (dynamic ? "_dynamic" : "") + (drift ? "_drift" : "");
WOLF_INFO("Creating prior from prefix ", prefix);
auto P = Prior(prefix, key, server);
@@ -420,7 +420,7 @@ TEST(Prior, ParamsServer)
TEST(Prior, ParamsServerWrong)
{
- ParserYaml parser = ParserYaml("test/yaml/params_prior_wrong.yaml", wolf_root, true);
+ ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/params_prior_wrong.yaml", true);
ParamsServer server = ParamsServer(parser.getParams());
std::vector<std::string> keys({"P","O"});
@@ -440,7 +440,7 @@ TEST(Prior, ParamsServerWrong)
if (not dynamic and drift)
continue;
- std::string prefix = key + "_" + to_string(dim) + "D_" + mode + (dynamic ? "_dynamic" : "") + (drift ? "_drift" : "");
+ std::string prefix = key + "_" + toString(dim) + "D_" + mode + (dynamic ? "_dynamic" : "") + (drift ? "_drift" : "");
WOLF_INFO("Creating prior from prefix ", prefix);
ASSERT_THROW(auto P = Prior(prefix, key, server),std::runtime_error);
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 26d939c13..c4b845ed6 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -526,7 +526,7 @@ TEST(Problem, autoSetupMap)
{
std::string wolf_root = _WOLF_ROOT_DIR;
- auto parser = ParserYaml("test/yaml/params_problem_autosetup.yaml", wolf_root);
+ auto parser = ParserYaml(wolf_root + "/test/yaml/params_problem_autosetup.yaml");
auto server = ParamsServer(parser.getParams());
auto P = Problem::autoSetup(server);
@@ -538,7 +538,7 @@ TEST(Problem, autoSetupNoMap)
{
std::string wolf_root = _WOLF_ROOT_DIR;
- auto parser = ParserYaml("test/yaml/params_problem_autosetup_no_map.yaml", wolf_root);
+ auto parser = ParserYaml(wolf_root + "/test/yaml/params_problem_autosetup_no_map.yaml");
auto server = ParamsServer(parser.getParams());
auto P = Problem::autoSetup(server);
@@ -550,7 +550,7 @@ TEST(Problem, getState)
{
std::string wolf_root = _WOLF_ROOT_DIR;
- auto parser = ParserYaml("test/yaml/params_problem_autosetup.yaml", wolf_root);
+ auto parser = ParserYaml(wolf_root + "/test/yaml/params_problem_autosetup.yaml");
auto server = ParamsServer(parser.getParams());
auto P = Problem::autoSetup(server);
diff --git a/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp b/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp
index 7e1e8624e..b957ef47a 100644
--- a/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp
+++ b/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp
@@ -126,11 +126,12 @@ class FactorPose3dWithExtrinsicsBase_Test : public testing::Test
solver_ = std::make_shared<SolverCeres>(problem_);
// pose sensor and proc (to get extrinsics in the prb)
- auto intr_sp = std::make_shared<ParamsSensorPose>();
- intr_sp->std_p = 1;
- intr_sp->std_o = 1;
- Vector7d extr; extr << b_p_bm_, b_q_m_.coeffs();
- sensor_pose_ = problem_->installSensor("SensorPose", "pose", extr, intr_sp);
+ auto params_sp = std::make_shared<ParamsSensorPose>();
+ params_sp->std_p = 1;
+ params_sp->std_o = 1;
+
+ sensor_pose_ = problem_->installSensor("SensorPose", "pose", params_sp, Priors{{'P',Prior("P",b_p_bm_)},
+ {'O',Prior("O",b_q_m_.coeffs())}});
auto params_proc = std::make_shared<ParamsProcessorPose>();
params_proc->time_tolerance = 0.5;
auto proc_pose = problem_->installProcessor("ProcessorPose", "pose", sensor_pose_, params_proc);
@@ -155,7 +156,7 @@ class FactorPose3dWithExtrinsicsMANUAL_Test : public FactorPose3dWithExtrinsicsB
///////////////////
// Create factor graph
- Matrix6d cov6d = sensor_pose_->getNoiseCov();
+ Matrix6d cov6d = sensor_pose_->computeNoiseCov(VectorXd(0));
// Odometry capture, feature and factor
auto cap_odom1 = CaptureBase::emplace<CapturePose>(KF2_, 2, nullptr, pose_12_, cov6d);
auto fea_odom1 = FeatureBase::emplace<FeatureBase>(cap_odom1, "odom", pose_12_, cov6d);
@@ -202,7 +203,7 @@ class FactorPose3dWithExtrinsics_ProcessorWithKeyFrameCallbackFirst_Test : publi
///////////////////
// Create factor graph
- Matrix6d cov6d = sensor_pose_->getNoiseCov();
+ Matrix6d cov6d = sensor_pose_->computeNoiseCov(VectorXd(0));
// Odometry capture, feature and factor
auto cap_odom1 = CaptureBase::emplace<CapturePose>(KF2_, 2, nullptr, pose_12_, cov6d);
auto fea_odom1 = FeatureBase::emplace<FeatureBase>(cap_odom1, "odom", pose_12_, cov6d);
@@ -238,7 +239,7 @@ class FactorPose3dWithExtrinsics_ProcessorWithProcessFirst_Test : public FactorP
KF2_ = problem_->emplaceFrame(2, pose2_);
KF3_ = problem_->emplaceFrame(3, pose3_);
- Matrix6d cov6d = sensor_pose_->getNoiseCov();
+ Matrix6d cov6d = sensor_pose_->computeNoiseCov(VectorXd(0));
///////////////////
// Create factor graph
// Odometry capture, feature and factor
diff --git a/test/gtest_processor_diff_drive.cpp b/test/gtest_processor_diff_drive.cpp
index 0fcf73467..11a0663c1 100644
--- a/test/gtest_processor_diff_drive.cpp
+++ b/test/gtest_processor_diff_drive.cpp
@@ -51,38 +51,38 @@ class ProcessorDiffDrivePublic : public ProcessorDiffDrive
Base::configure(_sensor);
}
void computeCurrentDelta(const Eigen::VectorXd& _data,
- const Eigen::MatrixXd& _data_cov,
- const Eigen::VectorXd& _calib,
- const double _dt,
- Eigen::VectorXd& _delta,
- Eigen::MatrixXd& _delta_cov,
- Eigen::MatrixXd& _jacobian_calib) const override
+ const Eigen::MatrixXd& _data_cov,
+ const Eigen::VectorXd& _calib,
+ const double _dt,
+ Eigen::VectorXd& _delta,
+ Eigen::MatrixXd& _delta_cov,
+ Eigen::MatrixXd& _jacobian_calib) const override
{
Base::computeCurrentDelta(_data, _data_cov, _calib, _dt, _delta, _delta_cov, _jacobian_calib);
}
void deltaPlusDelta(const Eigen::VectorXd& _delta1,
- const Eigen::VectorXd& _delta2,
- const double _dt2,
- Eigen::VectorXd& _delta1_plus_delta2) const override
+ const Eigen::VectorXd& _delta2,
+ const double _dt2,
+ Eigen::VectorXd& _delta1_plus_delta2) const override
{
Base::deltaPlusDelta(_delta1, _delta2, _dt2, _delta1_plus_delta2);
}
void deltaPlusDelta(const Eigen::VectorXd& _delta1,
- const Eigen::VectorXd& _delta2,
- const double _dt2,
- Eigen::VectorXd& _delta1_plus_delta2,
- Eigen::MatrixXd& _jacobian1,
- Eigen::MatrixXd& _jacobian2) const override
+ const Eigen::VectorXd& _delta2,
+ const double _dt2,
+ Eigen::VectorXd& _delta1_plus_delta2,
+ Eigen::MatrixXd& _jacobian1,
+ Eigen::MatrixXd& _jacobian2) const override
{
Base::deltaPlusDelta(_delta1, _delta2, _dt2, _delta1_plus_delta2, _jacobian1, _jacobian2);
}
void statePlusDelta(const VectorComposite& _x,
- const Eigen::VectorXd& _delta,
- const double _dt,
- VectorComposite& _x_plus_delta) const override
+ const Eigen::VectorXd& _delta,
+ const double _dt,
+ VectorComposite& _x_plus_delta) const override
{
Base::statePlusDelta(_x, _delta, _dt, _x_plus_delta);
}
@@ -93,13 +93,13 @@ class ProcessorDiffDrivePublic : public ProcessorDiffDrive
}
CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
- const SensorBasePtr& _sensor,
- const TimeStamp& _ts,
- const VectorXd& _data,
- const MatrixXd& _data_cov,
- const VectorXd& _calib,
- const VectorXd& _calib_preint,
- const CaptureBasePtr& _capture_origin) override
+ const SensorBasePtr& _sensor,
+ const TimeStamp& _ts,
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _calib,
+ const VectorXd& _calib_preint,
+ const CaptureBasePtr& _capture_origin) override
{
return Base::emplaceCapture(_frame_own, _sensor, _ts, _data, _data_cov, _calib, _calib_preint, _capture_origin);
}
@@ -130,7 +130,7 @@ class ProcessorDiffDrivePublic : public ProcessorDiffDrive
class ProcessorDiffDriveTest : public testing::Test
{
public:
- ParamsSensorDiffDrivePtr intr;
+ ParamsSensorDiffDrivePtr params_sen;
SensorDiffDrivePtr sensor;
ParamsProcessorDiffDrivePtr params;
ProcessorDiffDrivePublicPtr processor;
@@ -140,15 +140,18 @@ class ProcessorDiffDriveTest : public testing::Test
{
problem = Problem::create("PO", 2);
- // make a sensor first // DO NOT MODIFY THESE VALUES !!!
- intr = std::make_shared<ParamsSensorDiffDrive>();
- intr->radius_left = 1.0; // DO NOT MODIFY THESE VALUES !!!
- intr->radius_right = 1.0; // DO NOT MODIFY THESE VALUES !!!
- intr->wheel_separation = 1.0; // DO NOT MODIFY THESE VALUES !!!
- intr->ticks_per_wheel_revolution = 100; // DO NOT MODIFY THESE VALUES !!!
- Vector3d extr(0,0,0);
- sensor = std::static_pointer_cast<SensorDiffDrive>(problem->installSensor("SensorDiffDrive", "sensor diff drive", extr, intr));
-
+ // make a sensor first
+ params_sen = std::make_shared<ParamsSensorDiffDrive>();
+ params_sen->ticks_per_wheel_revolution = 100; // DO NOT MODIFY THESE VALUES !!!
+
+ Priors priors{{'P',Prior("P", Vector2d::Zero())}, //default "fix", not dynamic
+ {'O',Prior("O", Vector1d::Zero())}, //default "fix", not dynamic
+ {'I',Prior("StateBlock", Vector3d::Ones(), "initial_guess")}}; // not dynamic DO NOT MODIFY THESE VALUES !!!
+
+ sensor = std::static_pointer_cast<SensorDiffDrive>(problem->installSensor("SensorDiffDrive",
+ "sensor",
+ params_sen,
+ priors)); // DO NOT MODIFY THESE VALUES !!!
// params and processor
params = std::make_shared<ParamsProcessorDiffDrive>();
params->voting_active = true;
@@ -179,11 +182,6 @@ TEST(ProcessorDiffDrive, constructor)
TEST(ProcessorDiffDrive, create)
{
- // make a sensor first
- auto intr = std::make_shared<ParamsSensorDiffDrive>();
- Vector3d extr(1,2,3);
- auto sen = std::make_shared<SensorDiffDrive>(extr, intr);
-
// params
auto params = std::make_shared<ParamsProcessorDiffDrive>();
@@ -229,15 +227,15 @@ TEST_F(ProcessorDiffDriveTest, computeCurrentDelta)
ASSERT_MATRIX_APPROX(delta, Vector3d::Zero(), 1e-8);
// 2. left turn 90 deg --> ends up in (1.5, 1.5, pi/2)
- data(0) = 0.25*intr->ticks_per_wheel_revolution;
- data(1) = 0.50*intr->ticks_per_wheel_revolution;
+ data(0) = 0.25*params_sen->ticks_per_wheel_revolution;
+ data(1) = 0.50*params_sen->ticks_per_wheel_revolution;
processor->computeCurrentDelta(data, data_cov, calib, dt, delta, delta_cov, J_delta_calib);
ASSERT_MATRIX_APPROX(delta, Vector3d(1.5, 1.5, M_PI_2), 1e-6);
// 2. right turn 90 deg --> ends up in (1.5, -1.5, -pi/2)
- data(0) = 0.50*intr->ticks_per_wheel_revolution;
- data(1) = 0.25*intr->ticks_per_wheel_revolution;
+ data(0) = 0.50*params_sen->ticks_per_wheel_revolution;
+ data(1) = 0.25*params_sen->ticks_per_wheel_revolution;
processor->computeCurrentDelta(data, data_cov, calib, dt, delta, delta_cov, J_delta_calib);
ASSERT_MATRIX_APPROX(delta, Vector3d(1.5, -1.5, -M_PI_2), 1e-6);
@@ -253,8 +251,8 @@ TEST_F(ProcessorDiffDriveTest, deltaPlusDelta)
MatrixXd delta_cov(3,3), J_delta_calib(3,3);
// 1. left turn 90 deg in 3 steps of 30 deg --> ends up in (1.5, 1.5, pi/2)
- data(0) = 0.25*intr->ticks_per_wheel_revolution / 3;
- data(1) = 0.50*intr->ticks_per_wheel_revolution / 3;
+ data(0) = 0.25*params_sen->ticks_per_wheel_revolution / 3;
+ data(1) = 0.50*params_sen->ticks_per_wheel_revolution / 3;
processor->computeCurrentDelta(data, data_cov, calib, dt, delta, delta_cov, J_delta_calib);
delta1 .setZero();
@@ -267,8 +265,8 @@ TEST_F(ProcessorDiffDriveTest, deltaPlusDelta)
ASSERT_MATRIX_APPROX(delta2, Vector3d(1.5, 1.5, M_PI_2), 1e-6);
// 2. right turn 90 deg in 4 steps of 22.5 deg --> ends up in (1.5, -1.5, -pi/2)
- data(0) = 0.50*intr->ticks_per_wheel_revolution / 4;
- data(1) = 0.25*intr->ticks_per_wheel_revolution / 4;
+ data(0) = 0.50*params_sen->ticks_per_wheel_revolution / 4;
+ data(1) = 0.25*params_sen->ticks_per_wheel_revolution / 4;
processor->computeCurrentDelta(data, data_cov, calib, dt, delta, delta_cov, J_delta_calib);
delta1 .setZero();
@@ -294,8 +292,8 @@ TEST_F(ProcessorDiffDriveTest, statePlusDelta)
MatrixXd delta_cov(3,3), J_delta_calib(3,3);
// 1. left turn 90 deg in 3 steps of 30 deg --> ends up in (1.5, 1.5, pi/2)
- data(0) = 0.25*intr->ticks_per_wheel_revolution / 3;
- data(1) = 0.50*intr->ticks_per_wheel_revolution / 3;
+ data(0) = 0.25*params_sen->ticks_per_wheel_revolution / 3;
+ data(1) = 0.50*params_sen->ticks_per_wheel_revolution / 3;
processor->computeCurrentDelta(data, data_cov, calib, dt, delta, delta_cov, J_delta_calib);
x1 .setZero();
@@ -308,8 +306,8 @@ TEST_F(ProcessorDiffDriveTest, statePlusDelta)
ASSERT_MATRIX_APPROX(x2.vector("PO"), Vector3d(1.5, 1.5, M_PI_2), 1e-6);
// 2. right turn 90 deg in 4 steps of 22.5 deg --> ends up in (1.5, -1.5, -pi/2)
- data(0) = 0.50*intr->ticks_per_wheel_revolution / 4;
- data(1) = 0.25*intr->ticks_per_wheel_revolution / 4;
+ data(0) = 0.50*params_sen->ticks_per_wheel_revolution / 4;
+ data(1) = 0.25*params_sen->ticks_per_wheel_revolution / 4;
processor->computeCurrentDelta(data, data_cov, calib, dt, delta, delta_cov, J_delta_calib);
x1 .setZero();
@@ -340,8 +338,8 @@ TEST_F(ProcessorDiffDriveTest, process)
// 1. left turn 90 deg in N steps of 90/N deg --> ends up in (1.5, 1.5, pi/2)
int N = 9;
- data(0) = 0.25*intr->ticks_per_wheel_revolution / N;
- data(1) = 0.50*intr->ticks_per_wheel_revolution / N;
+ data(0) = 0.25*params_sen->ticks_per_wheel_revolution / N;
+ data(1) = 0.50*params_sen->ticks_per_wheel_revolution / N;
auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, F0->getCaptureList().front());
@@ -418,8 +416,6 @@ TEST_F(ProcessorDiffDriveTest, angular)
ASSERT_MATRIX_APPROX(processor->getState().vector("PO"), Vector3d(distance,0,angle), 1e-6)
}
-
-
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
diff --git a/test/gtest_processor_fixed_wing_model.cpp b/test/gtest_processor_fixed_wing_model.cpp
index b2bcb1990..53e4a4a9c 100644
--- a/test/gtest_processor_fixed_wing_model.cpp
+++ b/test/gtest_processor_fixed_wing_model.cpp
@@ -33,7 +33,9 @@
using namespace wolf;
using namespace Eigen;
-class ProcessorFixWingModelTest : public testing::Test
+std::string wolf_root = _WOLF_ROOT_DIR;
+
+class ProcessorFixedWingModelTest : public testing::Test
{
protected:
ProblemPtr problem;
@@ -50,19 +52,14 @@ class ProcessorFixWingModelTest : public testing::Test
solver = std::make_shared<SolverCeres>(problem);
// Emplace sensor
- sensor = SensorBase::emplace<SensorBase>(problem->getHardware(),
- "SensorBase",
- std::make_shared<StateBlock>(Vector3d::Zero()),
- std::make_shared<StateQuaternion>((Vector4d() << 0,0,0,1).finished()),
- nullptr,
- 2);
+ sensor = problem->installSensor("SensorMotionModel", "sensor_1", wolf_root + "/test/yaml/sensor_pose_3d.yaml");
// Emplace processor
ParamsProcessorFixedWingModelPtr params = std::make_shared<ParamsProcessorFixedWingModel>();
params->velocity_local = (Vector3d() << 1, 0, 0).finished();
params->angle_stdev = 0.1;
params->min_vel_norm = 0;
- processor = ProcessorBase::emplace<ProcessorFixedWingModel>(sensor, params);
+ processor = problem->installProcessor("ProcessorFixedWingModel","processor_1", sensor, params);
}
FrameBasePtr emplaceFrame(TimeStamp ts, const Vector10d& x)
@@ -72,12 +69,12 @@ class ProcessorFixWingModelTest : public testing::Test
}
};
-TEST_F(ProcessorFixWingModelTest, setup)
+TEST_F(ProcessorFixedWingModelTest, setup)
{
EXPECT_TRUE(problem->check());
}
-TEST_F(ProcessorFixWingModelTest, keyFrameCallback)
+TEST_F(ProcessorFixedWingModelTest, keyFrameCallback)
{
// new frame
auto frm1 = emplaceFrame(1, (Vector10d() << 0,0,0,0,0,0,1,1,0,0).finished());
@@ -100,7 +97,7 @@ TEST_F(ProcessorFixWingModelTest, keyFrameCallback)
ASSERT_TRUE(fac->getCapture() == cap);
}
-TEST_F(ProcessorFixWingModelTest, keyFrameCallbackRepeated)
+TEST_F(ProcessorFixedWingModelTest, keyFrameCallbackRepeated)
{
// new frame
auto frm1 = emplaceFrame(1, (Vector10d() << 0,0,0,0,0,0,1,1,0,0).finished());
@@ -126,7 +123,7 @@ TEST_F(ProcessorFixWingModelTest, keyFrameCallbackRepeated)
ASSERT_TRUE(fac->getCapture() == cap);
}
-TEST_F(ProcessorFixWingModelTest, solve_origin)
+TEST_F(ProcessorFixedWingModelTest, solve_origin)
{
// new frame
auto frm1 = emplaceFrame(1, (Vector10d() << 0,0,0,0,0,0,1,1,0,0).finished());
diff --git a/test/gtest_processor_loop_closure.cpp b/test/gtest_processor_loop_closure.cpp
index 9623ecbaa..773f85b02 100644
--- a/test/gtest_processor_loop_closure.cpp
+++ b/test/gtest_processor_loop_closure.cpp
@@ -34,6 +34,8 @@
using namespace wolf;
using namespace Eigen;
+std::string wolf_root = _WOLF_ROOT_DIR;
+
class ProcessorLoopClosureTest : public testing::Test
{
protected:
@@ -44,19 +46,13 @@ class ProcessorLoopClosureTest : public testing::Test
virtual void SetUp()
{
- // Emplace sensor
- sensor = SensorBase::emplace<SensorBase>(problem->getHardware(),
- "SensorBase",
- std::make_shared<StateBlock>(Vector2d::Zero()),
- std::make_shared<StateBlock>(Vector1d::Zero()),
- nullptr,
- 2);
-
- // Emplace processor
+ // install sensor
+ sensor = problem->installSensor("SensorOdom","my_sensor", wolf_root + "/test/yaml/sensor_odom_2d.yaml");
+
+ // install processor
ParamsProcessorLoopClosurePtr params = std::make_shared<ParamsProcessorLoopClosure>();
params->time_tolerance = 0.5;
- processor = ProcessorBase::emplace<ProcessorLoopClosureDummy>(sensor,
- params);
+ processor = ProcessorBase::emplace<ProcessorLoopClosureDummy>(sensor, params);
}
FrameBasePtr emplaceFrame(TimeStamp ts, const Vector3d& x)
diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp
index ee8c33e5f..33848e703 100644
--- a/test/gtest_processor_motion.cpp
+++ b/test/gtest_processor_motion.cpp
@@ -63,7 +63,7 @@ class ProcessorMotion_test : public testing::Test{
public:
double dt;
ProblemPtr problem;
- SensorOdom2dPtr sensor;
+ SensorOdomPtr sensor;
ProcessorOdom2dPublicPtr processor;
CaptureMotionPtr capture;
Vector2d data;
@@ -75,7 +75,7 @@ class ProcessorMotion_test : public testing::Test{
dt = 1.0;
problem = Problem::create("PO", 2);
- sensor = static_pointer_cast<SensorOdom2d>(problem->installSensor("SensorOdom2d", "odom", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2d.yaml"));
+ sensor = static_pointer_cast<SensorOdom>(problem->installSensor("SensorOdom", "odom", wolf_root + "/test/yaml/sensor_odom_2d.yaml"));
ParamsProcessorOdom2dPtr params(std::make_shared<ParamsProcessorOdom2d>());
params->time_tolerance = 0.5;
params->dist_traveled = 100;
diff --git a/test/gtest_sensor_base.cpp b/test/gtest_sensor_base.cpp
index 27b0bea3a..22bf01d11 100644
--- a/test/gtest_sensor_base.cpp
+++ b/test/gtest_sensor_base.cpp
@@ -405,7 +405,7 @@ TEST(SensorBase, makeshared_server_PO)
continue;
std::string name = "sensor_PO_" +
- to_string(dim) +
+ toString(dim) +
"D_" +
mode +
(dynamic ? "_dynamic" : "") +
@@ -413,7 +413,7 @@ TEST(SensorBase, makeshared_server_PO)
(wrong ? "_wrong" : "");
// Yaml parser
- ParserYaml parser = ParserYaml("test/yaml/sensor_tests/" + name + ".yaml", wolf_root, true);
+ ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/sensor_tests/" + name + ".yaml", true);
ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_1");
WOLF_INFO("Creating sensor from ", name, ".yaml");
@@ -455,7 +455,7 @@ TEST(SensorBase, makeshared_server_PO)
// POIA - 3D - CORRECT YAML
{
// Yaml parser
- ParserYaml parser = ParserYaml("test/yaml/sensor_tests/sensor_POIA_3D.yaml", wolf_root, true);
+ ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/sensor_tests/sensor_POIA_3D.yaml", true);
ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_1");
server.print();
@@ -481,7 +481,7 @@ TEST(SensorBase, makeshared_server_PO)
// POIA - 3D - INCORRECT YAML
{
// Yaml parser
- ParserYaml parser = ParserYaml("test/yaml/sensor_tests/sensor_POIA_3D_wrong.yaml", wolf_root, true);
+ ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/sensor_tests/sensor_POIA_3D_wrong.yaml", true);
ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_1");
WOLF_INFO("Creating sensor from sensor_POIA_3D_wrong.yaml");
@@ -523,14 +523,14 @@ TEST(SensorBase, factory)
continue;
std::string name = "sensor_PO_" +
- to_string(dim) +
+ toString(dim) +
"D_" +
mode +
(dynamic ? "_dynamic" : "") +
(drift ? "_drift" : "") +
(wrong ? "_wrong" : "");
// Yaml parser
- ParserYaml parser = ParserYaml("test/yaml/sensor_tests/" + name + ".yaml", wolf_root, true);
+ ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/sensor_tests/" + name + ".yaml", true);
ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_1");
WOLF_INFO("Creating sensor from ", name, ".yaml");
@@ -571,7 +571,7 @@ TEST(SensorBase, factory)
WOLF_INFO("Creating sensor from name sensor_POIA_3D.yaml");
// Yaml parser
- ParserYaml parser = ParserYaml("test/yaml/sensor_tests/sensor_POIA_3D.yaml", wolf_root, true);
+ ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/sensor_tests/sensor_POIA_3D.yaml", true);
ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_1");
server.print();
@@ -597,7 +597,7 @@ TEST(SensorBase, factory)
WOLF_INFO("Creating sensor from name sensor_POIA_3D_wrong.yaml");
// Yaml parser
- ParserYaml parser = ParserYaml("test/yaml/sensor_tests/sensor_POIA_3D_wrong.yaml", wolf_root, true);
+ ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/sensor_tests/sensor_POIA_3D_wrong.yaml", true);
ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_1");
// create sensor
@@ -637,7 +637,7 @@ TEST(SensorBase, factory_yaml)
std::string yaml_filepath = wolf_root +
"/test/yaml/sensor_tests/" +
"sensor_PO_" +
- to_string(dim) +
+ toString(dim) +
"D_" +
mode +
(dynamic ? "_dynamic" : "") +
diff --git a/test/gtest_sensor_diff_drive.cpp b/test/gtest_sensor_diff_drive.cpp
index 016476db8..2339b5026 100644
--- a/test/gtest_sensor_diff_drive.cpp
+++ b/test/gtest_sensor_diff_drive.cpp
@@ -62,7 +62,7 @@ TEST(SensorDiffDrive, constructor_priors)
TEST(SensorDiffDrive, constructor_server)
{
- ParserYaml parser = ParserYaml("test/yaml/sensor_diff_drive.yaml", wolf_root, true);
+ ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/sensor_diff_drive.yaml", true);
ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_1");
auto params = std::make_shared<ParamsSensorDiffDrive>("sensor_1", server);
@@ -79,7 +79,7 @@ TEST(SensorDiffDrive, constructor_server)
TEST(SensorDiffDrive, factory)
{
- ParserYaml parser = ParserYaml("test/yaml/sensor_diff_drive.yaml", wolf_root, true);
+ ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/sensor_diff_drive.yaml", true);
ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_1");
auto sb = FactorySensor::create("SensorDiffDrive","sensor_1", 2, server);
diff --git a/test/gtest_sensor_odom.cpp b/test/gtest_sensor_odom.cpp
index 3cb35fdb1..e58b353f3 100644
--- a/test/gtest_sensor_odom.cpp
+++ b/test/gtest_sensor_odom.cpp
@@ -345,7 +345,7 @@ TEST(SensorOdom, makeshared_server)
continue;
std::string name = "sensor_PO_" +
- to_string(dim) +
+ toString(dim) +
"D_" +
mode +
(dynamic ? "_dynamic" : "") +
@@ -353,7 +353,7 @@ TEST(SensorOdom, makeshared_server)
(wrong ? "_wrong" : "");
// Yaml parser
- ParserYaml parser = ParserYaml("test/yaml/sensor_tests/" + name + ".yaml", wolf_root, true);
+ ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/sensor_tests/" + name + ".yaml", true);
ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_1");
WOLF_INFO("Creating sensor from ", name, ".yaml");
@@ -416,14 +416,14 @@ TEST(SensorOdom, factory)
continue;
std::string name = "sensor_PO_" +
- to_string(dim) +
+ toString(dim) +
"D_" +
mode +
(dynamic ? "_dynamic" : "") +
(drift ? "_drift" : "") +
(wrong ? "_wrong" : "");
// Yaml parser
- ParserYaml parser = ParserYaml("test/yaml/sensor_tests/" + name + ".yaml", wolf_root, true);
+ ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/sensor_tests/" + name + ".yaml", true);
ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_1");
WOLF_INFO("Creating sensor from ", name, ".yaml");
@@ -485,7 +485,7 @@ TEST(SensorOdom, factory_yaml)
std::string yaml_filepath = wolf_root +
"/test/yaml/sensor_tests/" +
"sensor_PO_" +
- to_string(dim) +
+ toString(dim) +
"D_" +
mode +
(dynamic ? "_dynamic" : "") +
diff --git a/test/gtest_sensor_pose.cpp b/test/gtest_sensor_pose.cpp
index b62d81e5b..85f0c0668 100644
--- a/test/gtest_sensor_pose.cpp
+++ b/test/gtest_sensor_pose.cpp
@@ -98,7 +98,7 @@ TEST(Pose, constructor_priors_3D)
TEST(Pose, constructor_server_2D)
{
- ParserYaml parser = ParserYaml("test/yaml/sensor_pose_2d.yaml", wolf_root, true);
+ ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/sensor_pose_2d.yaml", true);
ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_1");
auto params = std::make_shared<ParamsSensorPose>("sensor_1", server);
@@ -120,7 +120,7 @@ TEST(Pose, constructor_server_2D)
TEST(Pose, constructor_server_3D)
{
- ParserYaml parser = ParserYaml("test/yaml/sensor_pose_3d.yaml", wolf_root, true);
+ ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/sensor_pose_3d.yaml", true);
ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_1");
auto params = std::make_shared<ParamsSensorPose>("sensor_1", server);
@@ -142,7 +142,7 @@ TEST(Pose, constructor_server_3D)
TEST(Pose, factory_2D)
{
- ParserYaml parser = ParserYaml("test/yaml/sensor_pose_2d.yaml", wolf_root, true);
+ ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/sensor_pose_2d.yaml", true);
ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_1");
auto sb = FactorySensor::create("SensorPose","sensor_1", 2, server);
@@ -165,7 +165,7 @@ TEST(Pose, factory_2D)
TEST(Pose, factory_3D)
{
- ParserYaml parser = ParserYaml("test/yaml/sensor_pose_3d.yaml", wolf_root, true);
+ ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/sensor_pose_3d.yaml", true);
ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_1");
auto sb = FactorySensor::create("SensorPose","sensor_1", 3, server);
diff --git a/test/gtest_tree_manager.cpp b/test/gtest_tree_manager.cpp
index 3d3ded934..1ae353d9f 100644
--- a/test/gtest_tree_manager.cpp
+++ b/test/gtest_tree_manager.cpp
@@ -22,6 +22,7 @@
#include "core/utils/utils_gtest.h"
#include "core/problem/problem.h"
+#include "core/tree_manager/factory_tree_manager.h"
#include "dummy/tree_manager_dummy.h"
#include "core/yaml/parser_yaml.h"
@@ -50,7 +51,7 @@ TEST(TreeManager, createParams)
auto ParamsGM = std::make_shared<ParamsTreeManagerBase>();
- auto GM = TreeManagerDummy::create("tree_manager", ParamsGM);
+ auto GM = TreeManagerDummy::create(ParamsGM);
ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerDummy>(GM) != nullptr);
@@ -64,10 +65,71 @@ TEST(TreeManager, createParamServer)
{
ProblemPtr P = Problem::create("PO", 2);
- ParserYaml parser = ParserYaml("test/yaml/params_tree_manager1.yaml", wolf_root);
+ ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/params_tree_manager1.yaml");
ParamsServer server = ParamsServer(parser.getParams());
- auto GM = TreeManagerDummy::create("tree_manager", server);
+ auto GM = TreeManagerDummy::create(server);
+
+ ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerDummy>(GM) != nullptr);
+
+ P->setTreeManager(GM);
+
+ ASSERT_EQ(std::static_pointer_cast<TreeManagerDummy>(P->getTreeManager())->n_KF_, 0);
+ ASSERT_EQ(P->getTreeManager(), GM);
+}
+
+TEST(TreeManager, createYaml)
+{
+ ProblemPtr P = Problem::create("PO", 2);
+
+ auto GM = TreeManagerDummy::create(wolf_root + "/test/yaml/tree_manager_dummy.yaml");
+
+ ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerDummy>(GM) != nullptr);
+
+ P->setTreeManager(GM);
+
+ ASSERT_EQ(std::static_pointer_cast<TreeManagerDummy>(P->getTreeManager())->n_KF_, 0);
+ ASSERT_EQ(P->getTreeManager(), GM);
+}
+
+TEST(TreeManager, Factory)
+{
+ ProblemPtr P = Problem::create("PO", 2);
+
+ auto ParamsGM = std::make_shared<ParamsTreeManagerBase>();
+
+ auto GM = FactoryTreeManager::create("TreeManagerDummy",ParamsGM);
+
+ ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerDummy>(GM) != nullptr);
+
+ P->setTreeManager(GM);
+
+ ASSERT_EQ(std::static_pointer_cast<TreeManagerDummy>(P->getTreeManager())->n_KF_, 0);
+ ASSERT_EQ(P->getTreeManager(), GM);
+}
+
+TEST(TreeManager, FactoryParamServer)
+{
+ ProblemPtr P = Problem::create("PO", 2);
+
+ ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/params_tree_manager1.yaml");
+ ParamsServer server = ParamsServer(parser.getParams());
+
+ auto GM = FactoryTreeManagerServer::create("TreeManagerDummy", server);
+
+ ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerDummy>(GM) != nullptr);
+
+ P->setTreeManager(GM);
+
+ ASSERT_EQ(std::static_pointer_cast<TreeManagerDummy>(P->getTreeManager())->n_KF_, 0);
+ ASSERT_EQ(P->getTreeManager(), GM);
+}
+
+TEST(TreeManager, FactoryYaml)
+{
+ ProblemPtr P = Problem::create("PO", 2);
+
+ auto GM = FactoryTreeManagerYaml::create("TreeManagerDummy", wolf_root + "/test/yaml/tree_manager_dummy.yaml");
ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerDummy>(GM) != nullptr);
@@ -80,7 +142,7 @@ TEST(TreeManager, createParamServer)
TEST(TreeManager, autoConf)
{
- ParserYaml parser = ParserYaml("test/yaml/params_tree_manager1.yaml", wolf_root);
+ ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/params_tree_manager1.yaml");
ParamsServer server = ParamsServer(parser.getParams());
ProblemPtr P = Problem::autoSetup(server);
@@ -92,8 +154,7 @@ TEST(TreeManager, autoConf)
TEST(TreeManager, autoConfNone)
{
-
- ParserYaml parser = ParserYaml("test/yaml/params_tree_manager2.yaml", wolf_root);
+ ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/params_tree_manager2.yaml");
ParamsServer server = ParamsServer(parser.getParams());
ProblemPtr P = Problem::autoSetup(server);
diff --git a/test/gtest_tree_manager_sliding_window.cpp b/test/gtest_tree_manager_sliding_window.cpp
index e4b1c92bb..0f03f5652 100644
--- a/test/gtest_tree_manager_sliding_window.cpp
+++ b/test/gtest_tree_manager_sliding_window.cpp
@@ -23,6 +23,7 @@
#include "core/factor/factor_relative_pose_3d.h"
#include "core/problem/problem.h"
+#include "core/tree_manager/factory_tree_manager.h"
#include "core/tree_manager/tree_manager_sliding_window.h"
#include "core/yaml/parser_yaml.h"
#include "core/capture/capture_void.h"
@@ -53,7 +54,7 @@ TEST(TreeManagerSlidingWindow, createParams)
auto ParamsGM = std::make_shared<ParamsTreeManagerSlidingWindow>();
- auto GM = TreeManagerSlidingWindow::create("tree_manager", ParamsGM);
+ auto GM = TreeManagerSlidingWindow::create(ParamsGM);
EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(GM) != nullptr);
@@ -67,10 +68,71 @@ TEST(TreeManagerSlidingWindow, createParamServer)
{
ProblemPtr P = Problem::create("PO", 2);
- ParserYaml parser = ParserYaml("test/yaml/params_tree_manager_sliding_window1.yaml", wolf_root);
+ ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/params_tree_manager_sliding_window1.yaml");
ParamsServer server = ParamsServer(parser.getParams());
- auto GM = TreeManagerSlidingWindow::create("tree_manager", server);
+ auto GM = TreeManagerSlidingWindow::create(server);
+
+ EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(GM) != nullptr);
+
+ P->setTreeManager(GM);
+
+ EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(P->getTreeManager()) != nullptr);
+ EXPECT_EQ(P->getTreeManager(), GM);
+}
+
+TEST(TreeManagerSlidingWindow, createYaml)
+{
+ ProblemPtr P = Problem::create("PO", 2);
+
+ auto GM = TreeManagerSlidingWindow::create(wolf_root + "/test/yaml/tree_manager_sliding_window1.yaml");
+
+ EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(GM) != nullptr);
+
+ P->setTreeManager(GM);
+
+ EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(P->getTreeManager()) != nullptr);
+ EXPECT_EQ(P->getTreeManager(), GM);
+}
+
+TEST(TreeManager, Factory)
+{
+ ProblemPtr P = Problem::create("PO", 2);
+
+ auto ParamsGM = std::make_shared<ParamsTreeManagerSlidingWindow>();
+
+ auto GM = FactoryTreeManager::create("TreeManagerSlidingWindow",ParamsGM);
+
+ EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(GM) != nullptr);
+
+ P->setTreeManager(GM);
+
+ EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(P->getTreeManager()) != nullptr);
+ ASSERT_EQ(P->getTreeManager(), GM);
+}
+
+TEST(TreeManager, FactoryParamServer)
+{
+ ProblemPtr P = Problem::create("PO", 2);
+
+ ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/params_tree_manager_sliding_window1.yaml");
+ ParamsServer server = ParamsServer(parser.getParams());
+
+ auto GM = FactoryTreeManagerServer::create("TreeManagerSlidingWindow", server);
+
+ EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(GM) != nullptr);
+
+ P->setTreeManager(GM);
+
+ EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(P->getTreeManager()) != nullptr);
+ EXPECT_EQ(P->getTreeManager(), GM);
+}
+
+TEST(TreeManager, FactoryYaml)
+{
+ ProblemPtr P = Problem::create("PO", 2);
+
+ auto GM = FactoryTreeManagerYaml::create("TreeManagerSlidingWindow", wolf_root + "/test/yaml/tree_manager_sliding_window1.yaml");
EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindow>(GM) != nullptr);
@@ -82,7 +144,7 @@ TEST(TreeManagerSlidingWindow, createParamServer)
TEST(TreeManagerSlidingWindow, autoConf)
{
- ParserYaml parser = ParserYaml("test/yaml/params_tree_manager_sliding_window1.yaml", wolf_root);
+ ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/params_tree_manager_sliding_window1.yaml");
ParamsServer server = ParamsServer(parser.getParams());
ProblemPtr P = Problem::autoSetup(server);
@@ -95,7 +157,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
// window size: 3
// first 2 frames fixed
- ParserYaml parser = ParserYaml("test/yaml/params_tree_manager_sliding_window1.yaml", wolf_root);
+ ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/params_tree_manager_sliding_window1.yaml");
ParamsServer server = ParamsServer(parser.getParams());
ProblemPtr P = Problem::autoSetup(server);
@@ -204,7 +266,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
{
- ParserYaml parser = ParserYaml("test/yaml/params_tree_manager_sliding_window2.yaml", wolf_root);
+ ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/params_tree_manager_sliding_window2.yaml");
ParamsServer server = ParamsServer(parser.getParams());
ProblemPtr P = Problem::autoSetup(server);
diff --git a/test/gtest_tree_manager_sliding_window_dual_rate.cpp b/test/gtest_tree_manager_sliding_window_dual_rate.cpp
index 448fc0d93..6f827c592 100644
--- a/test/gtest_tree_manager_sliding_window_dual_rate.cpp
+++ b/test/gtest_tree_manager_sliding_window_dual_rate.cpp
@@ -23,6 +23,7 @@
#include "core/factor/factor_relative_pose_3d.h"
#include "core/problem/problem.h"
+#include "core/tree_manager/factory_tree_manager.h"
#include "core/tree_manager/tree_manager_sliding_window_dual_rate.h"
#include "core/yaml/parser_yaml.h"
#include "core/capture/capture_void.h"
@@ -55,7 +56,7 @@ TEST(TreeManagerSlidingWindowDualRate, createParams)
auto ParamsGM = std::make_shared<ParamsTreeManagerSlidingWindowDualRate>();
- auto GM = TreeManagerSlidingWindowDualRate::create("tree_manager", ParamsGM);
+ auto GM = TreeManagerSlidingWindowDualRate::create(ParamsGM);
EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(GM) != nullptr);
@@ -69,10 +70,71 @@ TEST(TreeManagerSlidingWindowDualRate, createParamServer)
{
ProblemPtr P = Problem::create("PO", 2);
- ParserYaml parser = ParserYaml("test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml", wolf_root);
+ ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml");
ParamsServer server = ParamsServer(parser.getParams());
- auto GM = TreeManagerSlidingWindowDualRate::create("tree_manager", server);
+ auto GM = TreeManagerSlidingWindowDualRate::create(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, createYaml)
+{
+ ProblemPtr P = Problem::create("PO", 2);
+
+ auto GM = TreeManagerSlidingWindowDualRate::create(wolf_root + "/test/yaml/tree_manager_sliding_window_dual_rate1.yaml");
+
+ 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, FactoryParams)
+{
+ ProblemPtr P = Problem::create("PO", 2);
+
+ auto ParamsGM = std::make_shared<ParamsTreeManagerSlidingWindowDualRate>();
+
+ auto GM = FactoryTreeManager::create("TreeManagerSlidingWindowDualRate",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, FactoryParamServer)
+{
+ ProblemPtr P = Problem::create("PO", 2);
+
+ ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml");
+ ParamsServer server = ParamsServer(parser.getParams());
+
+ auto GM = FactoryTreeManagerServer::create("TreeManagerSlidingWindowDualRate",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, FactoryYaml)
+{
+ ProblemPtr P = Problem::create("PO", 2);
+
+ auto GM = FactoryTreeManagerYaml::create("TreeManagerSlidingWindowDualRate", wolf_root + "/test/yaml/tree_manager_sliding_window_dual_rate1.yaml");
EXPECT_TRUE(std::dynamic_pointer_cast<TreeManagerSlidingWindowDualRate>(GM) != nullptr);
@@ -84,7 +146,7 @@ TEST(TreeManagerSlidingWindowDualRate, createParamServer)
TEST(TreeManagerSlidingWindowDualRate, autoConf)
{
- ParserYaml parser = ParserYaml("test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml", wolf_root);
+ ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml");
ParamsServer server = ParamsServer(parser.getParams());
ProblemPtr P = Problem::autoSetup(server);
@@ -101,7 +163,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
* rate_old_frames: 2
*/
- ParserYaml parser = ParserYaml("test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml", wolf_root);
+ ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml");
ParamsServer server = ParamsServer(parser.getParams());
ProblemPtr P = Problem::autoSetup(server);
@@ -577,7 +639,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowNoFixNoViral)
* rate_old_frames: 2
*/
- ParserYaml parser = ParserYaml("test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml", wolf_root);
+ ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml");
ParamsServer server = ParamsServer(parser.getParams());
ProblemPtr P = Problem::autoSetup(server);
@@ -1033,13 +1095,13 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowNoFixNoViral)
TEST(TreeManagerSlidingWindowDualRate, slidingWindowWithProcessor)
{
// SLIDING WINDOW DUAL RATE
- ParserYaml parser = ParserYaml("test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml", wolf_root);
+ ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml");
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);
+ ParserYaml parser_bl = ParserYaml(wolf_root + "/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml");
ParamsServer server_bl = ParamsServer(parser_bl.getParams());
ProblemPtr problem_bl = Problem::autoSetup(server_bl);
SolverManagerPtr solver_bl = FactorySolver::create("SolverCeres", problem_bl, server);
diff --git a/test/yaml/params1.yaml b/test/yaml/params1.yaml
index b8d6dd7b6..5e6431172 100644
--- a/test/yaml/params1.yaml
+++ b/test/yaml/params1.yaml
@@ -32,10 +32,10 @@ config:
name: "my_proc_test"
sensor_name: "odom"
plugin: "core"
- follow: "test/yaml/params3.1.yaml"
+ follow: "params3.1.yaml"
-
type: "ODOM 3d"
name: "my_proc_odom3d"
sensor_name: "odom"
plugin: "core"
- follow: "test/yaml/processor_odom_3d.yaml"
+ follow: "processor_odom_3d.yaml"
diff --git a/test/yaml/params3.yaml b/test/yaml/params3.yaml
index e23b24ea4..4d0487bde 100644
--- a/test/yaml/params3.yaml
+++ b/test/yaml/params3.yaml
@@ -18,4 +18,4 @@ config:
name: "my_proc_test"
plugin: "core"
sensor_name: "odom"
- extern_params: "@test/yaml/params3.1.yaml"
\ No newline at end of file
+ extern_params: "@params3.1.yaml"
\ No newline at end of file
diff --git a/test/yaml/params_tree_manager1.yaml b/test/yaml/params_tree_manager1.yaml
index 5a75360a6..2cc4c231e 100644
--- a/test/yaml/params_tree_manager1.yaml
+++ b/test/yaml/params_tree_manager1.yaml
@@ -16,9 +16,7 @@ config:
V: [0.31, 0.31, 0.31]
time_tolerance: 0.1
tree_manager:
- type: "TreeManagerDummy"
- plugin: "core"
- toy_param: 0
+ follow: "tree_manager_dummy.yaml"
sensors:
-
type: "SensorOdom"
diff --git a/test/yaml/params_tree_manager_sliding_window1.yaml b/test/yaml/params_tree_manager_sliding_window1.yaml
index 61498d1b6..5a6f7cac2 100644
--- a/test/yaml/params_tree_manager_sliding_window1.yaml
+++ b/test/yaml/params_tree_manager_sliding_window1.yaml
@@ -12,23 +12,26 @@ config:
O: [0.31, 0.31, 0.31]
time_tolerance: 0.1
tree_manager:
- type: "TreeManagerSlidingWindow"
- plugin: "core"
- n_frames: 3
- n_fix_first_frames: 2
- viral_remove_empty_parent: true
+ follow: "tree_manager_sliding_window1.yaml"
sensors:
-
- type: "SensorOdom3d"
+ type: "SensorOdom"
name: "odom"
plugin: "core"
+ states:
+ P:
+ mode: fix
+ state: [1, 2, 3]
+ dynamic: false
+ O:
+ mode: fix
+ state: [0, 0, 0, 1]
+ dynamic: false
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"
diff --git a/test/yaml/params_tree_manager_sliding_window2.yaml b/test/yaml/params_tree_manager_sliding_window2.yaml
index db0a176fc..768cedbe4 100644
--- a/test/yaml/params_tree_manager_sliding_window2.yaml
+++ b/test/yaml/params_tree_manager_sliding_window2.yaml
@@ -12,23 +12,26 @@ config:
O: [0.31, 0.31, 0.31]
time_tolerance: 0.1
tree_manager:
- type: "TreeManagerSlidingWindow"
- plugin: "core"
- n_frames: 3
- n_fix_first_frames: 0
- viral_remove_empty_parent: false
+ follow: "tree_manager_sliding_window2.yaml"
sensors:
-
- type: "SensorOdom3d"
+ type: "SensorOdom"
name: "odom"
plugin: "core"
+ states:
+ P:
+ mode: fix
+ state: [1, 2, 3]
+ dynamic: false
+ O:
+ mode: fix
+ state: [0, 0, 0, 1]
+ dynamic: false
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"
diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml
index 2b6313f5b..6da054449 100644
--- a/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml
+++ b/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml
@@ -11,11 +11,4 @@ config:
P: [0.31, 0.31, 0.31]
O: [0.31, 0.31, 0.31]
time_tolerance: 0.1
- tree_manager:
- type: "TreeManagerSlidingWindowDualRate"
- plugin: "core"
- n_frames: 5
- n_frames_recent: 3
- rate_old_frames: 2
- n_fix_first_frames: 2
- viral_remove_empty_parent: true
+ tree_manager: "@tree_manager_sliding_window_dual_rate1.yaml"
\ No newline at end of file
diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml
index aed7a0c7e..79f9a97fa 100644
--- a/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml
+++ b/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml
@@ -1,6 +1,5 @@
config:
- solver:
- follow: "test/yaml/solver.yaml"
+ solver: "@solver.yaml"
problem:
frame_structure: "PO"
dimension: 3
@@ -23,16 +22,23 @@ config:
viral_remove_empty_parent: true
sensors:
-
- type: "SensorOdom3d"
+ type: "SensorOdom"
name: "odom"
plugin: "core"
+ states:
+ P:
+ mode: fix
+ state: [1, 2, 3]
+ dynamic: false
+ O:
+ mode: fix
+ state: [0, 0, 0, 1]
+ dynamic: false
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"
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
index d00b201f9..46e4eeff5 100644
--- a/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml
+++ b/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml
@@ -1,6 +1,5 @@
config:
- solver:
- follow: "test/yaml/solver.yaml"
+ solver: "@solver.yaml"
problem:
frame_structure: "PO"
dimension: 3
@@ -17,16 +16,23 @@ config:
type: "None"
sensors:
-
- type: "SensorOdom3d"
+ type: "SensorOdom"
name: "odom"
plugin: "core"
+ states:
+ P:
+ mode: fix
+ state: [1, 2, 3]
+ dynamic: false
+ O:
+ mode: fix
+ state: [0, 0, 0, 1]
+ dynamic: false
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"
diff --git a/test/yaml/processor_odom_3d.yaml b/test/yaml/processor_odom_3d.yaml
index 0fe5e5c6d..1ada1d20a 100644
--- a/test/yaml/processor_odom_3d.yaml
+++ b/test/yaml/processor_odom_3d.yaml
@@ -9,4 +9,9 @@ keyframe_vote:
dist_traveled: 0.5 # meters
angle_turned: 0.1 # radians (1 rad approx 57 deg, approx 60 deg)
-unmeasured_perturbation_std: 0.001
\ No newline at end of file
+unmeasured_perturbation_std: 0.001
+
+apply_loss_function: false
+
+state_getter: true
+state_priority: 1
\ No newline at end of file
diff --git a/test/yaml/tree_manager_dummy.yaml b/test/yaml/tree_manager_dummy.yaml
new file mode 100644
index 000000000..f4d78ccc5
--- /dev/null
+++ b/test/yaml/tree_manager_dummy.yaml
@@ -0,0 +1,3 @@
+type: "TreeManagerDummy"
+plugin: "core"
+toy_param: 0
\ No newline at end of file
diff --git a/test/yaml/tree_manager_sliding_window1.yaml b/test/yaml/tree_manager_sliding_window1.yaml
new file mode 100644
index 000000000..48234b6c8
--- /dev/null
+++ b/test/yaml/tree_manager_sliding_window1.yaml
@@ -0,0 +1,5 @@
+type: "TreeManagerSlidingWindow"
+plugin: "core"
+n_frames: 3
+n_fix_first_frames: 2
+viral_remove_empty_parent: true
diff --git a/test/yaml/tree_manager_sliding_window2.yaml b/test/yaml/tree_manager_sliding_window2.yaml
new file mode 100644
index 000000000..7f074d9e4
--- /dev/null
+++ b/test/yaml/tree_manager_sliding_window2.yaml
@@ -0,0 +1,5 @@
+type: "TreeManagerSlidingWindow"
+plugin: "core"
+n_frames: 3
+n_fix_first_frames: 0
+viral_remove_empty_parent: false
\ No newline at end of file
diff --git a/test/yaml/tree_manager_sliding_window_dual_rate1.yaml b/test/yaml/tree_manager_sliding_window_dual_rate1.yaml
new file mode 100644
index 000000000..d18a4fb29
--- /dev/null
+++ b/test/yaml/tree_manager_sliding_window_dual_rate1.yaml
@@ -0,0 +1,7 @@
+type: "TreeManagerSlidingWindowDualRate"
+plugin: "core"
+n_frames: 5
+n_frames_recent: 3
+rate_old_frames: 2
+n_fix_first_frames: 2
+viral_remove_empty_parent: true
--
GitLab