diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 5f55b72dded65866629a550f8307910d9f50d755..48eddca9fe75da37f142e5c84af507c5d5f98df4 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 b1562a90678b17dfab4b1ea3f3839f8f09c93144..2ca64f4cc97f11e81616ca716835be2b4681b8d8 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 dc064e6b7d6b234132e2ece6127246811d23aa7e..f624a36dfced0a59a0f26e9aa3bac6d3df3240fc 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 b8f2475bde3fcbbf4ceadf246814d96310a883fd..249dabd38183def44e8339129405fcc417e2ec98 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 76a9a0d424357f763819a8d24ded2034bb3a6f10..06e1430b2f0f58a0c9132014c246abdd6b048307 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 c8a072fc1fb927bd7c582c76c89c0a165d01bdd5..8fdaf0907eff360158d905b07d09c4d021012ba1 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 0000000000000000000000000000000000000000..4c04f54c51c1c9edaec2feb5983f6e3285644169
--- /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 0000000000000000000000000000000000000000..cd811547a5f5e9199a53eebf789f682771d0eec1
--- /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 0000000000000000000000000000000000000000..497085eb8492e27cb57574d23e4820f21e2dc270
--- /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 0000000000000000000000000000000000000000..889d53245c286e14e25b532d507f265e925a699f
--- /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 0000000000000000000000000000000000000000..733ec88fbc29f6e02eb9877c07c1fd5fd315c9ac
--- /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 0000000000000000000000000000000000000000..a1d4e79eb53fee4e8f7d1fac009c41b14c5ced5f
--- /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 0000000000000000000000000000000000000000..d28cb5f1eb471e36d4ac355652c39b5822fe3b68
--- /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 0000000000000000000000000000000000000000..c02f9dfbc17c4048cf7e25b8d5647b5868e701fa
--- /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 0000000000000000000000000000000000000000..8d9e966274c905a10a591d802d74c3ad1c9938d4
--- /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 0000000000000000000000000000000000000000..a20af82a31cb5fdd67d926d3f29181c6277ca857
--- /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 0000000000000000000000000000000000000000..410281dbe08d60cc61099a8c006f0b86d1e080b1
--- /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 0000000000000000000000000000000000000000..2da413660cf9106e33c6ded18e71fdd2331075b5
--- /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 0000000000000000000000000000000000000000..27affb496a0e4f213e7911b8dcf32ec5d5bf5c7a
--- /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 0000000000000000000000000000000000000000..10179eae0fe1b151eff97e26a6e60656ac35c0c8
--- /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 0000000000000000000000000000000000000000..901364dfbbe896443bb1e5d074de17c6085006ec
--- /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 0000000000000000000000000000000000000000..0bfebd5b4bdadb875db5c819cec4baf5cc84245c
--- /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 0000000000000000000000000000000000000000..ec021b9d46f3c13df799735aa21864fec28bfc75
--- /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 0000000000000000000000000000000000000000..9e8a172f98e5fcfd3e5fc87a29231c6c692c4c4a
--- /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 0000000000000000000000000000000000000000..b4e4c44be2745e69a11aa50bf3b278c79e893b81
--- /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 0000000000000000000000000000000000000000..cb55b4446ea8e0ef37b9d7a92d5bfd94006f5fbe
--- /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 0000000000000000000000000000000000000000..665ac5cd2e2b14f860fbabac39dced993fac19c4
--- /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 0000000000000000000000000000000000000000..b1fc18e127bcfabaa088f4126e6ccb3d9dfa7776
--- /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 0000000000000000000000000000000000000000..f016ed4fda8150e54da25c0c8fea9d7650095b5a
--- /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 0000000000000000000000000000000000000000..89a3409d9f5ebf9a1e7168b61b36173e56f4cdd4
--- /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 0000000000000000000000000000000000000000..d2da5482ddf25cf5ae99ecc4cf4fbbd9b2308689
--- /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 0000000000000000000000000000000000000000..05bdc260a54a6909a6deea2df0a6d7895d0e755d
--- /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 0000000000000000000000000000000000000000..a9e83338120ffdbbe7d8b6ae9a3160242b102504
--- /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 0000000000000000000000000000000000000000..a490378968b7e6540ca71460bb907f4d9b727dce
--- /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 0000000000000000000000000000000000000000..23f5848b90ab20eaad617cfd934b00de31d2fcca
--- /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 0000000000000000000000000000000000000000..58df603f77ebb76be2e9c8661f7822e8d5ac4c78
--- /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 0000000000000000000000000000000000000000..dd89b3f6cdce6c844b645fef4a8b2ff977e2ad30
--- /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 0000000000000000000000000000000000000000..498ecdd9e0721ec413e0adc780c1094d2064a3eb
--- /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 0000000000000000000000000000000000000000..9ad6891fb7fcf77e5ea0e29fa1f8646eff9e3f7b
--- /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 0000000000000000000000000000000000000000..1cf0568a8635bad5454db769918940c3372667bc
--- /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 0000000000000000000000000000000000000000..bbc3b2558933466d8c028ab3ff1291133638ec85
--- /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 0000000000000000000000000000000000000000..7c96eb4b42504919b4f01b12901c90968471dba0
--- /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 0000000000000000000000000000000000000000..41d218bea2adb73f73a66f0637c3ac982482bf73
--- /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 0000000000000000000000000000000000000000..243de825be306bfbcba7b4495ffff74401896b1e
--- /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 0000000000000000000000000000000000000000..6e1111173262fa4a5fc6a61d25e18b8cb335103f
--- /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 0000000000000000000000000000000000000000..89467b4cfd264014428152d4704d0f5ef570d13a
--- /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 6fb39c70fe6506e0c822fe29b9ddcc57f0a02c5d..0000000000000000000000000000000000000000
--- 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 079c3022ccae53c510fea589e9c6827f27174155..0000000000000000000000000000000000000000
--- 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 0000000000000000000000000000000000000000..42777646ea7c3a31a2ef78cbfab707bcc77a5a4d
--- /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 0000000000000000000000000000000000000000..25318d527db4bae12bdfe01e1248a252e72671fc
--- /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 0000000000000000000000000000000000000000..05b7c5cb1904fd45867467585b25b3570959551b
--- /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 0000000000000000000000000000000000000000..76d8a922249aa941d36f4cf31e220501bdd71459
--- /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 0000000000000000000000000000000000000000..b2c4971c1dba5771184623eba88780240229c0ea
--- /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 0000000000000000000000000000000000000000..7095775fa1327b141db927575769124b3dd8880f
--- /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 0000000000000000000000000000000000000000..b8a86c874abd41bb90352c1e5b9a9a6d7c69da0d
--- /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 0000000000000000000000000000000000000000..16adc66acf10995f74007686e2722bb5a7518f7c
--- /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 0000000000000000000000000000000000000000..fd24b509f20858576a94089a123e9272973530bb
--- /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 0000000000000000000000000000000000000000..a4b81b49058b477ed153bd9f6610a6e9cf02d38e
--- /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 0000000000000000000000000000000000000000..0c5beb28c659ec0e5dbd929597fbacfeff40d748
--- /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 0000000000000000000000000000000000000000..ca9746a491bb1a82e32f7d2f0d677a7f600d5d8c
--- /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 0000000000000000000000000000000000000000..d927120f2690d8f1e42ce2cb5dd66e57bbd770bd
--- /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 0000000000000000000000000000000000000000..7d8038b84aeb2abf4acd1f7cc5e53bc6c2cb8be4
--- /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 0000000000000000000000000000000000000000..547c9ea892375464645b6723c5b6e257f2cad0fa
--- /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 0000000000000000000000000000000000000000..693e91bc24ccdb0e1d8ff0a82764cfa64e28d50f
--- /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 0000000000000000000000000000000000000000..49d6a56c2df7915a0704ca9f24fc669dd261bcff
--- /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 0000000000000000000000000000000000000000..4dcf14f132cea51f61daa6f15621857fb1a4b760
--- /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 0000000000000000000000000000000000000000..d6e901058b599ca4db1d8275df37919759c4d428
--- /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 0000000000000000000000000000000000000000..7b1023e84d563a3d71f54a76c0ccf63a3b4e5e6d
--- /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