From a70dd2e4ec96d6990c744d7bfe10d9345ea1d2c3 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Wed, 25 May 2022 14:41:33 +0200
Subject: [PATCH] FactorySensor & FactorySensorYaml same yaml
---
include/core/sensor/sensor_base.h | 2 +-
include/core/utils/params_server.h | 8 +-
src/problem/problem.cpp | 8 +-
src/utils/params_server.cpp | 23 +-
test/gtest_sensor_base.cpp | 61 +++-
test/gtest_sensor_odom_2d.cpp | 338 ++++++++++++------
test/yaml/sensor_base/sensor_POIA_3D.yaml | 23 ++
.../sensor_base/sensor_POIA_3D_wrong.yaml | 23 ++
.../yaml/sensor_base/sensor_PO_2D_factor.yaml | 13 +
.../sensor_PO_2D_factor_dynamic.yaml | 13 +
.../sensor_PO_2D_factor_dynamic_drift.yaml | 15 +
...nsor_PO_2D_factor_dynamic_drift_wrong.yaml | 15 +
.../sensor_PO_2D_factor_dynamic_wrong.yaml | 13 +
.../sensor_PO_2D_factor_wrong.yaml | 13 +
test/yaml/sensor_base/sensor_PO_2D_fix.yaml | 11 +
.../sensor_base/sensor_PO_2D_fix_dynamic.yaml | 11 +
.../sensor_PO_2D_fix_dynamic_drift.yaml | 13 +
.../sensor_PO_2D_fix_dynamic_drift_wrong.yaml | 13 +
.../sensor_PO_2D_fix_dynamic_wrong.yaml | 11 +
.../sensor_base/sensor_PO_2D_fix_wrong.yaml | 11 +
.../sensor_PO_2D_initial_guess.yaml | 11 +
.../sensor_PO_2D_initial_guess_dynamic.yaml | 11 +
...sor_PO_2D_initial_guess_dynamic_drift.yaml | 13 +
..._2D_initial_guess_dynamic_drift_wrong.yaml | 13 +
...sor_PO_2D_initial_guess_dynamic_wrong.yaml | 11 +
.../sensor_PO_2D_initial_guess_wrong.yaml | 11 +
.../yaml/sensor_base/sensor_PO_3D_factor.yaml | 13 +
.../sensor_PO_3D_factor_dynamic.yaml | 13 +
.../sensor_PO_3D_factor_dynamic_drift.yaml | 15 +
...nsor_PO_3D_factor_dynamic_drift_wrong.yaml | 15 +
.../sensor_PO_3D_factor_dynamic_wrong.yaml | 13 +
.../sensor_PO_3D_factor_wrong.yaml | 13 +
test/yaml/sensor_base/sensor_PO_3D_fix.yaml | 11 +
.../sensor_base/sensor_PO_3D_fix_dynamic.yaml | 11 +
.../sensor_PO_3D_fix_dynamic_drift.yaml | 13 +
.../sensor_PO_3D_fix_dynamic_drift_wrong.yaml | 13 +
.../sensor_PO_3D_fix_dynamic_wrong.yaml | 11 +
.../sensor_base/sensor_PO_3D_fix_wrong.yaml | 11 +
.../sensor_PO_3D_initial_guess.yaml | 11 +
.../sensor_PO_3D_initial_guess_dynamic.yaml | 11 +
...sor_PO_3D_initial_guess_dynamic_drift.yaml | 13 +
..._3D_initial_guess_dynamic_drift_wrong.yaml | 13 +
...sor_PO_3D_initial_guess_dynamic_wrong.yaml | 11 +
.../sensor_PO_3D_initial_guess_wrong.yaml | 11 +
.../sensor_all.yaml} | 0
test/yaml/sensor_base/sensor_dummy.yaml | 24 ++
test/yaml/sensor_base/sensor_dummy_wrong.yaml | 24 ++
test/yaml/sensor_dummy.yaml | 52 ---
test/yaml/sensor_odom_2d.yaml | 316 ----------------
test/yaml/sensor_odom_2d/sensor_odom_2d.yaml | 14 +
.../sensor_odom_2d/sensor_odom_2d_factor.yaml | 14 +
.../sensor_odom_2d_factor_dynamic.yaml | 14 +
.../sensor_odom_2d_factor_dynamic_drift.yaml | 16 +
...or_odom_2d_factor_dynamic_drift_wrong.yaml | 16 +
.../sensor_odom_2d_factor_dynamic_wrong.yaml | 14 +
.../sensor_odom_2d_factor_wrong.yaml | 14 +
.../sensor_odom_2d/sensor_odom_2d_fix.yaml | 12 +
.../sensor_odom_2d_fix_dynamic.yaml | 12 +
.../sensor_odom_2d_fix_dynamic_drift.yaml | 14 +
...ensor_odom_2d_fix_dynamic_drift_wrong.yaml | 14 +
.../sensor_odom_2d_fix_dynamic_wrong.yaml | 12 +
.../sensor_odom_2d_fix_wrong.yaml | 12 +
.../sensor_odom_2d_initial_guess.yaml | 12 +
.../sensor_odom_2d_initial_guess_dynamic.yaml | 12 +
...r_odom_2d_initial_guess_dynamic_drift.yaml | 14 +
..._2d_initial_guess_dynamic_drift_wrong.yaml | 14 +
...r_odom_2d_initial_guess_dynamic_wrong.yaml | 12 +
.../sensor_odom_2d_initial_guess_wrong.yaml | 12 +
.../sensor_odom_2d/sensor_odom_2d_wrong.yaml | 13 +
69 files changed, 1116 insertions(+), 497 deletions(-)
create mode 100644 test/yaml/sensor_base/sensor_POIA_3D.yaml
create mode 100644 test/yaml/sensor_base/sensor_POIA_3D_wrong.yaml
create mode 100644 test/yaml/sensor_base/sensor_PO_2D_factor.yaml
create mode 100644 test/yaml/sensor_base/sensor_PO_2D_factor_dynamic.yaml
create mode 100644 test/yaml/sensor_base/sensor_PO_2D_factor_dynamic_drift.yaml
create mode 100644 test/yaml/sensor_base/sensor_PO_2D_factor_dynamic_drift_wrong.yaml
create mode 100644 test/yaml/sensor_base/sensor_PO_2D_factor_dynamic_wrong.yaml
create mode 100644 test/yaml/sensor_base/sensor_PO_2D_factor_wrong.yaml
create mode 100644 test/yaml/sensor_base/sensor_PO_2D_fix.yaml
create mode 100644 test/yaml/sensor_base/sensor_PO_2D_fix_dynamic.yaml
create mode 100644 test/yaml/sensor_base/sensor_PO_2D_fix_dynamic_drift.yaml
create mode 100644 test/yaml/sensor_base/sensor_PO_2D_fix_dynamic_drift_wrong.yaml
create mode 100644 test/yaml/sensor_base/sensor_PO_2D_fix_dynamic_wrong.yaml
create mode 100644 test/yaml/sensor_base/sensor_PO_2D_fix_wrong.yaml
create mode 100644 test/yaml/sensor_base/sensor_PO_2D_initial_guess.yaml
create mode 100644 test/yaml/sensor_base/sensor_PO_2D_initial_guess_dynamic.yaml
create mode 100644 test/yaml/sensor_base/sensor_PO_2D_initial_guess_dynamic_drift.yaml
create mode 100644 test/yaml/sensor_base/sensor_PO_2D_initial_guess_dynamic_drift_wrong.yaml
create mode 100644 test/yaml/sensor_base/sensor_PO_2D_initial_guess_dynamic_wrong.yaml
create mode 100644 test/yaml/sensor_base/sensor_PO_2D_initial_guess_wrong.yaml
create mode 100644 test/yaml/sensor_base/sensor_PO_3D_factor.yaml
create mode 100644 test/yaml/sensor_base/sensor_PO_3D_factor_dynamic.yaml
create mode 100644 test/yaml/sensor_base/sensor_PO_3D_factor_dynamic_drift.yaml
create mode 100644 test/yaml/sensor_base/sensor_PO_3D_factor_dynamic_drift_wrong.yaml
create mode 100644 test/yaml/sensor_base/sensor_PO_3D_factor_dynamic_wrong.yaml
create mode 100644 test/yaml/sensor_base/sensor_PO_3D_factor_wrong.yaml
create mode 100644 test/yaml/sensor_base/sensor_PO_3D_fix.yaml
create mode 100644 test/yaml/sensor_base/sensor_PO_3D_fix_dynamic.yaml
create mode 100644 test/yaml/sensor_base/sensor_PO_3D_fix_dynamic_drift.yaml
create mode 100644 test/yaml/sensor_base/sensor_PO_3D_fix_dynamic_drift_wrong.yaml
create mode 100644 test/yaml/sensor_base/sensor_PO_3D_fix_dynamic_wrong.yaml
create mode 100644 test/yaml/sensor_base/sensor_PO_3D_fix_wrong.yaml
create mode 100644 test/yaml/sensor_base/sensor_PO_3D_initial_guess.yaml
create mode 100644 test/yaml/sensor_base/sensor_PO_3D_initial_guess_dynamic.yaml
create mode 100644 test/yaml/sensor_base/sensor_PO_3D_initial_guess_dynamic_drift.yaml
create mode 100644 test/yaml/sensor_base/sensor_PO_3D_initial_guess_dynamic_drift_wrong.yaml
create mode 100644 test/yaml/sensor_base/sensor_PO_3D_initial_guess_dynamic_wrong.yaml
create mode 100644 test/yaml/sensor_base/sensor_PO_3D_initial_guess_wrong.yaml
rename test/yaml/{sensor_base.yaml => sensor_base/sensor_all.yaml} (100%)
create mode 100644 test/yaml/sensor_base/sensor_dummy.yaml
create mode 100644 test/yaml/sensor_base/sensor_dummy_wrong.yaml
delete mode 100644 test/yaml/sensor_dummy.yaml
delete mode 100644 test/yaml/sensor_odom_2d.yaml
create mode 100644 test/yaml/sensor_odom_2d/sensor_odom_2d.yaml
create mode 100644 test/yaml/sensor_odom_2d/sensor_odom_2d_factor.yaml
create mode 100644 test/yaml/sensor_odom_2d/sensor_odom_2d_factor_dynamic.yaml
create mode 100644 test/yaml/sensor_odom_2d/sensor_odom_2d_factor_dynamic_drift.yaml
create mode 100644 test/yaml/sensor_odom_2d/sensor_odom_2d_factor_dynamic_drift_wrong.yaml
create mode 100644 test/yaml/sensor_odom_2d/sensor_odom_2d_factor_dynamic_wrong.yaml
create mode 100644 test/yaml/sensor_odom_2d/sensor_odom_2d_factor_wrong.yaml
create mode 100644 test/yaml/sensor_odom_2d/sensor_odom_2d_fix.yaml
create mode 100644 test/yaml/sensor_odom_2d/sensor_odom_2d_fix_dynamic.yaml
create mode 100644 test/yaml/sensor_odom_2d/sensor_odom_2d_fix_dynamic_drift.yaml
create mode 100644 test/yaml/sensor_odom_2d/sensor_odom_2d_fix_dynamic_drift_wrong.yaml
create mode 100644 test/yaml/sensor_odom_2d/sensor_odom_2d_fix_dynamic_wrong.yaml
create mode 100644 test/yaml/sensor_odom_2d/sensor_odom_2d_fix_wrong.yaml
create mode 100644 test/yaml/sensor_odom_2d/sensor_odom_2d_initial_guess.yaml
create mode 100644 test/yaml/sensor_odom_2d/sensor_odom_2d_initial_guess_dynamic.yaml
create mode 100644 test/yaml/sensor_odom_2d/sensor_odom_2d_initial_guess_dynamic_drift.yaml
create mode 100644 test/yaml/sensor_odom_2d/sensor_odom_2d_initial_guess_dynamic_drift_wrong.yaml
create mode 100644 test/yaml/sensor_odom_2d/sensor_odom_2d_initial_guess_dynamic_wrong.yaml
create mode 100644 test/yaml/sensor_odom_2d/sensor_odom_2d_initial_guess_wrong.yaml
create mode 100644 test/yaml/sensor_odom_2d/sensor_odom_2d_wrong.yaml
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 5f55b72dd..48eddca9f 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -71,7 +71,7 @@ static SensorBasePtr create(const std::string& _unique_name, SizeEigen _dim, con
{ \
auto parser = ParserYaml(_yaml_filepath, "", true); \
\
- auto server = ParamsServer(parser.getParams()); \
+ auto server = ParamsServer(parser.getParams(), "sensor/" + _unique_name); \
\
auto params = std::make_shared<ParamsSensorClass>(_unique_name, server); \
\
diff --git a/include/core/utils/params_server.h b/include/core/utils/params_server.h
index b1562a906..2ca64f4cc 100644
--- a/include/core/utils/params_server.h
+++ b/include/core/utils/params_server.h
@@ -39,12 +39,13 @@ class ParamsServer
{
private:
+ std::string global_prefix_;
std::map<std::string, std::string> params_;
public:
- ParamsServer();
- ParamsServer(std::map<std::string, std::string> _params);
+ ParamsServer(const std::string& _global_prefix = "");
+ ParamsServer(std::map<std::string, std::string> _params, const std::string& _global_prefix = "");
~ParamsServer()
{
//
@@ -62,11 +63,10 @@ class ParamsServer
T getParam(std::string _key) const
{
if(params_.count(_key))
- return converter<T>::convert(params_.find(_key)->second);
+ return converter<T>::convert(params_.at(_key));
else
throw MissingValueException("The following key: '" + _key + "' has not been found in the parameters server.");
}
-
};
template<typename Derived>
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index dc064e6b7..f624a36df 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -223,12 +223,11 @@ SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
const std::string& _unique_sensor_name, //
const Eigen::VectorXd& _extrinsics, //
- const std::string& _intrinsics_filename)
+ const std::string& _params_yaml_filename)
{
-
- if (_intrinsics_filename != "")
+ if (_params_yaml_filename != "")
{
- assert(file_exists(_intrinsics_filename) && "Cannot install sensor: intrinsics' YAML file does not exist.");
+ assert(file_exists(_params_yaml_filename) && "Cannot install sensor: intrinsics' YAML file does not exist.");
//ParamsSensorBasePtr intr_ptr = FactoryParamsSensor::create(_sen_type, _intrinsics_filename);
return installSensor(_sen_type, _unique_sensor_name, _extrinsics, ParamsSensorBasePtr());
}
@@ -241,7 +240,6 @@ SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
const std::string& _unique_sensor_name, //
const ParamsServer& _server)
{
- //ParamsSensorBasePtr params = FactoryParamsSensor::create(_sen_type, _unique_sensor_name, _server);
SensorBasePtr sen_ptr = FactorySensor::create(_sen_type, _unique_sensor_name, dim_, _server);
sen_ptr->link(getHardware());
return sen_ptr;
diff --git a/src/utils/params_server.cpp b/src/utils/params_server.cpp
index b8f2475bd..249dabd38 100644
--- a/src/utils/params_server.cpp
+++ b/src/utils/params_server.cpp
@@ -24,13 +24,21 @@
namespace wolf
{
-ParamsServer::ParamsServer()
+ParamsServer::ParamsServer(const std::string& _global_prefix) :
+ ParamsServer::ParamsServer(std::map<std::string, std::string>{}, _global_prefix)
{
- params_ = std::map<std::string, std::string>();
}
-ParamsServer::ParamsServer(std::map<std::string, std::string> _params)
-{
- params_ = _params;
+ParamsServer::ParamsServer(std::map<std::string, std::string> _params, const std::string& _global_prefix) :
+ global_prefix_(_global_prefix)
+{
+ if (not global_prefix_.empty())
+ {
+ if (global_prefix_.front() == '/')
+ global_prefix_.erase(0,1);
+ if (global_prefix_.back() != '/')
+ global_prefix_.push_back('/');
+ }
+ addParams(_params);
}
void ParamsServer::print()
@@ -41,12 +49,13 @@ void ParamsServer::print()
void ParamsServer::addParam(std::string _key, std::string _value)
{
- params_.insert(std::pair<std::string, std::string>(_key, _value));
+ params_.insert(std::pair<std::string, std::string>(global_prefix_ + _key, _value));
}
void ParamsServer::addParams(std::map<std::string, std::string> _params)
{
- params_.insert(_params.begin(), _params.end());
+ for (auto param_pair : _params)
+ addParam(param_pair.first, param_pair.second);
}
std::string toString(bool _arg)
diff --git a/test/gtest_sensor_base.cpp b/test/gtest_sensor_base.cpp
index 76a9a0d42..06e1430b2 100644
--- a/test/gtest_sensor_base.cpp
+++ b/test/gtest_sensor_base.cpp
@@ -344,11 +344,8 @@ TEST(SensorBase, POI_mixed)
}
// CONSTRUCTOR WITH PARAM SERVER and KEY_TYPES
-TEST(SensorBase, server)
+TEST(SensorBase, server_PO)
{
- ParserYaml parser = ParserYaml("test/yaml/sensor_base.yaml", wolf_root, true);
- ParamsServer server = ParamsServer(parser.getParams());
-
std::vector<int> dims({2, 3});
std::vector<std::string> modes({"fix", "initial_guess", "factor"});
std::vector<bool> dynamics({false, true});
@@ -369,8 +366,14 @@ TEST(SensorBase, server)
if (not dynamic and drift)
continue;
- // CORRECT YAML
std::string name = "sensor_PO_" + to_string(dim) + "D_" + mode + (dynamic ? "_dynamic" : "") + (drift ? "_drift" : "");
+
+ // Yaml parser
+ ParserYaml parser = ParserYaml("test/yaml/sensor_base/" + name + ".yaml", wolf_root, true);
+ ParamsServer server = ParamsServer(parser.getParams(), "/sensor/" + name);
+ server.print();
+
+ // CORRECT YAML
WOLF_INFO("Creating sensor from name ", name);
auto params = std::make_shared<ParamsSensorBase>(name, server);
@@ -401,6 +404,19 @@ TEST(SensorBase, server)
std::make_shared<ParamsSensorBase>(name + "_wrong", server),
server),std::runtime_error);
}
+}
+
+TEST(SensorBase, server_POIA)
+{
+ VectorXd p_state(4), o_state(4), po_std(4);
+ p_state << 1, 2, 3, 4;
+ o_state << 1, 0, 0, 0;
+ po_std << 0.1, 0.2, 0.3, 0.4;
+
+ // Yaml parser
+ ParserYaml parser = ParserYaml("test/yaml/sensor_base/sensor_POIA_3D.yaml", wolf_root, true);
+ ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_POIA_3D");
+ server.print();
// POIA - 3D - CORRECT YAML
WOLF_INFO("Creating sensor from name sensor_POIA_3D");
@@ -422,6 +438,13 @@ TEST(SensorBase, server)
checkSensor(S, 'O', o_state.head(4), true, vector0, false, vector0);
checkSensor(S, 'I', p_state.head(4), false, vector0, true, po_std.head(4));
checkSensor(S, 'A', o_state.head(4), false, po_std.head(3), true, po_std.head(3));
+}
+
+TEST(SensorBase, server_POIA_wrong)
+{
+ // Yaml parser
+ ParserYaml parser = ParserYaml("test/yaml/sensor_base/sensor_POIA_3D_wrong.yaml", wolf_root, true);
+ ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_POIA_3D_wrong");
// POIA - 3D - INCORRECT YAML
WOLF_INFO("Creating sensor from name sensor_POIA_3D_wrong");
@@ -437,8 +460,8 @@ TEST(SensorBase, server)
// DUMMY CLASS
TEST(SensorBase, dummy_make_shared)
{
- ParserYaml parser = ParserYaml("test/yaml/sensor_dummy.yaml", wolf_root, true);
- ParamsServer server = ParamsServer(parser.getParams());
+ ParserYaml parser = ParserYaml("test/yaml/sensor_base/sensor_dummy.yaml", wolf_root, true);
+ ParamsServer server = ParamsServer(parser.getParams(), "sensor/sensor_dummy_1");
VectorXd p_state(4), o_state(4), po_std(4);
p_state << 1, 2, 3, 4;
@@ -463,7 +486,12 @@ TEST(SensorBase, dummy_make_shared)
checkSensor(S, 'O', o_state.head(4), true, vector0, false, vector0);
checkSensor(S, 'I', p_state.head(4), false, vector0, true, po_std.head(4));
checkSensor(S, 'A', o_state.head(4), false, po_std.head(3), true, po_std.head(3));
+}
+TEST(SensorBase, dummy_make_shared_wrong)
+{
+ ParserYaml parser = ParserYaml("test/yaml/sensor_base/sensor_dummy_wrong.yaml", wolf_root, true);
+ ParamsServer server = ParamsServer(parser.getParams(), "sensor/sensor_dummy_1_wrong");
// POIA - 3D - INCORRECT YAML
WOLF_INFO("Creating sensor from name sensor_dummy_1_wrong");
@@ -481,8 +509,8 @@ TEST(SensorBase, dummy_factory)
o_state << 1, 0, 0, 0;
po_std << 0.1, 0.2, 0.3, 0.4;
- ParserYaml parser = ParserYaml("test/yaml/sensor_dummy.yaml", wolf_root, true);
- ParamsServer server = ParamsServer(parser.getParams());
+ ParserYaml parser = ParserYaml("test/yaml/sensor_base/sensor_dummy.yaml", wolf_root, true);
+ ParamsServer server = ParamsServer(parser.getParams(), "sensor/sensor_dummy_1");
// POIA - 3D - CORRECT YAML
WOLF_INFO("Creating sensor from name sensor_dummy_1");
@@ -501,7 +529,12 @@ TEST(SensorBase, dummy_factory)
checkSensor(S, 'O', o_state.head(4), true, vector0, false, vector0);
checkSensor(S, 'I', p_state.head(4), false, vector0, true, po_std.head(4));
checkSensor(S, 'A', o_state.head(4), false, po_std.head(3), true, po_std.head(3));
+}
+TEST(SensorBase, dummy_factory_wrong)
+{
+ ParserYaml parser = ParserYaml("test/yaml/sensor_base/sensor_dummy_wrong.yaml", wolf_root, true);
+ ParamsServer server = ParamsServer(parser.getParams(), "sensor/sensor_dummy_1_wrong");
// POIA - 3D - INCORRECT YAML
WOLF_INFO("Creating sensor from name sensor_dummy_1_wrong");
@@ -520,7 +553,10 @@ TEST(SensorBase, dummy_factory_yaml)
// POIA - 3D - CORRECT YAML
WOLF_INFO("Creating sensor from name sensor_dummy_1");
- auto S = FactorySensorYaml::create("SensorDummy", "sensor_dummy_1", 3, wolf_root + "/test/yaml/sensor_dummy.yaml");
+ auto S = FactorySensorYaml::create("SensorDummy",
+ "sensor_dummy_1",
+ 3,
+ wolf_root + "/test/yaml/sensor_base/sensor_dummy.yaml");
// noise
ASSERT_MATRIX_APPROX(S->getNoiseStd(), po_std.head<2>(), Constants::EPS);
@@ -538,7 +574,10 @@ TEST(SensorBase, dummy_factory_yaml)
// POIA - 3D - INCORRECT YAML
WOLF_INFO("Creating sensor from name sensor_dummy_1_wrong");
- ASSERT_THROW(FactorySensorYaml::create("SensorDummy", "sensor_dummy_1_wrong", 3, wolf_root + "/test/yaml/sensor_dummy.yaml"),
+ ASSERT_THROW(FactorySensorYaml::create("SensorDummy",
+ "sensor_dummy_1_wrong",
+ 3,
+ wolf_root + "/test/yaml/sensor_base/sensor_dummy_wrong.yaml"),
std::runtime_error);
}
diff --git a/test/gtest_sensor_odom_2d.cpp b/test/gtest_sensor_odom_2d.cpp
index c8a072fc1..8fdaf0907 100644
--- a/test/gtest_sensor_odom_2d.cpp
+++ b/test/gtest_sensor_odom_2d.cpp
@@ -43,23 +43,26 @@ Matrix2d noise_cov = noise_std.cwiseAbs2().asDiagonal();
double k_disp_to_disp = 0.5;
double k_rot_to_rot = 0.8;
-void checkSensorOdom2d(SensorOdom2dPtr S,
- const Vector2d& _p_state,
- const Vector1d& _o_state,
- bool _p_fixed,
- bool _o_fixed,
- const VectorXd& _p_noise_std,
- const VectorXd& _o_noise_std,
- bool _p_dynamic,
- bool _o_dynamic,
- const VectorXd& _p_drift_std,
- const VectorXd& _o_drift_std)
+void checkSensorOdom2d(SensorBasePtr S_base,
+ const Vector2d& _p_state,
+ const Vector1d& _o_state,
+ bool _p_fixed,
+ bool _o_fixed,
+ const VectorXd& _p_noise_std,
+ const VectorXd& _o_noise_std,
+ bool _p_dynamic,
+ bool _o_dynamic,
+ const VectorXd& _p_drift_std,
+ const VectorXd& _o_drift_std)
{
MatrixXd p_noise_cov = _p_noise_std.cwiseAbs2().asDiagonal();
MatrixXd o_noise_cov = _o_noise_std.cwiseAbs2().asDiagonal();
MatrixXd p_drift_cov = _p_drift_std.cwiseAbs2().asDiagonal();
MatrixXd o_drift_cov = _o_drift_std.cwiseAbs2().asDiagonal();
+ // cast
+ SensorOdom2dPtr S = std::dynamic_pointer_cast<SensorOdom2d>(S_base);
+ ASSERT_TRUE(S!=nullptr);
// params
ASSERT_NEAR(S->getDispVarToDispNoiseFactor(), k_disp_to_disp, Constants::EPS);
ASSERT_NEAR(S->getRotVarToRotNoiseFactor(), k_rot_to_rot, Constants::EPS);
@@ -200,7 +203,7 @@ TEST(SensorOdom2d, initial_guess_dynamic_drift)
}
// mixed
-TEST(SensorOdom2d, POI_mixed)
+TEST(SensorOdom2d, mixed)
{
auto params = std::make_shared<ParamsSensorOdom2d>();
params->noise_std = noise_std;
@@ -217,123 +220,256 @@ TEST(SensorOdom2d, POI_mixed)
// CONSTRUCTOR WITH PARAM SERVER and KEY_TYPES
TEST(SensorOdom2d, server)
{
- ParserYaml parser = ParserYaml("test/yaml/sensor_odom_2d.yaml", wolf_root, true);
- ParamsServer server = ParamsServer(parser.getParams());
-
std::vector<std::string> modes({"fix", "initial_guess", "factor"});
std::vector<bool> dynamics({false, true});
std::vector<bool> drifts({false, true});
+ std::vector<bool> wrongs({false, true});
// P & O
for (auto mode : modes)
for (auto dynamic : dynamics)
for (auto drift : drifts)
- {
- // nonsense combination
- if (not dynamic and drift)
- continue;
-
- // CORRECT YAML
- std::string name = "sensor_" + mode + (dynamic ? "_dynamic" : "") + (drift ? "_drift" : "");
- WOLF_INFO("Creating sensor from name ", name);
-
- auto params = std::make_shared<ParamsSensorOdom2d>(name, server);
- auto S = std::make_shared<SensorOdom2d>(name, 2, params, server);
-
- auto p_size_std = mode == "factor" ? 2 : 0;
- auto o_size_std = mode == "factor" ? 1 : 0;
- auto p_size_std_drift = drift ? 2 : 0;
- auto o_size_std_drift = drift ? 1 : 0;
-
- // check
- checkSensorOdom2d(S, p_state, o_state,
- mode == "fix", mode == "fix",
- p_std.head(p_size_std), o_std.head(o_size_std),
- dynamic, dynamic,
- p_std.head(p_size_std_drift), o_std.head(o_size_std_drift));
-
- // INCORRECT YAML
- WOLF_INFO("Creating sensor from name ", name + "_wrong");
- ASSERT_THROW(std::make_shared<SensorOdom2d>(name + "_wrong", 2,
- std::make_shared<ParamsSensorOdom2d>(name + "_wrong", server),
- server),
- std::runtime_error);
- }
+ for (auto wrong : wrongs)
+ {
+ // nonsense combination
+ if (not dynamic and drift)
+ continue;
+
+ // Yaml parser
+ std::string name = "sensor_odom_2d_" +
+ mode +
+ (dynamic ? "_dynamic" : "") +
+ (drift ? "_drift" : "") +
+ (wrong ? "_wrong" : "");
+ ParserYaml parser = ParserYaml("test/yaml/sensor_odom_2d/" + name + ".yaml", wolf_root, true);
+ ParamsServer server = ParamsServer(parser.getParams(), "sensor/" + name);
+
+ WOLF_INFO("Creating sensor from name ", name);
+
+ // CORRECT YAML
+ if (not wrong)
+ {
+ auto params = std::make_shared<ParamsSensorOdom2d>(name, server);
+ auto S = std::make_shared<SensorOdom2d>(name, 2, params, server);
+
+ auto p_size_std = mode == "factor" ? 2 : 0;
+ auto o_size_std = mode == "factor" ? 1 : 0;
+ auto p_size_std_drift = drift ? 2 : 0;
+ auto o_size_std_drift = drift ? 1 : 0;
+
+ // check
+ checkSensorOdom2d(S, p_state, o_state,
+ mode == "fix", mode == "fix",
+ p_std.head(p_size_std), o_std.head(o_size_std),
+ dynamic, dynamic,
+ p_std.head(p_size_std_drift), o_std.head(o_size_std_drift));
+ }
+ // INCORRECT YAML
+ else
+ {
+ ASSERT_THROW(std::make_shared<SensorOdom2d>(name + "_wrong", 2,
+ std::make_shared<ParamsSensorOdom2d>(name + "_wrong", server),
+ server),
+ std::runtime_error);
+ }
+ }
// MIXED - CORRECT YAML
- WOLF_INFO("Creating sensor from name sensor_mixed");
-
- auto params = std::make_shared<ParamsSensorOdom2d>("sensor_mixed", server);
- auto S = std::make_shared<SensorOdom2d>("sensor_mixed", 2, params, server);
-
- // check
- checkSensorOdom2d(S,
- p_state, o_state,
- false, true,
- p_std, vector0,
- true, false,
- p_std, vector0);
+ {
+ WOLF_INFO("Creating sensor from name sensor_mixed");
+
+ // Yaml parser
+ ParserYaml parser = ParserYaml("test/yaml/sensor_odom_2d/sensor_odom_2d.yaml", wolf_root, true);
+ ParamsServer server = ParamsServer(parser.getParams(), "sensor/sensor_mixed");
+
+ auto params = std::make_shared<ParamsSensorOdom2d>("sensor_mixed", server);
+ auto S = std::make_shared<SensorOdom2d>("sensor_mixed", 2, params, server);
+
+ // check
+ checkSensorOdom2d(S,
+ p_state, o_state,
+ false, true,
+ p_std, vector0,
+ true, false,
+ p_std, vector0);
+ }
// MIXED - INCORRECT YAML
- WOLF_INFO("Creating sensor from name sensor_mixed_wrong");
+ {
+ WOLF_INFO("Creating sensor from name sensor_mixed_wrong");
+
+ // Yaml parser
+ ParserYaml parser = ParserYaml("test/yaml/sensor_odom_2d/sensor_odom_2d_wrong.yaml", wolf_root, true);
+ ParamsServer server = ParamsServer(parser.getParams(), "sensor/sensor_mixed_wrong");
- ASSERT_THROW(std::make_shared<SensorOdom2d>("sensor_mixed_wrong", 2,
- std::make_shared<ParamsSensorOdom2d>("sensor_mixed_wrong", server),
- server),
- std::runtime_error);
+ ASSERT_THROW(std::make_shared<SensorOdom2d>("sensor_mixed_wrong", 2,
+ std::make_shared<ParamsSensorOdom2d>("sensor_mixed_wrong", server),
+ server),
+ std::runtime_error);
+ }
}
TEST(SensorOdom2d, factory)
{
- ParserYaml parser = ParserYaml("test/yaml/sensor_odom_2d.yaml", wolf_root, true);
- ParamsServer server = ParamsServer(parser.getParams());
+ std::vector<std::string> modes({"fix", "initial_guess", "factor"});
+ std::vector<bool> dynamics({false, true});
+ std::vector<bool> drifts({false, true});
+ std::vector<bool> wrongs({false, true});
+
+ // P & O
+ for (auto mode : modes)
+ for (auto dynamic : dynamics)
+ for (auto drift : drifts)
+ for (auto wrong : wrongs)
+ {
+ // nonsense combination
+ if (not dynamic and drift)
+ continue;
+
+ // Yaml parser
+ std::string name = "sensor_odom_2d_" +
+ mode +
+ (dynamic ? "_dynamic" : "") +
+ (drift ? "_drift" : "") +
+ (wrong ? "_wrong" : "");
+ ParserYaml parser = ParserYaml("test/yaml/sensor_odom_2d/" + name + ".yaml", wolf_root, true);
+ ParamsServer server = ParamsServer(parser.getParams(), "sensor/" + name);
+
+ WOLF_INFO("Creating sensor from name ", name);
+
+ // CORRECT YAML
+ if (not wrong)
+ {
+ auto S = FactorySensor::create("SensorOdom2d", name, 2, server);
+
+ auto p_size_std = mode == "factor" ? 2 : 0;
+ auto o_size_std = mode == "factor" ? 1 : 0;
+ auto p_size_std_drift = drift ? 2 : 0;
+ auto o_size_std_drift = drift ? 1 : 0;
+
+ // check
+ checkSensorOdom2d(S, p_state, o_state,
+ mode == "fix", mode == "fix",
+ p_std.head(p_size_std), o_std.head(o_size_std),
+ dynamic, dynamic,
+ p_std.head(p_size_std_drift), o_std.head(o_size_std_drift));
+ }
+ // INCORRECT YAML
+ else
+ {
+ ASSERT_THROW(FactorySensor::create("SensorOdom2d", name, 2, server),
+ std::runtime_error);
+ }
+ }
- // CORRECT YAML
- WOLF_INFO("Creating sensor from name sensor_mixed");
+ // MIXED - CORRECT YAML
+ {
+ WOLF_INFO("Creating sensor from name sensor_mixed");
- auto S = FactorySensor::create("SensorOdom2d", "sensor_mixed", 2, server);
+ // Yaml parser
+ ParserYaml parser = ParserYaml("test/yaml/sensor_odom_2d/sensor_odom_2d.yaml", wolf_root, true);
+ ParamsServer server = ParamsServer(parser.getParams(), "sensor/sensor_mixed");
- SensorOdom2dPtr S_odom = std::dynamic_pointer_cast<SensorOdom2d>(S);
+ auto S = FactorySensor::create("SensorOdom2d", "sensor_mixed", 2, server);
- // checks
- ASSERT_TRUE(S_odom != nullptr);
- checkSensorOdom2d(S_odom,
- p_state, o_state,
- false, true,
- p_std, vector0,
- true, false,
- p_std, vector0);
-
- // INCORRECT YAML
- WOLF_INFO("Creating sensor from name sensor_mixed_wrong");
-
- ASSERT_THROW(FactorySensor::create("SensorOdom2d", "sensor_mixed_wrong", 2, server),
- std::runtime_error);
+ // check
+ checkSensorOdom2d(S,
+ p_state, o_state,
+ false, true,
+ p_std, vector0,
+ true, false,
+ p_std, vector0);
+ }
+
+ // MIXED - INCORRECT YAML
+ {
+ WOLF_INFO("Creating sensor from name sensor_mixed_wrong");
+
+ // Yaml parser
+ ParserYaml parser = ParserYaml("test/yaml/sensor_odom_2d/sensor_odom_2d_wrong.yaml", wolf_root, true);
+ ParamsServer server = ParamsServer(parser.getParams(), "sensor/sensor_mixed_wrong");
+
+ ASSERT_THROW(FactorySensor::create("SensorOdom2d", "sensor_mixed_wrong", 2, server),
+ std::runtime_error);
+ }
}
TEST(SensorOdom2d, factory_yaml)
{
- // CORRECT YAML
- WOLF_INFO("Creating sensor from name sensor_mixed");
+ std::vector<std::string> modes({"fix", "initial_guess", "factor"});
+ std::vector<bool> dynamics({false, true});
+ std::vector<bool> drifts({false, true});
+ std::vector<bool> wrongs({false, true});
- auto S = FactorySensorYaml::create("SensorOdom2d", "sensor_mixed", 2, wolf_root + "/test/yaml/sensor_odom_2d.yaml");
+ // P & O
+ for (auto mode : modes)
+ for (auto dynamic : dynamics)
+ for (auto drift : drifts)
+ for (auto wrong : wrongs)
+ {
+ // nonsense combination
+ if (not dynamic and drift)
+ continue;
+
+ std::string name = "sensor_odom_2d_" +
+ mode +
+ (dynamic ? "_dynamic" : "") +
+ (drift ? "_drift" : "") +
+ (wrong ? "_wrong" : "");
+
+ WOLF_INFO("Creating sensor from name ", name);
+
+ // CORRECT YAML
+ if (not wrong)
+ {
+ auto S = FactorySensorYaml::create("SensorOdom2d", name, 2,
+ wolf_root + "/test/yaml/sensor_odom_2d/" + name + ".yaml");
+
+ auto p_size_std = mode == "factor" ? 2 : 0;
+ auto o_size_std = mode == "factor" ? 1 : 0;
+ auto p_size_std_drift = drift ? 2 : 0;
+ auto o_size_std_drift = drift ? 1 : 0;
+
+ // check
+ checkSensorOdom2d(S, p_state, o_state,
+ mode == "fix", mode == "fix",
+ p_std.head(p_size_std), o_std.head(o_size_std),
+ dynamic, dynamic,
+ p_std.head(p_size_std_drift), o_std.head(o_size_std_drift));
+ }
+ // INCORRECT YAML
+ else
+ {
+ ASSERT_THROW(FactorySensorYaml::create("SensorOdom2d", name, 2,
+ wolf_root + "/test/yaml/sensor_odom_2d/" + name + ".yaml"),
+ std::runtime_error);
+ }
+ }
+
+ // MIXED - CORRECT YAML
+ {
+ WOLF_INFO("Creating sensor from name sensor_mixed");
+
+ auto S = FactorySensorYaml::create("SensorOdom2d", "sensor_mixed", 2,
+ wolf_root + "/test/yaml/sensor_odom_2d/sensor_odom_2d.yaml");
+
+ // check
+ checkSensorOdom2d(S,
+ p_state, o_state,
+ false, true,
+ p_std, vector0,
+ true, false,
+ p_std, vector0);
+ }
- SensorOdom2dPtr S_odom = std::dynamic_pointer_cast<SensorOdom2d>(S);
+ // MIXED - INCORRECT YAML
+ {
+ WOLF_INFO("Creating sensor from name sensor_mixed_wrong");
- // checks
- ASSERT_TRUE(S_odom != nullptr);
- checkSensorOdom2d(S_odom,
- p_state, o_state,
- false, true,
- p_std, vector0,
- true, false,
- p_std, vector0);
-
- // INCORRECT YAML
- WOLF_INFO("Creating sensor from name sensor_mixed_wrong");
-
- ASSERT_THROW(FactorySensorYaml::create("SensorOdom2d", "sensor_mixed_wrong", 2, wolf_root + "/test/yaml/sensor_odom_2d.yaml"),
- std::runtime_error);
+ ASSERT_THROW(FactorySensorYaml::create("SensorOdom2d", "sensor_mixed_wrong", 2,
+ wolf_root + "/test/yaml/sensor_odom_2d/sensor_odom_2d_wrong.yaml"),
+ std::runtime_error);
+ }
}
int main(int argc, char **argv)
diff --git a/test/yaml/sensor_base/sensor_POIA_3D.yaml b/test/yaml/sensor_base/sensor_POIA_3D.yaml
new file mode 100644
index 000000000..4c04f54c5
--- /dev/null
+++ b/test/yaml/sensor_base/sensor_POIA_3D.yaml
@@ -0,0 +1,23 @@
+# sensor_POIA_3D:
+states:
+ P:
+ mode: factor
+ state: [1, 2, 3]
+ noise_std: [0.1, 0.2, 0.3]
+ dynamic: true
+ O:
+ mode: fix
+ state: [1, 0, 0, 0]
+ dynamic: false
+ I:
+ mode: initial_guess
+ state: [1, 2, 3, 4]
+ dynamic: true
+ drift_std: [0.1, 0.2, 0.3, 0.4]
+ A:
+ mode: factor
+ state: [1, 0, 0, 0]
+ noise_std: [0.1, 0.2, 0.3]
+ dynamic: true
+ drift_std: [0.1, 0.2, 0.3]
+noise_std: [0.1, 0.2]
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_POIA_3D_wrong.yaml b/test/yaml/sensor_base/sensor_POIA_3D_wrong.yaml
new file mode 100644
index 000000000..cd811547a
--- /dev/null
+++ b/test/yaml/sensor_base/sensor_POIA_3D_wrong.yaml
@@ -0,0 +1,23 @@
+# sensor_POIA_3D_wrong:
+states:
+ P:
+ mode: factor
+ state: [1, 2, 3]
+ noise_std: [0.1, 0.2, 0.3]
+ dynamic: true
+ O:
+ mode: fix
+ state: [1, 0, 0, 0]
+ dynamic: false
+ I:
+ mode: initial_guess
+ state: [1, 2, 3, 4]
+ dynamic: true
+ drift_std: [0.1, 0.2, 0.3, 0.4]
+ # A:
+ # mode: factor
+ # state: [1, 0, 0, 0]
+ # noise_std: [0.1, 0.2, 0.3]
+ # dynamic: true
+ # drift_std: [0.1, 0.2, 0.3]
+noise_std: [0.1, 0.2]
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_2D_factor.yaml b/test/yaml/sensor_base/sensor_PO_2D_factor.yaml
new file mode 100644
index 000000000..497085eb8
--- /dev/null
+++ b/test/yaml/sensor_base/sensor_PO_2D_factor.yaml
@@ -0,0 +1,13 @@
+# sensor_PO_2D_factor:
+states:
+ P:
+ mode: factor
+ state: [1, 2]
+ noise_std: [0.1, 0.2]
+ dynamic: false
+ O:
+ mode: factor
+ state: [1]
+ noise_std: [0.1]
+ dynamic: false
+noise_std: [0.1, 0.2]
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_2D_factor_dynamic.yaml b/test/yaml/sensor_base/sensor_PO_2D_factor_dynamic.yaml
new file mode 100644
index 000000000..889d53245
--- /dev/null
+++ b/test/yaml/sensor_base/sensor_PO_2D_factor_dynamic.yaml
@@ -0,0 +1,13 @@
+# sensor_PO_2D_factor_dynamic:
+states:
+ P:
+ mode: factor
+ state: [1, 2]
+ noise_std: [0.1, 0.2]
+ dynamic: true
+ O:
+ mode: factor
+ state: [1]
+ noise_std: [0.1]
+ dynamic: true
+noise_std: [0.1, 0.2]
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_2D_factor_dynamic_drift.yaml b/test/yaml/sensor_base/sensor_PO_2D_factor_dynamic_drift.yaml
new file mode 100644
index 000000000..733ec88fb
--- /dev/null
+++ b/test/yaml/sensor_base/sensor_PO_2D_factor_dynamic_drift.yaml
@@ -0,0 +1,15 @@
+# sensor_PO_2D_factor_dynamic_drift:
+states:
+ P:
+ mode: factor
+ state: [1, 2]
+ noise_std: [0.1, 0.2]
+ dynamic: true
+ drift_std: [0.1, 0.2]
+ O:
+ mode: factor
+ state: [1]
+ noise_std: [0.1]
+ dynamic: true
+ drift_std: [0.1]
+noise_std: [0.1, 0.2]
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_2D_factor_dynamic_drift_wrong.yaml b/test/yaml/sensor_base/sensor_PO_2D_factor_dynamic_drift_wrong.yaml
new file mode 100644
index 000000000..a1d4e79eb
--- /dev/null
+++ b/test/yaml/sensor_base/sensor_PO_2D_factor_dynamic_drift_wrong.yaml
@@ -0,0 +1,15 @@
+# sensor_PO_2D_factor_dynamic_drift_wrong:
+states:
+ P:
+ mode: factor
+ state: [1, 2]
+ noise_std: [0.1, 0.2]
+ dynamic: true
+ drift_std: [0.1] #wrong size
+ O:
+ mode: factor
+ state: [1]
+ noise_std: [0.1]
+ dynamic: true
+ drift_std: [0.1]
+noise_std: [0.1, 0.2]
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_2D_factor_dynamic_wrong.yaml b/test/yaml/sensor_base/sensor_PO_2D_factor_dynamic_wrong.yaml
new file mode 100644
index 000000000..d28cb5f1e
--- /dev/null
+++ b/test/yaml/sensor_base/sensor_PO_2D_factor_dynamic_wrong.yaml
@@ -0,0 +1,13 @@
+# sensor_PO_2D_factor_dynamic_wrong:
+states:
+ P:
+ mode: factor
+ state: [1, 2]
+ noise_std: [0.1, 0.2, 0.3] #wrong size
+ dynamic: true
+ O:
+ mode: factor
+ state: [1]
+ noise_std: [0.1]
+ dynamic: true
+noise_std: [0.1, 0.2]
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_2D_factor_wrong.yaml b/test/yaml/sensor_base/sensor_PO_2D_factor_wrong.yaml
new file mode 100644
index 000000000..c02f9dfbc
--- /dev/null
+++ b/test/yaml/sensor_base/sensor_PO_2D_factor_wrong.yaml
@@ -0,0 +1,13 @@
+# sensor_PO_2D_factor_wrong:
+states:
+ P:
+ mode: factor
+ state: [1, 2]
+ noise_std: [0.1, 0.2]
+ dynamic: false
+ O:
+ mode: factor
+ #state: [1] #missing
+ noise_std: [0.1]
+ dynamic: false
+noise_std: [0.1, 0.2]
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_2D_fix.yaml b/test/yaml/sensor_base/sensor_PO_2D_fix.yaml
new file mode 100644
index 000000000..8d9e96627
--- /dev/null
+++ b/test/yaml/sensor_base/sensor_PO_2D_fix.yaml
@@ -0,0 +1,11 @@
+# sensor_PO_2D_fix:
+states:
+ P:
+ mode: fix
+ state: [1, 2]
+ dynamic: false
+ O:
+ mode: fix
+ state: [1]
+ dynamic: false
+noise_std: [0.1, 0.2]
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_2D_fix_dynamic.yaml b/test/yaml/sensor_base/sensor_PO_2D_fix_dynamic.yaml
new file mode 100644
index 000000000..a20af82a3
--- /dev/null
+++ b/test/yaml/sensor_base/sensor_PO_2D_fix_dynamic.yaml
@@ -0,0 +1,11 @@
+# sensor_PO_2D_fix_dynamic:
+states:
+ P:
+ mode: fix
+ state: [1, 2]
+ dynamic: true
+ O:
+ mode: fix
+ state: [1]
+ dynamic: true
+noise_std: [0.1, 0.2]
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_2D_fix_dynamic_drift.yaml b/test/yaml/sensor_base/sensor_PO_2D_fix_dynamic_drift.yaml
new file mode 100644
index 000000000..410281dbe
--- /dev/null
+++ b/test/yaml/sensor_base/sensor_PO_2D_fix_dynamic_drift.yaml
@@ -0,0 +1,13 @@
+# sensor_PO_2D_fix_dynamic_drift:
+states:
+ P:
+ mode: fix
+ state: [1, 2]
+ dynamic: true
+ drift_std: [0.1, 0.2]
+ O:
+ mode: fix
+ state: [1]
+ dynamic: true
+ drift_std: [0.1]
+noise_std: [0.1, 0.2]
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_2D_fix_dynamic_drift_wrong.yaml b/test/yaml/sensor_base/sensor_PO_2D_fix_dynamic_drift_wrong.yaml
new file mode 100644
index 000000000..2da413660
--- /dev/null
+++ b/test/yaml/sensor_base/sensor_PO_2D_fix_dynamic_drift_wrong.yaml
@@ -0,0 +1,13 @@
+# sensor_PO_2D_fix_dynamic_drift_wrong:
+states:
+ P:
+ mode: fix
+ state: [1, 2]
+ dynamic: true
+ drift_std: [0.1, 0.2, 0.3] #wrong size
+ O:
+ mode: fix
+ state: [1]
+ dynamic: true
+ drift_std: [0.1]
+noise_std: [0.1, 0.2]
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_2D_fix_dynamic_wrong.yaml b/test/yaml/sensor_base/sensor_PO_2D_fix_dynamic_wrong.yaml
new file mode 100644
index 000000000..27affb496
--- /dev/null
+++ b/test/yaml/sensor_base/sensor_PO_2D_fix_dynamic_wrong.yaml
@@ -0,0 +1,11 @@
+# sensor_PO_2D_fix_dynamic_wrong:
+states:
+ P:
+ mode: fix
+ state: [1, 2, 3] # wrong size
+ dynamic: true
+ O:
+ mode: fix
+ state: [1]
+ dynamic: true
+noise_std: [0.1, 0.2]
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_2D_fix_wrong.yaml b/test/yaml/sensor_base/sensor_PO_2D_fix_wrong.yaml
new file mode 100644
index 000000000..10179eae0
--- /dev/null
+++ b/test/yaml/sensor_base/sensor_PO_2D_fix_wrong.yaml
@@ -0,0 +1,11 @@
+# sensor_PO_2D_fix_wrong:
+states:
+ P:
+ mode: fix
+ state: [1, 2]
+ #dynamic: false #missing
+ O:
+ mode: fix
+ state: [1]
+ dynamic: false
+noise_std: [0.1, 0.2]
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_2D_initial_guess.yaml b/test/yaml/sensor_base/sensor_PO_2D_initial_guess.yaml
new file mode 100644
index 000000000..901364dfb
--- /dev/null
+++ b/test/yaml/sensor_base/sensor_PO_2D_initial_guess.yaml
@@ -0,0 +1,11 @@
+# sensor_PO_2D_initial_guess:
+states:
+ P:
+ mode: initial_guess
+ state: [1, 2]
+ dynamic: false
+ O:
+ mode: initial_guess
+ state: [1]
+ dynamic: false
+noise_std: [0.1, 0.2]
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_2D_initial_guess_dynamic.yaml b/test/yaml/sensor_base/sensor_PO_2D_initial_guess_dynamic.yaml
new file mode 100644
index 000000000..0bfebd5b4
--- /dev/null
+++ b/test/yaml/sensor_base/sensor_PO_2D_initial_guess_dynamic.yaml
@@ -0,0 +1,11 @@
+# sensor_PO_2D_initial_guess_dynamic:
+states:
+ P:
+ mode: initial_guess
+ state: [1, 2]
+ dynamic: true
+ O:
+ mode: initial_guess
+ state: [1]
+ dynamic: true
+noise_std: [0.1, 0.2]
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_2D_initial_guess_dynamic_drift.yaml b/test/yaml/sensor_base/sensor_PO_2D_initial_guess_dynamic_drift.yaml
new file mode 100644
index 000000000..ec021b9d4
--- /dev/null
+++ b/test/yaml/sensor_base/sensor_PO_2D_initial_guess_dynamic_drift.yaml
@@ -0,0 +1,13 @@
+# sensor_PO_2D_initial_guess_dynamic_drift:
+states:
+ P:
+ mode: initial_guess
+ state: [1, 2]
+ dynamic: true
+ drift_std: [0.1, 0.2]
+ O:
+ mode: initial_guess
+ state: [1]
+ dynamic: true
+ drift_std: [0.1]
+noise_std: [0.1, 0.2]
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_2D_initial_guess_dynamic_drift_wrong.yaml b/test/yaml/sensor_base/sensor_PO_2D_initial_guess_dynamic_drift_wrong.yaml
new file mode 100644
index 000000000..9e8a172f9
--- /dev/null
+++ b/test/yaml/sensor_base/sensor_PO_2D_initial_guess_dynamic_drift_wrong.yaml
@@ -0,0 +1,13 @@
+# sensor_PO_2D_initial_guess_dynamic_drift_wrong:
+states:
+ P:
+ mode: initial_guess
+ state: [1] #wrong size
+ dynamic: true
+ drift_std: [0.1, 0.2]
+ O:
+ mode: initial_guess
+ state: [1]
+ dynamic: true
+ drift_std: [0.1]
+noise_std: [0.1, 0.2]
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_2D_initial_guess_dynamic_wrong.yaml b/test/yaml/sensor_base/sensor_PO_2D_initial_guess_dynamic_wrong.yaml
new file mode 100644
index 000000000..b4e4c44be
--- /dev/null
+++ b/test/yaml/sensor_base/sensor_PO_2D_initial_guess_dynamic_wrong.yaml
@@ -0,0 +1,11 @@
+# sensor_PO_2D_initial_guess_dynamic_wrong:
+states:
+ # P: #missing
+ # mode: initial_guess
+ # state: [1, 2]
+ # dynamic: true
+ O:
+ mode: initial_guess
+ state: [1]
+ dynamic: true
+noise_std: [0.1, 0.2]
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_2D_initial_guess_wrong.yaml b/test/yaml/sensor_base/sensor_PO_2D_initial_guess_wrong.yaml
new file mode 100644
index 000000000..cb55b4446
--- /dev/null
+++ b/test/yaml/sensor_base/sensor_PO_2D_initial_guess_wrong.yaml
@@ -0,0 +1,11 @@
+# sensor_PO_2D_initial_guess_wrong:
+states:
+ P:
+ mode: initial_guess
+ state: [1, 2]
+ dynamic: false
+ O:
+ #mode: initial_guess #missing
+ state: [1]
+ dynamic: false
+noise_std: [0.1, 0.2]
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_3D_factor.yaml b/test/yaml/sensor_base/sensor_PO_3D_factor.yaml
new file mode 100644
index 000000000..665ac5cd2
--- /dev/null
+++ b/test/yaml/sensor_base/sensor_PO_3D_factor.yaml
@@ -0,0 +1,13 @@
+# sensor_PO_3D_factor:
+states:
+ P:
+ mode: factor
+ state: [1, 2, 3]
+ noise_std: [0.1, 0.2, 0.3]
+ dynamic: false
+ O:
+ mode: factor
+ state: [1, 0, 0, 0]
+ noise_std: [0.1, 0.2, 0.3]
+ dynamic: false
+noise_std: [0.1, 0.2]
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_3D_factor_dynamic.yaml b/test/yaml/sensor_base/sensor_PO_3D_factor_dynamic.yaml
new file mode 100644
index 000000000..b1fc18e12
--- /dev/null
+++ b/test/yaml/sensor_base/sensor_PO_3D_factor_dynamic.yaml
@@ -0,0 +1,13 @@
+# sensor_PO_3D_factor_dynamic:
+states:
+ P:
+ mode: factor
+ state: [1, 2, 3]
+ noise_std: [0.1, 0.2, 0.3]
+ dynamic: true
+ O:
+ mode: factor
+ state: [1, 0, 0, 0]
+ noise_std: [0.1, 0.2, 0.3]
+ dynamic: true
+noise_std: [0.1, 0.2]
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_3D_factor_dynamic_drift.yaml b/test/yaml/sensor_base/sensor_PO_3D_factor_dynamic_drift.yaml
new file mode 100644
index 000000000..f016ed4fd
--- /dev/null
+++ b/test/yaml/sensor_base/sensor_PO_3D_factor_dynamic_drift.yaml
@@ -0,0 +1,15 @@
+# sensor_PO_3D_factor_dynamic_drift:
+states:
+ P:
+ mode: factor
+ state: [1, 2, 3]
+ noise_std: [0.1, 0.2, 0.3]
+ dynamic: true
+ drift_std: [0.1, 0.2, 0.3]
+ O:
+ mode: factor
+ state: [1, 0, 0, 0]
+ noise_std: [0.1, 0.2, 0.3]
+ dynamic: true
+ drift_std: [0.1, 0.2, 0.3]
+noise_std: [0.1, 0.2]
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_3D_factor_dynamic_drift_wrong.yaml b/test/yaml/sensor_base/sensor_PO_3D_factor_dynamic_drift_wrong.yaml
new file mode 100644
index 000000000..89a3409d9
--- /dev/null
+++ b/test/yaml/sensor_base/sensor_PO_3D_factor_dynamic_drift_wrong.yaml
@@ -0,0 +1,15 @@
+# sensor_PO_3D_factor_dynamic_drift_wrong:
+states:
+ P:
+ mode: factor
+ state: [1, 2, 3]
+ noise_std: [0.1, 0.2, 0.3]
+ dynamic: true
+ drift_std: [0.1, 0.2, 0.3]
+ O:
+ mode: factor
+ state: [1, 0, 0, 0]
+ noise_std: [0.1, 0.2, 0.3, 0.4] #wrong size
+ dynamic: true
+ drift_std: [0.1, 0.2, 0.3]
+noise_std: [0.1, 0.2]
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_3D_factor_dynamic_wrong.yaml b/test/yaml/sensor_base/sensor_PO_3D_factor_dynamic_wrong.yaml
new file mode 100644
index 000000000..d2da5482d
--- /dev/null
+++ b/test/yaml/sensor_base/sensor_PO_3D_factor_dynamic_wrong.yaml
@@ -0,0 +1,13 @@
+# sensor_PO_3D_factor_dynamic_wrong:
+states:
+ P:
+ mode: factor
+ state: [1, 2, 3]
+ noise_std: [0.1, 0.2, 0.3]
+ dynamic: true
+ O:
+ mode: factor
+ state: [1, 0, 0, 0]
+ noise_std: [0.1, 0.2, 0.3, 0.4] # wrong size
+ dynamic: true
+noise_std: [0.1, 0.2]
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_3D_factor_wrong.yaml b/test/yaml/sensor_base/sensor_PO_3D_factor_wrong.yaml
new file mode 100644
index 000000000..05bdc260a
--- /dev/null
+++ b/test/yaml/sensor_base/sensor_PO_3D_factor_wrong.yaml
@@ -0,0 +1,13 @@
+# sensor_PO_3D_factor_wrong:
+states:
+ P:
+ mode: factor
+ state: [1, 2, 3]
+ #noise_std: [0.1, 0.2, 0.3] #missing
+ dynamic: false
+ O:
+ mode: factor
+ state: [1, 0, 0, 0]
+ noise_std: [0.1, 0.2, 0.3]
+ dynamic: false
+noise_std: [0.1, 0.2]
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_3D_fix.yaml b/test/yaml/sensor_base/sensor_PO_3D_fix.yaml
new file mode 100644
index 000000000..a9e833381
--- /dev/null
+++ b/test/yaml/sensor_base/sensor_PO_3D_fix.yaml
@@ -0,0 +1,11 @@
+# sensor_PO_3D_fix:
+states:
+ P:
+ mode: fix
+ state: [1, 2, 3]
+ dynamic: false
+ O:
+ mode: fix
+ state: [1, 0, 0, 0]
+ dynamic: false
+noise_std: [0.1, 0.2]
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_3D_fix_dynamic.yaml b/test/yaml/sensor_base/sensor_PO_3D_fix_dynamic.yaml
new file mode 100644
index 000000000..a49037896
--- /dev/null
+++ b/test/yaml/sensor_base/sensor_PO_3D_fix_dynamic.yaml
@@ -0,0 +1,11 @@
+# sensor_PO_3D_fix_dynamic:
+states:
+ P:
+ mode: fix
+ state: [1, 2, 3]
+ dynamic: true
+ O:
+ mode: fix
+ state: [1, 0, 0, 0]
+ dynamic: true
+noise_std: [0.1, 0.2]
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_3D_fix_dynamic_drift.yaml b/test/yaml/sensor_base/sensor_PO_3D_fix_dynamic_drift.yaml
new file mode 100644
index 000000000..23f5848b9
--- /dev/null
+++ b/test/yaml/sensor_base/sensor_PO_3D_fix_dynamic_drift.yaml
@@ -0,0 +1,13 @@
+# sensor_PO_3D_fix_dynamic_drift:
+states:
+ P:
+ mode: fix
+ state: [1, 2, 3]
+ dynamic: true
+ drift_std: [0.1, 0.2, 0.3]
+ O:
+ mode: fix
+ state: [1, 0, 0, 0]
+ dynamic: true
+ drift_std: [0.1, 0.2, 0.3]
+noise_std: [0.1, 0.2]
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_3D_fix_dynamic_drift_wrong.yaml b/test/yaml/sensor_base/sensor_PO_3D_fix_dynamic_drift_wrong.yaml
new file mode 100644
index 000000000..58df603f7
--- /dev/null
+++ b/test/yaml/sensor_base/sensor_PO_3D_fix_dynamic_drift_wrong.yaml
@@ -0,0 +1,13 @@
+# sensor_PO_3D_fix_dynamic_drift_wrong:
+states:
+ P:
+ mode: fix
+ state: [1, 2, 3]
+ dynamic: true
+ drift_std: [0.1, 0.2, 0.3]
+ O:
+ mode: fix
+ state: [1, 0, 0, 0]
+ #dynamic: true #missing
+ drift_std: [0.1, 0.2, 0.3]
+noise_std: [0.1, 0.2]
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_3D_fix_dynamic_wrong.yaml b/test/yaml/sensor_base/sensor_PO_3D_fix_dynamic_wrong.yaml
new file mode 100644
index 000000000..dd89b3f6c
--- /dev/null
+++ b/test/yaml/sensor_base/sensor_PO_3D_fix_dynamic_wrong.yaml
@@ -0,0 +1,11 @@
+# sensor_PO_3D_fix_dynamic_wrong:
+states:
+ P:
+ mode: fix
+ state: [1, 2, 3]
+ dynamic: true
+ O:
+ mode: fix
+ state: [1, 0, 0, 1] # not normalized
+ dynamic: true
+noise_std: [0.1, 0.2]
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_3D_fix_wrong.yaml b/test/yaml/sensor_base/sensor_PO_3D_fix_wrong.yaml
new file mode 100644
index 000000000..498ecdd9e
--- /dev/null
+++ b/test/yaml/sensor_base/sensor_PO_3D_fix_wrong.yaml
@@ -0,0 +1,11 @@
+# sensor_PO_3D_fix_wrong:
+states:
+ P:
+ mode: fix
+ state: [1, 2, 3]
+ dynamic: false
+ O:
+ mode: fix
+ state: [1, 0, 0, 0]
+ dynamic: false
+#noise_std: [0.1, 0.2] #missing
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_3D_initial_guess.yaml b/test/yaml/sensor_base/sensor_PO_3D_initial_guess.yaml
new file mode 100644
index 000000000..9ad6891fb
--- /dev/null
+++ b/test/yaml/sensor_base/sensor_PO_3D_initial_guess.yaml
@@ -0,0 +1,11 @@
+# sensor_PO_3D_initial_guess:
+states:
+ P:
+ mode: initial_guess
+ state: [1, 2, 3]
+ dynamic: false
+ O:
+ mode: initial_guess
+ state: [1, 0, 0, 0]
+ dynamic: false
+noise_std: [0.1, 0.2]
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_3D_initial_guess_dynamic.yaml b/test/yaml/sensor_base/sensor_PO_3D_initial_guess_dynamic.yaml
new file mode 100644
index 000000000..1cf0568a8
--- /dev/null
+++ b/test/yaml/sensor_base/sensor_PO_3D_initial_guess_dynamic.yaml
@@ -0,0 +1,11 @@
+# sensor_PO_3D_initial_guess_dynamic:
+states:
+ P:
+ mode: initial_guess
+ state: [1, 2, 3]
+ dynamic: true
+ O:
+ mode: initial_guess
+ state: [1, 0, 0, 0]
+ dynamic: true
+noise_std: [0.1, 0.2]
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_3D_initial_guess_dynamic_drift.yaml b/test/yaml/sensor_base/sensor_PO_3D_initial_guess_dynamic_drift.yaml
new file mode 100644
index 000000000..bbc3b2558
--- /dev/null
+++ b/test/yaml/sensor_base/sensor_PO_3D_initial_guess_dynamic_drift.yaml
@@ -0,0 +1,13 @@
+# sensor_PO_3D_initial_guess_dynamic_drift:
+states:
+ P:
+ mode: initial_guess
+ state: [1, 2, 3]
+ dynamic: true
+ drift_std: [0.1, 0.2, 0.3]
+ O:
+ mode: initial_guess
+ state: [1, 0, 0, 0]
+ dynamic: true
+ drift_std: [0.1, 0.2, 0.3]
+noise_std: [0.1, 0.2]
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_3D_initial_guess_dynamic_drift_wrong.yaml b/test/yaml/sensor_base/sensor_PO_3D_initial_guess_dynamic_drift_wrong.yaml
new file mode 100644
index 000000000..7c96eb4b4
--- /dev/null
+++ b/test/yaml/sensor_base/sensor_PO_3D_initial_guess_dynamic_drift_wrong.yaml
@@ -0,0 +1,13 @@
+# sensor_PO_3D_initial_guess_dynamic_drift_wrong:
+states:
+ P:
+ mode: initial_guess
+ state: [1, 2, 3]
+ dynamic: true
+ drift_std: [0.1, 0.2, 0.3]
+ O:
+ #mode: initial_guess #missing
+ state: [1, 0, 0, 0]
+ dynamic: true
+ drift_std: [0.1, 0.2, 0.3]
+noise_std: [0.1, 0.2]
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_3D_initial_guess_dynamic_wrong.yaml b/test/yaml/sensor_base/sensor_PO_3D_initial_guess_dynamic_wrong.yaml
new file mode 100644
index 000000000..41d218bea
--- /dev/null
+++ b/test/yaml/sensor_base/sensor_PO_3D_initial_guess_dynamic_wrong.yaml
@@ -0,0 +1,11 @@
+# sensor_PO_3D_initial_guess_dynamic_wrong:
+states:
+ P:
+ mode: initial_guess
+ state: [1, 2] # wrong size
+ dynamic: true
+ O:
+ mode: initial_guess
+ state: [1, 0, 0, 0]
+ dynamic: true
+noise_std: [0.1, 0.2]
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_PO_3D_initial_guess_wrong.yaml b/test/yaml/sensor_base/sensor_PO_3D_initial_guess_wrong.yaml
new file mode 100644
index 000000000..243de825b
--- /dev/null
+++ b/test/yaml/sensor_base/sensor_PO_3D_initial_guess_wrong.yaml
@@ -0,0 +1,11 @@
+# sensor_PO_3D_initial_guess_wrong:
+states:
+ P:
+ mode: initial_guess
+ state: [1, 2, 3]
+ dynamic: false
+ # O: #missing
+ # mode: initial_guess
+ # state: [1, 0, 0, 0]
+ # dynamic: false
+noise_std: [0.1, 0.2]
\ No newline at end of file
diff --git a/test/yaml/sensor_base.yaml b/test/yaml/sensor_base/sensor_all.yaml
similarity index 100%
rename from test/yaml/sensor_base.yaml
rename to test/yaml/sensor_base/sensor_all.yaml
diff --git a/test/yaml/sensor_base/sensor_dummy.yaml b/test/yaml/sensor_base/sensor_dummy.yaml
new file mode 100644
index 000000000..6e1111173
--- /dev/null
+++ b/test/yaml/sensor_base/sensor_dummy.yaml
@@ -0,0 +1,24 @@
+param1: 1.2
+param2: 3
+noise_std: [0.1, 0.2]
+states:
+ P:
+ mode: factor
+ state: [1, 2, 3]
+ noise_std: [0.1, 0.2, 0.3]
+ dynamic: true
+ O:
+ mode: fix
+ state: [1, 0, 0, 0]
+ dynamic: false
+ I:
+ mode: initial_guess
+ state: [1, 2, 3, 4]
+ dynamic: true
+ drift_std: [0.1, 0.2, 0.3, 0.4]
+ A:
+ mode: factor
+ state: [1, 0, 0, 0]
+ noise_std: [0.1, 0.2, 0.3]
+ dynamic: true
+ drift_std: [0.1, 0.2, 0.3]
\ No newline at end of file
diff --git a/test/yaml/sensor_base/sensor_dummy_wrong.yaml b/test/yaml/sensor_base/sensor_dummy_wrong.yaml
new file mode 100644
index 000000000..89467b4cf
--- /dev/null
+++ b/test/yaml/sensor_base/sensor_dummy_wrong.yaml
@@ -0,0 +1,24 @@
+param1: 1.2
+param2: 3
+noise_std: [0.1, 0.2]
+states:
+ P:
+ mode: factor
+ state: [1, 2, 3]
+ noise_std: [0.1, 0.2, 0.3]
+ dynamic: true
+ O:
+ mode: fix
+ state: [1, 0, 0, 0]
+ dynamic: false
+ # I:
+ # mode: initial_guess
+ # state: [1, 2, 3, 4]
+ # dynamic: true
+ # drift_std: [0.1, 0.2, 0.3, 0.4]
+ A:
+ mode: factor
+ state: [1, 0, 0, 0]
+ noise_std: [0.1, 0.2, 0.3]
+ dynamic: true
+ drift_std: [0.1, 0.2, 0.3]
\ No newline at end of file
diff --git a/test/yaml/sensor_dummy.yaml b/test/yaml/sensor_dummy.yaml
deleted file mode 100644
index 6fb39c70f..000000000
--- a/test/yaml/sensor_dummy.yaml
+++ /dev/null
@@ -1,52 +0,0 @@
-sensor:
- sensor_dummy_1:
- param1: 1.2
- param2: 3
- noise_std: [0.1, 0.2]
- states:
- P:
- mode: factor
- state: [1, 2, 3]
- noise_std: [0.1, 0.2, 0.3]
- dynamic: true
- O:
- mode: fix
- state: [1, 0, 0, 0]
- dynamic: false
- I:
- mode: initial_guess
- state: [1, 2, 3, 4]
- dynamic: true
- drift_std: [0.1, 0.2, 0.3, 0.4]
- A:
- mode: factor
- state: [1, 0, 0, 0]
- noise_std: [0.1, 0.2, 0.3]
- dynamic: true
- drift_std: [0.1, 0.2, 0.3]
-
- sensor_dummy_1_wrong:
- param1: 1.2
- param2: 3
- noise_std: [0.1, 0.2]
- states:
- P:
- mode: factor
- state: [1, 2, 3]
- noise_std: [0.1, 0.2, 0.3]
- dynamic: true
- O:
- mode: fix
- state: [1, 0, 0, 0]
- dynamic: false
- # I:
- # mode: initial_guess
- # state: [1, 2, 3, 4]
- # dynamic: true
- # drift_std: [0.1, 0.2, 0.3, 0.4]
- A:
- mode: factor
- state: [1, 0, 0, 0]
- noise_std: [0.1, 0.2, 0.3]
- dynamic: true
- drift_std: [0.1, 0.2, 0.3]
\ No newline at end of file
diff --git a/test/yaml/sensor_odom_2d.yaml b/test/yaml/sensor_odom_2d.yaml
deleted file mode 100644
index 079c3022c..000000000
--- a/test/yaml/sensor_odom_2d.yaml
+++ /dev/null
@@ -1,316 +0,0 @@
-sensor:
-
- #############################################################################################
- ########################################## CORRECT ##########################################
- #############################################################################################
-
- sensor_fix:
- states:
- P:
- mode: fix
- state: [1, 2]
- dynamic: false
- O:
- mode: fix
- state: [3]
- dynamic: false
- noise_std: [0.1, 0.2]
- k_disp_to_disp: 0.5
- k_rot_to_rot: 0.8
-
- sensor_initial_guess:
- states:
- P:
- mode: initial_guess
- state: [1, 2]
- dynamic: false
- O:
- mode: initial_guess
- state: [3]
- dynamic: false
- noise_std: [0.1, 0.2]
- k_disp_to_disp: 0.5
- k_rot_to_rot: 0.8
-
- sensor_factor:
- states:
- P:
- mode: factor
- state: [1, 2]
- noise_std: [0.1, 0.2]
- dynamic: false
- O:
- mode: factor
- state: [3]
- noise_std: [0.3]
- dynamic: false
- noise_std: [0.1, 0.2]
- k_disp_to_disp: 0.5
- k_rot_to_rot: 0.8
-
- sensor_fix_dynamic:
- states:
- P:
- mode: fix
- state: [1, 2]
- dynamic: true
- O:
- mode: fix
- state: [3]
- dynamic: true
- noise_std: [0.1, 0.2]
- k_disp_to_disp: 0.5
- k_rot_to_rot: 0.8
-
- sensor_initial_guess_dynamic:
- states:
- P:
- mode: initial_guess
- state: [1, 2]
- dynamic: true
- O:
- mode: initial_guess
- state: [3]
- dynamic: true
- noise_std: [0.1, 0.2]
- k_disp_to_disp: 0.5
- k_rot_to_rot: 0.8
-
- sensor_factor_dynamic:
- states:
- P:
- mode: factor
- state: [1, 2]
- noise_std: [0.1, 0.2]
- dynamic: true
- O:
- mode: factor
- state: [3]
- noise_std: [0.3]
- dynamic: true
- noise_std: [0.1, 0.2]
- k_disp_to_disp: 0.5
- k_rot_to_rot: 0.8
-
- sensor_fix_dynamic_drift:
- states:
- P:
- mode: fix
- state: [1, 2]
- dynamic: true
- drift_std: [0.1, 0.2]
- O:
- mode: fix
- state: [3]
- dynamic: true
- drift_std: [0.3]
- noise_std: [0.1, 0.2]
- k_disp_to_disp: 0.5
- k_rot_to_rot: 0.8
-
- sensor_initial_guess_dynamic_drift:
- states:
- P:
- mode: initial_guess
- state: [1, 2]
- dynamic: true
- drift_std: [0.1, 0.2]
- O:
- mode: initial_guess
- state: [3]
- dynamic: true
- drift_std: [0.3]
- noise_std: [0.1, 0.2]
- k_disp_to_disp: 0.5
- k_rot_to_rot: 0.8
-
- sensor_factor_dynamic_drift:
- states:
- P:
- mode: factor
- state: [1, 2]
- noise_std: [0.1, 0.2]
- dynamic: true
- drift_std: [0.1, 0.2]
- O:
- mode: factor
- state: [3]
- noise_std: [0.3]
- dynamic: true
- drift_std: [0.3]
- noise_std: [0.1, 0.2]
- k_disp_to_disp: 0.5
- k_rot_to_rot: 0.8
-
- sensor_mixed:
- states:
- P:
- mode: factor
- state: [1, 2]
- noise_std: [0.1, 0.2]
- dynamic: true
- drift_std: [0.1, 0.2]
- O:
- mode: fix
- state: [3]
- dynamic: false
- noise_std: [0.1, 0.2]
- k_disp_to_disp: 0.5
- k_rot_to_rot: 0.8
-
- #############################################################################################
- ######################################### INCORRECT #########################################
- #############################################################################################
-
- sensor_fix_wrong:
- states:
- P:
- mode: fix
- state: [1, 2]
- #dynamic: false #missing
- O:
- mode: fix
- state: [3]
- dynamic: false
- noise_std: [0.1, 0.2]
- k_disp_to_disp: 0.5
- k_rot_to_rot: 0.8
-
- sensor_initial_guess_wrong:
- states:
- P:
- mode: initial_guess
- state: [1, 2]
- dynamic: false
- O:
- mode: initial_guess
- state: [3]
- dynamic: false
- noise_std: [0.1, 0.2]
- #k_disp_to_disp: 0.5 #missing
- k_rot_to_rot: 0.8
-
- sensor_factor_wrong:
- states:
- P:
- mode: factor
- state: [1, 2]
- noise_std: [0.1, 0.2]
- dynamic: false
- O:
- mode: factor
- #state: [3] #missing
- noise_std: [0.3]
- dynamic: false
- noise_std: [0.1, 0.2]
- k_disp_to_disp: 0.5
- k_rot_to_rot: 0.8
-
- sensor_fix_dynamic_wrong:
- states:
- P:
- mode: fix
- state: [1, 2, 3] # wrong size
- dynamic: true
- O:
- mode: fix
- state: [3]
- dynamic: true
- noise_std: [0.1, 0.2]
- k_disp_to_disp: 0.5
- k_rot_to_rot: 0.8
-
- sensor_initial_guess_dynamic_wrong:
- states:
- # P: #missing
- # mode: initial_guess
- # state: [1, 2]
- # dynamic: true
- O:
- mode: initial_guess
- state: [3]
- dynamic: true
- noise_std: [0.1, 0.2]
- k_disp_to_disp: 0.5
- k_rot_to_rot: 0.8
-
- sensor_factor_dynamic_wrong:
- states:
- P:
- mode: factor
- state: [1, 2]
- noise_std: [0.1, 0.2, 0.3] #wrong size
- dynamic: true
- O:
- mode: factor
- state: [3]
- noise_std: [0.3]
- dynamic: true
- noise_std: [0.1, 0.2]
- k_disp_to_disp: 0.5
- k_rot_to_rot: 0.8
-
- sensor_fix_dynamic_drift_wrong:
- states:
- P:
- mode: fix
- state: [1, 2]
- dynamic: true
- drift_std: [0.1, 0.2, 0.3] #wrong size
- O:
- mode: fix
- state: [3]
- dynamic: true
- drift_std: [0.3]
- noise_std: [0.1, 0.2]
- k_disp_to_disp: 0.5
- k_rot_to_rot: 0.8
-
- sensor_initial_guess_dynamic_drift_wrong:
- states:
- P:
- mode: initial_guess
- state: [3] # wrong size
- dynamic: true
- drift_std: [0.1, 0.2]
- O:
- mode: initial_guess
- state: [3]
- dynamic: true
- drift_std: [0.3]
- noise_std: [0.1, 0.2]
- k_disp_to_disp: 0.5
- k_rot_to_rot: 0.8
-
- sensor_factor_dynamic_drift_wrong:
- states:
- P:
- mode: factor
- state: [1, 2]
- noise_std: [0.1, 0.2]
- dynamic: true
- drift_std: [0.3] # wrong size
- O:
- mode: factor
- state: [3]
- noise_std: [0.3]
- dynamic: true
- drift_std: [0.3]
- noise_std: [0.1, 0.2]
- k_disp_to_disp: 0.5
- k_rot_to_rot: 0.8
-
- sensor_mixed_wrong:
- states:
- P:
- mode: factor
- state: [1, 2]
- noise_std: [0.1, 0.2, 0.3] # wrong size
- dynamic: true
- O:
- mode: fix
- state: [3]
- dynamic: false
- noise_std: [0.1, 0.2]
- k_disp_to_disp: 0.5
- k_rot_to_rot: 0.8
\ No newline at end of file
diff --git a/test/yaml/sensor_odom_2d/sensor_odom_2d.yaml b/test/yaml/sensor_odom_2d/sensor_odom_2d.yaml
new file mode 100644
index 000000000..42777646e
--- /dev/null
+++ b/test/yaml/sensor_odom_2d/sensor_odom_2d.yaml
@@ -0,0 +1,14 @@
+states:
+ P:
+ mode: factor
+ state: [1, 2]
+ noise_std: [0.1, 0.2]
+ dynamic: true
+ drift_std: [0.1, 0.2]
+ O:
+ mode: fix
+ state: [3]
+ dynamic: false
+noise_std: [0.1, 0.2]
+k_disp_to_disp: 0.5
+k_rot_to_rot: 0.8
\ No newline at end of file
diff --git a/test/yaml/sensor_odom_2d/sensor_odom_2d_factor.yaml b/test/yaml/sensor_odom_2d/sensor_odom_2d_factor.yaml
new file mode 100644
index 000000000..25318d527
--- /dev/null
+++ b/test/yaml/sensor_odom_2d/sensor_odom_2d_factor.yaml
@@ -0,0 +1,14 @@
+states:
+ P:
+ mode: factor
+ state: [1, 2]
+ noise_std: [0.1, 0.2]
+ dynamic: false
+ O:
+ mode: factor
+ state: [3]
+ noise_std: [0.3]
+ dynamic: false
+noise_std: [0.1, 0.2]
+k_disp_to_disp: 0.5
+k_rot_to_rot: 0.8
\ No newline at end of file
diff --git a/test/yaml/sensor_odom_2d/sensor_odom_2d_factor_dynamic.yaml b/test/yaml/sensor_odom_2d/sensor_odom_2d_factor_dynamic.yaml
new file mode 100644
index 000000000..05b7c5cb1
--- /dev/null
+++ b/test/yaml/sensor_odom_2d/sensor_odom_2d_factor_dynamic.yaml
@@ -0,0 +1,14 @@
+states:
+ P:
+ mode: factor
+ state: [1, 2]
+ noise_std: [0.1, 0.2]
+ dynamic: true
+ O:
+ mode: factor
+ state: [3]
+ noise_std: [0.3]
+ dynamic: true
+noise_std: [0.1, 0.2]
+k_disp_to_disp: 0.5
+k_rot_to_rot: 0.8
\ No newline at end of file
diff --git a/test/yaml/sensor_odom_2d/sensor_odom_2d_factor_dynamic_drift.yaml b/test/yaml/sensor_odom_2d/sensor_odom_2d_factor_dynamic_drift.yaml
new file mode 100644
index 000000000..76d8a9222
--- /dev/null
+++ b/test/yaml/sensor_odom_2d/sensor_odom_2d_factor_dynamic_drift.yaml
@@ -0,0 +1,16 @@
+states:
+ P:
+ mode: factor
+ state: [1, 2]
+ noise_std: [0.1, 0.2]
+ dynamic: true
+ drift_std: [0.1, 0.2]
+ O:
+ mode: factor
+ state: [3]
+ noise_std: [0.3]
+ dynamic: true
+ drift_std: [0.3]
+noise_std: [0.1, 0.2]
+k_disp_to_disp: 0.5
+k_rot_to_rot: 0.8
\ No newline at end of file
diff --git a/test/yaml/sensor_odom_2d/sensor_odom_2d_factor_dynamic_drift_wrong.yaml b/test/yaml/sensor_odom_2d/sensor_odom_2d_factor_dynamic_drift_wrong.yaml
new file mode 100644
index 000000000..b2c4971c1
--- /dev/null
+++ b/test/yaml/sensor_odom_2d/sensor_odom_2d_factor_dynamic_drift_wrong.yaml
@@ -0,0 +1,16 @@
+states:
+ P:
+ mode: factor
+ state: [1, 2]
+ noise_std: [0.1, 0.2]
+ dynamic: true
+ drift_std: [0.3] # wrong size
+ O:
+ mode: factor
+ state: [3]
+ noise_std: [0.3]
+ dynamic: true
+ drift_std: [0.3]
+noise_std: [0.1, 0.2]
+k_disp_to_disp: 0.5
+k_rot_to_rot: 0.8
\ No newline at end of file
diff --git a/test/yaml/sensor_odom_2d/sensor_odom_2d_factor_dynamic_wrong.yaml b/test/yaml/sensor_odom_2d/sensor_odom_2d_factor_dynamic_wrong.yaml
new file mode 100644
index 000000000..7095775fa
--- /dev/null
+++ b/test/yaml/sensor_odom_2d/sensor_odom_2d_factor_dynamic_wrong.yaml
@@ -0,0 +1,14 @@
+states:
+ P:
+ mode: factor
+ state: [1, 2]
+ noise_std: [0.1, 0.2, 0.3] #wrong size
+ dynamic: true
+ O:
+ mode: factor
+ state: [3]
+ noise_std: [0.3]
+ dynamic: true
+noise_std: [0.1, 0.2]
+k_disp_to_disp: 0.5
+k_rot_to_rot: 0.8
\ No newline at end of file
diff --git a/test/yaml/sensor_odom_2d/sensor_odom_2d_factor_wrong.yaml b/test/yaml/sensor_odom_2d/sensor_odom_2d_factor_wrong.yaml
new file mode 100644
index 000000000..b8a86c874
--- /dev/null
+++ b/test/yaml/sensor_odom_2d/sensor_odom_2d_factor_wrong.yaml
@@ -0,0 +1,14 @@
+states:
+ P:
+ mode: factor
+ state: [1, 2]
+ noise_std: [0.1, 0.2]
+ dynamic: false
+ O:
+ mode: factor
+ #state: [3] #missing
+ noise_std: [0.3]
+ dynamic: false
+noise_std: [0.1, 0.2]
+k_disp_to_disp: 0.5
+k_rot_to_rot: 0.8
\ No newline at end of file
diff --git a/test/yaml/sensor_odom_2d/sensor_odom_2d_fix.yaml b/test/yaml/sensor_odom_2d/sensor_odom_2d_fix.yaml
new file mode 100644
index 000000000..16adc66ac
--- /dev/null
+++ b/test/yaml/sensor_odom_2d/sensor_odom_2d_fix.yaml
@@ -0,0 +1,12 @@
+states:
+ P:
+ mode: fix
+ state: [1, 2]
+ dynamic: false
+ O:
+ mode: fix
+ state: [3]
+ dynamic: false
+noise_std: [0.1, 0.2]
+k_disp_to_disp: 0.5
+k_rot_to_rot: 0.8
diff --git a/test/yaml/sensor_odom_2d/sensor_odom_2d_fix_dynamic.yaml b/test/yaml/sensor_odom_2d/sensor_odom_2d_fix_dynamic.yaml
new file mode 100644
index 000000000..fd24b509f
--- /dev/null
+++ b/test/yaml/sensor_odom_2d/sensor_odom_2d_fix_dynamic.yaml
@@ -0,0 +1,12 @@
+states:
+ P:
+ mode: fix
+ state: [1, 2]
+ dynamic: true
+ O:
+ mode: fix
+ state: [3]
+ dynamic: true
+noise_std: [0.1, 0.2]
+k_disp_to_disp: 0.5
+k_rot_to_rot: 0.8
\ No newline at end of file
diff --git a/test/yaml/sensor_odom_2d/sensor_odom_2d_fix_dynamic_drift.yaml b/test/yaml/sensor_odom_2d/sensor_odom_2d_fix_dynamic_drift.yaml
new file mode 100644
index 000000000..a4b81b490
--- /dev/null
+++ b/test/yaml/sensor_odom_2d/sensor_odom_2d_fix_dynamic_drift.yaml
@@ -0,0 +1,14 @@
+states:
+ P:
+ mode: fix
+ state: [1, 2]
+ dynamic: true
+ drift_std: [0.1, 0.2]
+ O:
+ mode: fix
+ state: [3]
+ dynamic: true
+ drift_std: [0.3]
+noise_std: [0.1, 0.2]
+k_disp_to_disp: 0.5
+k_rot_to_rot: 0.8
diff --git a/test/yaml/sensor_odom_2d/sensor_odom_2d_fix_dynamic_drift_wrong.yaml b/test/yaml/sensor_odom_2d/sensor_odom_2d_fix_dynamic_drift_wrong.yaml
new file mode 100644
index 000000000..0c5beb28c
--- /dev/null
+++ b/test/yaml/sensor_odom_2d/sensor_odom_2d_fix_dynamic_drift_wrong.yaml
@@ -0,0 +1,14 @@
+states:
+ P:
+ mode: fix
+ state: [1, 2]
+ dynamic: true
+ drift_std: [0.1, 0.2, 0.3] #wrong size
+ O:
+ mode: fix
+ state: [3]
+ dynamic: true
+ drift_std: [0.3]
+noise_std: [0.1, 0.2]
+k_disp_to_disp: 0.5
+k_rot_to_rot: 0.8
diff --git a/test/yaml/sensor_odom_2d/sensor_odom_2d_fix_dynamic_wrong.yaml b/test/yaml/sensor_odom_2d/sensor_odom_2d_fix_dynamic_wrong.yaml
new file mode 100644
index 000000000..ca9746a49
--- /dev/null
+++ b/test/yaml/sensor_odom_2d/sensor_odom_2d_fix_dynamic_wrong.yaml
@@ -0,0 +1,12 @@
+states:
+ P:
+ mode: fix
+ state: [1, 2, 3] # wrong size
+ dynamic: true
+ O:
+ mode: fix
+ state: [3]
+ dynamic: true
+noise_std: [0.1, 0.2]
+k_disp_to_disp: 0.5
+k_rot_to_rot: 0.8
\ No newline at end of file
diff --git a/test/yaml/sensor_odom_2d/sensor_odom_2d_fix_wrong.yaml b/test/yaml/sensor_odom_2d/sensor_odom_2d_fix_wrong.yaml
new file mode 100644
index 000000000..d927120f2
--- /dev/null
+++ b/test/yaml/sensor_odom_2d/sensor_odom_2d_fix_wrong.yaml
@@ -0,0 +1,12 @@
+states:
+ P:
+ mode: fix
+ state: [1, 2]
+ #dynamic: false #missing
+ O:
+ mode: fix
+ state: [3]
+ dynamic: false
+noise_std: [0.1, 0.2]
+k_disp_to_disp: 0.5
+k_rot_to_rot: 0.8
diff --git a/test/yaml/sensor_odom_2d/sensor_odom_2d_initial_guess.yaml b/test/yaml/sensor_odom_2d/sensor_odom_2d_initial_guess.yaml
new file mode 100644
index 000000000..7d8038b84
--- /dev/null
+++ b/test/yaml/sensor_odom_2d/sensor_odom_2d_initial_guess.yaml
@@ -0,0 +1,12 @@
+states:
+ P:
+ mode: initial_guess
+ state: [1, 2]
+ dynamic: false
+ O:
+ mode: initial_guess
+ state: [3]
+ dynamic: false
+noise_std: [0.1, 0.2]
+k_disp_to_disp: 0.5
+k_rot_to_rot: 0.8
\ No newline at end of file
diff --git a/test/yaml/sensor_odom_2d/sensor_odom_2d_initial_guess_dynamic.yaml b/test/yaml/sensor_odom_2d/sensor_odom_2d_initial_guess_dynamic.yaml
new file mode 100644
index 000000000..547c9ea89
--- /dev/null
+++ b/test/yaml/sensor_odom_2d/sensor_odom_2d_initial_guess_dynamic.yaml
@@ -0,0 +1,12 @@
+states:
+ P:
+ mode: initial_guess
+ state: [1, 2]
+ dynamic: true
+ O:
+ mode: initial_guess
+ state: [3]
+ dynamic: true
+noise_std: [0.1, 0.2]
+k_disp_to_disp: 0.5
+k_rot_to_rot: 0.8
diff --git a/test/yaml/sensor_odom_2d/sensor_odom_2d_initial_guess_dynamic_drift.yaml b/test/yaml/sensor_odom_2d/sensor_odom_2d_initial_guess_dynamic_drift.yaml
new file mode 100644
index 000000000..693e91bc2
--- /dev/null
+++ b/test/yaml/sensor_odom_2d/sensor_odom_2d_initial_guess_dynamic_drift.yaml
@@ -0,0 +1,14 @@
+states:
+ P:
+ mode: initial_guess
+ state: [1, 2]
+ dynamic: true
+ drift_std: [0.1, 0.2]
+ O:
+ mode: initial_guess
+ state: [3]
+ dynamic: true
+ drift_std: [0.3]
+noise_std: [0.1, 0.2]
+k_disp_to_disp: 0.5
+k_rot_to_rot: 0.8
diff --git a/test/yaml/sensor_odom_2d/sensor_odom_2d_initial_guess_dynamic_drift_wrong.yaml b/test/yaml/sensor_odom_2d/sensor_odom_2d_initial_guess_dynamic_drift_wrong.yaml
new file mode 100644
index 000000000..49d6a56c2
--- /dev/null
+++ b/test/yaml/sensor_odom_2d/sensor_odom_2d_initial_guess_dynamic_drift_wrong.yaml
@@ -0,0 +1,14 @@
+states:
+ P:
+ mode: initial_guess
+ state: [3] # wrong size
+ dynamic: true
+ drift_std: [0.1, 0.2]
+ O:
+ mode: initial_guess
+ state: [3]
+ dynamic: true
+ drift_std: [0.3]
+noise_std: [0.1, 0.2]
+k_disp_to_disp: 0.5
+k_rot_to_rot: 0.8
\ No newline at end of file
diff --git a/test/yaml/sensor_odom_2d/sensor_odom_2d_initial_guess_dynamic_wrong.yaml b/test/yaml/sensor_odom_2d/sensor_odom_2d_initial_guess_dynamic_wrong.yaml
new file mode 100644
index 000000000..4dcf14f13
--- /dev/null
+++ b/test/yaml/sensor_odom_2d/sensor_odom_2d_initial_guess_dynamic_wrong.yaml
@@ -0,0 +1,12 @@
+states:
+ # P: #missing
+ # mode: initial_guess
+ # state: [1, 2]
+ # dynamic: true
+ O:
+ mode: initial_guess
+ state: [3]
+ dynamic: true
+noise_std: [0.1, 0.2]
+k_disp_to_disp: 0.5
+k_rot_to_rot: 0.8
\ No newline at end of file
diff --git a/test/yaml/sensor_odom_2d/sensor_odom_2d_initial_guess_wrong.yaml b/test/yaml/sensor_odom_2d/sensor_odom_2d_initial_guess_wrong.yaml
new file mode 100644
index 000000000..d6e901058
--- /dev/null
+++ b/test/yaml/sensor_odom_2d/sensor_odom_2d_initial_guess_wrong.yaml
@@ -0,0 +1,12 @@
+states:
+ P:
+ mode: initial_guess
+ state: [1, 2]
+ dynamic: false
+ O:
+ mode: initial_guess
+ state: [3]
+ dynamic: false
+noise_std: [0.1, 0.2]
+#k_disp_to_disp: 0.5 #missing
+k_rot_to_rot: 0.8
\ No newline at end of file
diff --git a/test/yaml/sensor_odom_2d/sensor_odom_2d_wrong.yaml b/test/yaml/sensor_odom_2d/sensor_odom_2d_wrong.yaml
new file mode 100644
index 000000000..7b1023e84
--- /dev/null
+++ b/test/yaml/sensor_odom_2d/sensor_odom_2d_wrong.yaml
@@ -0,0 +1,13 @@
+states:
+ P:
+ mode: factor
+ state: [1, 2]
+ noise_std: [0.1, 0.2, 0.3] # wrong size
+ dynamic: true
+ O:
+ mode: fix
+ state: [3]
+ dynamic: false
+noise_std: [0.1, 0.2]
+k_disp_to_disp: 0.5
+k_rot_to_rot: 0.8
\ No newline at end of file
--
GitLab