diff --git a/include/core/common/factory.h b/include/core/common/factory.h
index 1e18985f9a047dea575af0852b24eac6ff6305bd..cc55ef5229a4ab876d4f4b845bfe294c4d5b2aec 100644
--- a/include/core/common/factory.h
+++ b/include/core/common/factory.h
@@ -329,7 +329,7 @@ class Factory
template<class TypeBase, typename... TypeInput>
inline Factory<TypeBase, TypeInput...>::~Factory<TypeBase, TypeInput...>()
{
- std::cout << " Factory destructor " << this->getClass() << std::endl;
+ //std::cout << " Factory destructor " << this->getClass() << std::endl;
}
template<class TypeBase, typename... TypeInput>
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 1b76c1b4800156c25a1bf932c6721456d6055e91..1157e55c2a5613a3bf3e43fdc5bcfca4fc275103 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -141,7 +141,9 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
*
* Constructor with parameter vector
* \param _tp Type of the sensor (types defined at wolf.h)
- * \param _priors priors of the sensor states
+ * \param _unique_name Name of the sensor
+ * \param _dim Problem dimension
+ * \param _priors priors of the sensor state blocks
* \param _params params struct
*
**/
@@ -154,9 +156,12 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
/** \brief Constructor with ParamServer and keys
*
* Constructor with parameter vector
+ * TODO: update
* \param _tp Type of the sensor (types defined at wolf.h)
+ * \param _unique_name Name of the sensor
+ * \param _dim Problem dimension
* \param _server parameter server
- * \param _keys keys of the states of the derived sensor
+ * \param _keys_types_apart_from_PO Map containing the keys and the types of the state blocks (apart from extrinsics: P, O)
*
**/
SensorBase(const std::string& _type,
@@ -222,6 +227,11 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
StateBlockConstPtr getIntrinsic() const;
StateBlockPtr getIntrinsic();
+ std::map<char, FeatureBaseConstPtr> getPriorFeatures() const;
+ std::map<char, FeatureBasePtr> getPriorFeatures();
+ FeatureBaseConstPtr getPriorFeature(char) const;
+ FeatureBasePtr getPriorFeature(char);
+
protected:
virtual void registerNewStateBlocks(ProblemPtr _problem) override;
@@ -257,8 +267,8 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
void setNoiseStd(const Eigen::VectorXd & _noise_std);
void setNoiseCov(const Eigen::MatrixXd & _noise_cov);
- void setDriftStd(char, const Eigen::VectorXd & _drift_rate_std);
- void setDriftCov(char, const Eigen::MatrixXd & _drift_rate_cov);
+ void setDriftStd(char _key, const Eigen::VectorXd & _drift_rate_std);
+ void setDriftCov(char _key, const Eigen::MatrixXd & _drift_rate_cov);
Eigen::VectorXd getNoiseStd() const;
Eigen::MatrixXd getNoiseCov() const;
Eigen::VectorXd getDriftStd(char) const;
@@ -365,7 +375,7 @@ inline Eigen::MatrixXd SensorBase::getNoiseCov() const
inline Eigen::VectorXd SensorBase::getDriftStd(char _key) const
{
if (drift_covs_.count(_key) == 0)
- return Eigen::MatrixXd(0,0);
+ return Eigen::VectorXd(0);
else
return drift_covs_.at(_key).diagonal().cwiseSqrt();
}
@@ -378,9 +388,6 @@ inline Eigen::MatrixXd SensorBase::getDriftCov(char _key) const
return drift_covs_.at(_key);
}
-void setDriftRateStd(char, const Eigen::VectorXd & _drift_rate_std);
-void setDriftRateCov(char, const Eigen::MatrixXd & _drift_rate_cov);
-
inline HardwareBaseConstPtr SensorBase::getHardware() const
{
return hardware_ptr_.lock();
@@ -411,6 +418,35 @@ inline void SensorBase::addPriorIntrinsics(const Eigen::VectorXd& _x, const Eige
addPriorParameter('I', _x, _cov, _start_idx);
}
+inline std::map<char, FeatureBaseConstPtr> SensorBase::getPriorFeatures() const
+{
+ std::map<char, FeatureBaseConstPtr> map_const;
+ for (auto&& pair : params_prior_map_)
+ map_const[pair.first] = pair.second;
+ return map_const;
+}
+
+inline std::map<char, FeatureBasePtr> SensorBase::getPriorFeatures()
+{
+ return params_prior_map_;
+}
+
+
+inline FeatureBaseConstPtr SensorBase::getPriorFeature(char key) const
+{
+ if (params_prior_map_.count(key) == 0)
+ return nullptr;
+ return params_prior_map_.at(key);
+}
+
+inline FeatureBasePtr SensorBase::getPriorFeature(char key)
+{
+ if (params_prior_map_.count(key) == 0)
+ return nullptr;
+ return params_prior_map_.at(key);
+}
+
+
} // namespace wolf
#endif
diff --git a/include/core/state_block/prior.h b/include/core/state_block/prior.h
index 1dc7b33839282579e412bc0928a882d28cb121a8..38bce67d8e53439f78122d9bcca370cebb29f433 100644
--- a/include/core/state_block/prior.h
+++ b/include/core/state_block/prior.h
@@ -38,8 +38,8 @@ class Prior
{
private:
std::string type_; // State type
- std::string mode_; // Prior mode. Can be 'initial_guess', 'fix' or 'factor'
Eigen::VectorXd state_; // state values
+ std::string mode_; // Prior mode. Can be 'initial_guess', 'fix' or 'factor'
Eigen::VectorXd noise_std_; // factor noise std, sqrt of the diagonal of the covariance mtrix (ONLY IF mode == 'factor')
bool dynamic_; // State dynamic
Eigen::VectorXd drift_std_; // OPTIONAL std of the state drift [in units/sqrt(s)] diagonal elements (ONLY IF if dynamic)
diff --git a/include/core/utils/converter.h b/include/core/utils/converter.h
index 1c53906c6c624ef4e88ac95785fa1871b9b5c122..0e2fd206dc03fdc325bf1f901d0294a1b8bdc44a 100644
--- a/include/core/utils/converter.h
+++ b/include/core/utils/converter.h
@@ -195,6 +195,15 @@ struct converter<std::string>{
if (result.size() > 0) result = result.substr(1, result.size());
return "[" + result + "]";
}
+ template<typename A>
+ static std::string convert(std::set<A> val){
+ std::string result = "";
+ for(auto it : val){
+ result += "," + converter<std::string>::convert(it);
+ }
+ if(result.size() > 0) result = result.substr(1,result.size());
+ return "[" + result + "]";
+ }
template<typename A, typename B>
static std::string convert(std::pair<A,B> val){
return "{" + converter<std::string>::convert(val.first) + ":" + converter<std::string>::convert(val.second) + "}";
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 9b8f70f7ba6b6943c4a45d04386ea4cedb1042c3..ecd199c2ef098d4e942dc777018226938b30432e 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -44,7 +44,8 @@ SensorBase::SensorBase(const std::string& _type,
sensor_id_(++sensor_id_count_), // simple ID factory
state_block_dynamic_(),
params_prior_map_(),
- last_capture_(nullptr)
+ last_capture_(nullptr),
+ noise_cov_(0,0)
{
assert(_dim == 2 or _dim == 3);
@@ -68,6 +69,7 @@ SensorBase::SensorBase(const std::string& _type,
params_prior_map_(),
last_capture_(nullptr)
{
+ assert(_dim == 2 or _dim == 3);
assert(_keys_types_apart_from_PO.count('P') == 0 and
_keys_types_apart_from_PO.count('O') == 0 and
"SensorBase: _keys_types_apart_from_PO should not contain 'P' or 'O' keys");
@@ -84,7 +86,7 @@ SensorBase::SensorBase(const std::string& _type,
keys_types['O'] = "O"; // equivalent to (_dim == 2 ? "StateAngle" : "StateQuaternion");
Priors priors;
for (auto pair : keys_types)
- priors[pair.first] = Prior("sensor/" + _unique_name, pair.second, _server);
+ priors[pair.first] = Prior("sensor/" + _unique_name + "/" + std::string(1, pair.first), pair.second, _server);
// load priors
loadPriors(priors, _dim);
@@ -142,7 +144,7 @@ void SensorBase::loadPriors(const Priors& _priors, SizeEigen _dim)
// Drift covariance rates
if (prior.isDynamic())
- setDriftStd(key, prior.getDriftStd().cwiseAbs2().asDiagonal());
+ setDriftStd(key, prior.getDriftStd());
}
if (not hasStateBlock('P') or not hasStateBlock('O'))
@@ -284,7 +286,7 @@ void SensorBase::registerNewStateBlocks(ProblemPtr _problem)
void SensorBase::setNoiseStd(const Eigen::VectorXd& _noise_std)
{
- noise_cov_ = _noise_std.array().square().matrix().asDiagonal();
+ noise_cov_ = _noise_std.cwiseAbs2().asDiagonal();
}
void SensorBase::setNoiseCov(const Eigen::MatrixXd& _noise_cov)
diff --git a/src/state_block/prior.cpp b/src/state_block/prior.cpp
index 3849ccb814b9fc2aad7ed3a36ab289fb7e86f621..a58824ece6d2e3fb0a986d1a1fa690d734cbd97c 100644
--- a/src/state_block/prior.cpp
+++ b/src/state_block/prior.cpp
@@ -32,14 +32,14 @@ Prior::Prior(const std::string& _type,
const Eigen::VectorXd& _noise_std,
bool _dynamic,
const Eigen::VectorXd& _drift_std)
- : type_(_type), mode_(_mode), state_(_state), noise_std_(_noise_std), dynamic_(_dynamic), drift_std_(_drift_std)
+ : type_(_type), state_(_state), mode_(_mode), noise_std_(_noise_std), dynamic_(_dynamic), drift_std_(_drift_std)
{
check();
}
-Prior::Prior(const std::string& _prefix, const std::string& _type, const ParamsServer& _server)
+Prior::Prior(const std::string& _prefix, const std::string& _type, const ParamsServer& _server)
+ : type_(_type)
{
- type_ = _type;
state_ = _server.getParam<Eigen::VectorXd>(_prefix + "/state");
mode_ = _server.getParam<std::string>(_prefix + "/mode");
@@ -64,6 +64,10 @@ void Prior::check() const
throw std::runtime_error("Prior::check() wrong mode value: " + mode_ +
", it should be: 'initial_guess', 'fix' or 'factor'. " + print());
+ // check empty state
+ if (state_.size() == 0)
+ throw std::runtime_error("Prior::check() Prior " + type_ + " state size zero. " + print());
+
// try to create a state block and check for local parameterization and dimensions consistency
StateBlockPtr sb;
try {
diff --git a/src/yaml/parser_yaml.cpp b/src/yaml/parser_yaml.cpp
index a813f71b2246d53d0056bb8ab26ec1f9db5ee000..ecc7aa088a21acc492ee0fbc16a22c94ae3a33fa 100644
--- a/src/yaml/parser_yaml.cpp
+++ b/src/yaml/parser_yaml.cpp
@@ -517,20 +517,14 @@ void ParserYaml::parse()
tags.push_back("ROS publisher");
walkTreeR(it.n_, tags, "ROS publisher/" + it.type_ + " - " + it.topic_);
}
- std::list<std::string> plugins, packages_subscriber, packages_publisher;
+ std::set<std::string> plugins, packages_subscriber, packages_publisher;
for (auto pair : params_)
if (pair.first.find("plugin") != std::string::npos and pair.first != "plugins")
- plugins.push_back(pair.second);
+ plugins.insert(pair.second);
for (const auto& it : subscriber_managers_)
- packages_subscriber.push_back(it.package_);
+ packages_subscriber.insert(it.package_);
for (const auto& it : publisher_managers_)
- packages_publisher.push_back(it.package_);
- plugins.sort();
- plugins.unique();
- packages_subscriber.sort();
- packages_subscriber.unique();
- packages_publisher.sort();
- packages_publisher.unique();
+ packages_publisher.insert(it.package_);
insert_register("plugins", wolf::converter<std::string>::convert(plugins));
insert_register("packages_subscriber", wolf::converter<std::string>::convert(packages_subscriber));
insert_register("packages_publisher", wolf::converter<std::string>::convert(packages_publisher));
diff --git a/test/gtest_prior.cpp b/test/gtest_prior.cpp
index 990c07d5dfe9caaa2a56df3618600987781c2d4f..1985c80b6c3e4e630898bcb2d8c421d54d230355 100644
--- a/test/gtest_prior.cpp
+++ b/test/gtest_prior.cpp
@@ -338,7 +338,7 @@ TEST(Prior, StateQuaternion)
TEST(Prior, ParamsServer)
{
- ParserYaml parser = ParserYaml("test/yaml/params_prior.yaml", wolf_root);
+ ParserYaml parser = ParserYaml("test/yaml/params_prior.yaml", wolf_root, true);
ParamsServer server = ParamsServer(parser.getParams());
std::vector<std::string> keys({"P","O"});
@@ -363,7 +363,7 @@ TEST(Prior, ParamsServer)
if (not dynamic and drift)
continue;
- std::string prefix = key + to_string(dim) + "_" + mode + (dynamic ? "_dynamic" : "") + (drift ? "_drift" : "");
+ std::string prefix = key + "_" + to_string(dim) + "D_" + mode + (dynamic ? "_dynamic" : "") + (drift ? "_drift" : "");
WOLF_INFO("Creating prior from prefix ", prefix);
auto P = Prior(prefix, key, server);
@@ -420,7 +420,7 @@ TEST(Prior, ParamsServer)
TEST(Prior, ParamsServerWrong)
{
- ParserYaml parser = ParserYaml("test/yaml/params_prior_wrong.yaml", wolf_root);
+ ParserYaml parser = ParserYaml("test/yaml/params_prior_wrong.yaml", wolf_root, true);
ParamsServer server = ParamsServer(parser.getParams());
std::vector<std::string> keys({"P","O"});
@@ -440,7 +440,7 @@ TEST(Prior, ParamsServerWrong)
if (not dynamic and drift)
continue;
- std::string prefix = key + to_string(dim) + "_" + mode + (dynamic ? "_dynamic" : "") + (drift ? "_drift" : "");
+ std::string prefix = key + "_" + to_string(dim) + "D_" + mode + (dynamic ? "_dynamic" : "") + (drift ? "_drift" : "");
WOLF_INFO("Creating prior from prefix ", prefix);
ASSERT_THROW(auto P = Prior(prefix, key, server),std::runtime_error);
diff --git a/test/gtest_sensor_base.cpp b/test/gtest_sensor_base.cpp
index f2ef98beed7eeae2c9928bc0abcdd982df69e435..1b98fed3c06c6f08977be4305116170bae8ebf5b 100644
--- a/test/gtest_sensor_base.cpp
+++ b/test/gtest_sensor_base.cpp
@@ -27,24 +27,74 @@
*/
#include "core/sensor/sensor_base.h"
-
#include "core/utils/utils_gtest.h"
+#include "core/yaml/parser_yaml.h"
+#include "core/utils/params_server.h"
using namespace wolf;
using namespace Eigen;
+std::string wolf_root = _WOLF_ROOT_DIR;
+
+VectorXd vector0 = VectorXd(0);
VectorXd p_state_2D = VectorXd::Random(2);
VectorXd p_state_3D = VectorXd::Random(3);
VectorXd o_state_2D = VectorXd::Random(1);
VectorXd o_state_3D = VectorXd::Random(4).normalized();
-VectorXd p_sigma_2D = VectorXd::Random(2).cwiseAbs();
-VectorXd p_sigma_3D = VectorXd::Random(3).cwiseAbs();
-VectorXd o_sigma_2D = VectorXd::Random(1).cwiseAbs();
-VectorXd o_sigma_3D = VectorXd::Random(3).cwiseAbs();
-
+VectorXd p_std_2D = VectorXd::Random(2).cwiseAbs();
+VectorXd p_std_3D = VectorXd::Random(3).cwiseAbs();
+VectorXd o_std_2D = VectorXd::Random(1).cwiseAbs();
+VectorXd o_std_3D = VectorXd::Random(3).cwiseAbs();
+MatrixXd p_cov_2D = p_std_2D.cwiseAbs2().asDiagonal();
+MatrixXd p_cov_3D = p_std_3D.cwiseAbs2().asDiagonal();
+MatrixXd o_cov_2D = o_std_2D.cwiseAbs2().asDiagonal();
+MatrixXd o_cov_3D = o_std_3D.cwiseAbs2().asDiagonal();
VectorXd noise_std = Vector2d::Constant(0.1);
MatrixXd noise_cov = noise_std.cwiseAbs2().asDiagonal();
+void testSensor(SensorBasePtr S,
+ char _key,
+ const VectorXd& _state,
+ bool _fixed,
+ const VectorXd& _noise_std,
+ bool _dynamic,
+ const VectorXd& _drift_std)
+{
+ MatrixXd noise_cov = _noise_std.cwiseAbs2().asDiagonal();
+ MatrixXd drift_cov = _drift_std.cwiseAbs2().asDiagonal();
+
+ // state
+ ASSERT_MATRIX_APPROX(_state, S->getStateBlockDynamic(_key)->getState(), Constants::EPS);
+ // fixed
+ ASSERT_EQ(S->getStateBlockDynamic(_key)->isFixed(), _fixed);
+ // dynamic
+ ASSERT_EQ(S->isStateBlockDynamic(_key), _dynamic);
+ // drift
+ ASSERT_EQ(_drift_std.size(), S->getDriftStd(_key).size());
+ ASSERT_EQ(drift_cov.size(), S->getDriftCov(_key).size());
+ if (_drift_std.size() != 0)
+ {
+ ASSERT_MATRIX_APPROX(_drift_std, S->getDriftStd(_key), Constants::EPS);
+ ASSERT_MATRIX_APPROX(drift_cov, S->getDriftCov(_key), Constants::EPS);
+ }
+ // factor
+ if (_noise_std.size() != 0)
+ {
+ ASSERT_TRUE(S->getPriorFeature(_key) != nullptr);
+ ASSERT_EQ(_state.size(), S->getPriorFeature(_key)->getMeasurement().size());
+ ASSERT_MATRIX_APPROX(_state,
+ S->getPriorFeature(_key)->getMeasurement(),
+ Constants::EPS);
+ ASSERT_EQ(noise_cov.size(), S->getPriorFeature(_key)->getMeasurementCovariance().size());
+ ASSERT_MATRIX_APPROX(noise_cov,
+ S->getPriorFeature(_key)->getMeasurementCovariance(),
+ Constants::EPS);
+ }
+ else
+ {
+ ASSERT_TRUE(S->getPriorFeature(_key) == nullptr);
+ }
+}
// CONSTRUCTOR WITH PRIORS AND PARAMS
// 2D Fix
@@ -53,19 +103,18 @@ TEST(SensorBase, POfix2D)
auto params = std::make_shared<ParamsSensorBase>();
params->noise_std = noise_std;
- auto S = std::make_shared<SensorBase>("SensorBase",
- "sensor1",
- 2,
+ auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 2,
Priors({{'P',Prior("P", p_state_2D)}, //default "fix", not dynamic
{'O',Prior("O", o_state_2D)}}),
params);
- ASSERT_MATRIX_APPROX(noise_std, S->getNoiseStd(), 1e-8);
- ASSERT_MATRIX_APPROX(noise_cov, S->getNoiseCov(), 1e-8);
- ASSERT_MATRIX_APPROX(p_state_2D, S->getP()->getState(), 1e-8);
- ASSERT_MATRIX_APPROX(o_state_2D, S->getO()->getState(), 1e-8);
- ASSERT_TRUE(S->getP()->isFixed());
- ASSERT_TRUE(S->getO()->isFixed());
+ // noise
+ ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS);
+ ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov, Constants::EPS);
+
+ // checks
+ testSensor(S, 'P', p_state_2D, true, vector0, false, vector0);
+ testSensor(S, 'O', o_state_2D, true, vector0, false, vector0);
}
// 3D Fix
@@ -74,19 +123,21 @@ TEST(SensorBase, POfix3D)
auto params = std::make_shared<ParamsSensorBase>();
params->noise_std = noise_std;
- auto S = std::make_shared<SensorBase>("SensorBase",
- "sensor1",
- 3,
+ auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 3,
Priors({{'P',Prior("P", p_state_3D)}, //default "fix", not dynamic
{'O',Prior("O", o_state_3D)}}),
params);
- ASSERT_MATRIX_APPROX(noise_std, S->getNoiseStd(), 1e-8);
- ASSERT_MATRIX_APPROX(noise_cov, S->getNoiseCov(), 1e-8);
- ASSERT_MATRIX_APPROX(p_state_3D, S->getP()->getState(), 1e-8);
- ASSERT_MATRIX_APPROX(o_state_3D, S->getO()->getState(), 1e-8);
- ASSERT_TRUE(S->getP()->isFixed());
- ASSERT_TRUE(S->getO()->isFixed());
+ // noise
+ ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS);
+ ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov, Constants::EPS);
+
+ // factors
+ ASSERT_EQ(S->getPriorFeatures().size(), 0);
+
+ // check
+ testSensor(S, 'P', p_state_3D, true, vector0, false, vector0);
+ testSensor(S, 'O', o_state_3D, true, vector0, false, vector0);
}
// 2D Initial guess
@@ -95,19 +146,21 @@ TEST(SensorBase, POinitial_guess2D)
auto params = std::make_shared<ParamsSensorBase>();
params->noise_std = noise_std;
- auto S = std::make_shared<SensorBase>("SensorBase",
- "sensor1",
- 2,
+ auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 2,
Priors({{'P',Prior("P", p_state_2D, "initial_guess")},
{'O',Prior("O", o_state_2D, "initial_guess")}}),
params);
- ASSERT_MATRIX_APPROX(noise_std, S->getNoiseStd(), 1e-8);
- ASSERT_MATRIX_APPROX(noise_cov, S->getNoiseCov(), 1e-8);
- ASSERT_MATRIX_APPROX(p_state_2D, S->getP()->getState(), 1e-8);
- ASSERT_MATRIX_APPROX(o_state_2D, S->getO()->getState(), 1e-8);
- ASSERT_FALSE(S->getP()->isFixed());
- ASSERT_FALSE(S->getO()->isFixed());
+ // noise
+ ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS);
+ ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov, Constants::EPS);
+
+ // factors
+ ASSERT_EQ(S->getPriorFeatures().size(), 0);
+
+ // check
+ testSensor(S, 'P', p_state_2D, false, vector0, false, vector0);
+ testSensor(S, 'O', o_state_2D, false, vector0, false, vector0);
}
// 3D Initial guess
@@ -116,19 +169,21 @@ TEST(SensorBase, POinitial_guess3D)
auto params = std::make_shared<ParamsSensorBase>();
params->noise_std = noise_std;
- auto S = std::make_shared<SensorBase>("SensorBase",
- "sensor1",
- 3,
+ auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 3,
Priors({{'P',Prior("P", p_state_3D, "initial_guess")},
{'O',Prior("O", o_state_3D, "initial_guess")}}),
params);
- ASSERT_MATRIX_APPROX(noise_std, S->getNoiseStd(), 1e-8);
- ASSERT_MATRIX_APPROX(noise_cov, S->getNoiseCov(), 1e-8);
- ASSERT_MATRIX_APPROX(p_state_3D, S->getP()->getState(), 1e-8);
- ASSERT_MATRIX_APPROX(o_state_3D, S->getO()->getState(), 1e-8);
- ASSERT_FALSE(S->getP()->isFixed());
- ASSERT_FALSE(S->getO()->isFixed());
+ // noise
+ ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS);
+ ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov, Constants::EPS);
+
+ // factors
+ ASSERT_EQ(S->getPriorFeatures().size(), 0);
+
+ // check
+ testSensor(S, 'P', p_state_3D, false, vector0, false, vector0);
+ testSensor(S, 'O', o_state_3D, false, vector0, false, vector0);
}
// 2D Factor
@@ -137,19 +192,21 @@ TEST(SensorBase, POfactor2D)
auto params = std::make_shared<ParamsSensorBase>();
params->noise_std = noise_std;
- auto S = std::make_shared<SensorBase>("SensorBase",
- "sensor1",
- 2,
- Priors({{'P',Prior("P", p_state_2D, "factor", p_sigma_2D)},
- {'O',Prior("O", o_state_2D, "factor", o_sigma_2D)}}),
+ auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 2,
+ Priors({{'P',Prior("P", p_state_2D, "factor", p_std_2D)},
+ {'O',Prior("O", o_state_2D, "factor", o_std_2D)}}),
params);
- ASSERT_MATRIX_APPROX(noise_std, S->getNoiseStd(), 1e-8);
- ASSERT_MATRIX_APPROX(noise_cov, S->getNoiseCov(), 1e-8);
- ASSERT_MATRIX_APPROX(p_state_2D, S->getP()->getState(), 1e-8);
- ASSERT_MATRIX_APPROX(o_state_2D, S->getO()->getState(), 1e-8);
- ASSERT_FALSE(S->getP()->isFixed());
- ASSERT_FALSE(S->getO()->isFixed());
+ // noise
+ ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS);
+ ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov, Constants::EPS);
+
+ // factors
+ ASSERT_EQ(S->getPriorFeatures().size(), 2);
+
+ // check
+ testSensor(S, 'P', p_state_2D, false, p_std_2D, false, vector0);
+ testSensor(S, 'O', o_state_2D, false, o_std_2D, false, vector0);
}
// 3D Factor
@@ -158,19 +215,21 @@ TEST(SensorBase, POfactor3D)
auto params = std::make_shared<ParamsSensorBase>();
params->noise_std = noise_std;
- auto S = std::make_shared<SensorBase>("SensorBase",
- "sensor1",
- 3,
- Priors({{'P',Prior("P", p_state_3D, "factor", p_sigma_3D)},
- {'O',Prior("O", o_state_3D, "factor", o_sigma_3D)}}),
+ auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 3,
+ Priors({{'P',Prior("P", p_state_3D, "factor", p_std_3D)},
+ {'O',Prior("O", o_state_3D, "factor", o_std_3D)}}),
params);
- ASSERT_MATRIX_APPROX(noise_std, S->getNoiseStd(), 1e-8);
- ASSERT_MATRIX_APPROX(noise_cov, S->getNoiseCov(), 1e-8);
- ASSERT_MATRIX_APPROX(p_state_3D, S->getP()->getState(), 1e-8);
- ASSERT_MATRIX_APPROX(o_state_3D, S->getO()->getState(), 1e-8);
- ASSERT_FALSE(S->getP()->isFixed());
- ASSERT_FALSE(S->getO()->isFixed());
+ // noise
+ ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS);
+ ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov, Constants::EPS);
+
+ // factors
+ ASSERT_EQ(S->getPriorFeatures().size(), 2);
+
+ // check
+ testSensor(S, 'P', p_state_3D, false, p_std_3D, false, vector0);
+ testSensor(S, 'O', o_state_3D, false, o_std_3D, false, vector0);
}
// 2D Initial guess dynamic
@@ -179,19 +238,21 @@ TEST(SensorBase, POinitial_guess_dynamic2D)
auto params = std::make_shared<ParamsSensorBase>();
params->noise_std = noise_std;
- auto S = std::make_shared<SensorBase>("SensorBase",
- "sensor1",
- 2,
- Priors({{'P',Prior("P", p_state_2D, "initial_guess", p_sigma_2D, true, p_sigma_2D)},
- {'O',Prior("O", o_state_2D, "initial_guess", o_sigma_2D, true, o_sigma_2D)}}),
+ auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 2,
+ Priors({{'P',Prior("P", p_state_2D, "initial_guess", vector0, true)},
+ {'O',Prior("O", o_state_2D, "initial_guess", vector0, true)}}),
params);
- ASSERT_MATRIX_APPROX(noise_std, S->getNoiseStd(), 1e-8);
- ASSERT_MATRIX_APPROX(noise_cov, S->getNoiseCov(), 1e-8);
- ASSERT_MATRIX_APPROX(p_state_2D, S->getP()->getState(), 1e-8);
- ASSERT_MATRIX_APPROX(o_state_2D, S->getO()->getState(), 1e-8);
- ASSERT_FALSE(S->getP()->isFixed());
- ASSERT_FALSE(S->getO()->isFixed());
+ // noise
+ ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS);
+ ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov, Constants::EPS);
+
+ // factors
+ ASSERT_EQ(S->getPriorFeatures().size(), 0);
+
+ // check
+ testSensor(S, 'P', p_state_2D, false, vector0, true, vector0);
+ testSensor(S, 'O', o_state_2D, false, vector0, true, vector0);
}
// 3D Initial guess dynamic
@@ -200,19 +261,178 @@ TEST(SensorBase, POinitial_guess_dynamic3D)
auto params = std::make_shared<ParamsSensorBase>();
params->noise_std = noise_std;
- auto S = std::make_shared<SensorBase>("SensorBase",
- "sensor1",
- 3,
- Priors({{'P',Prior("P", p_state_3D, "initial_guess", p_sigma_3D, true, p_sigma_3D)},
- {'O',Prior("O", o_state_3D, "initial_guess", o_sigma_3D, true, o_sigma_3D)}}),
+ auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 3,
+ Priors({{'P',Prior("P", p_state_3D, "initial_guess", vector0, true)},
+ {'O',Prior("O", o_state_3D, "initial_guess", vector0, true)}}),
+ params);
+
+ // noise
+ ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS);
+ ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov, Constants::EPS);
+
+ // factors
+ ASSERT_EQ(S->getPriorFeatures().size(), 0);
+
+ // check
+ testSensor(S, 'P', p_state_3D, false, vector0, true, vector0);
+ testSensor(S, 'O', o_state_3D, false, vector0, true, vector0);
+}
+
+// 2D Initial guess dynamic drift
+TEST(SensorBase, POinitial_guess_dynamic2D_drift)
+{
+ auto params = std::make_shared<ParamsSensorBase>();
+ params->noise_std = noise_std;
+
+ auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 2,
+ Priors({{'P',Prior("P", p_state_2D, "initial_guess", vector0, true, p_std_2D)},
+ {'O',Prior("O", o_state_2D, "initial_guess", vector0, true, o_std_2D)}}),
+ params);
+
+ // noise
+ ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS);
+ ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov, Constants::EPS);
+
+ // factors
+ ASSERT_EQ(S->getPriorFeatures().size(), 0);
+
+ // check
+ testSensor(S, 'P', p_state_2D, false, vector0, true, p_std_2D);
+ testSensor(S, 'O', o_state_2D, false, vector0, true, o_std_2D);
+}
+
+// 3D Initial guess dynamic drift
+TEST(SensorBase, POinitial_guess_dynamic3D_drift)
+{
+ auto params = std::make_shared<ParamsSensorBase>();
+ params->noise_std = noise_std;
+
+ auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 3,
+ Priors({{'P',Prior("P", p_state_3D, "initial_guess", vector0, true, p_std_3D)},
+ {'O',Prior("O", o_state_3D, "initial_guess", vector0, true, o_std_3D)}}),
+ params);
+
+ // noise
+ ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS);
+ ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov, Constants::EPS);
+
+ // factors
+ ASSERT_EQ(S->getPriorFeatures().size(), 0);
+
+ // check
+ testSensor(S, 'P', p_state_3D, false, vector0, true, p_std_3D);
+ testSensor(S, 'O', o_state_3D, false, vector0, true, o_std_3D);
+}
+
+// 3D POI mixed
+TEST(SensorBase, POI_mixed)
+{
+ auto params = std::make_shared<ParamsSensorBase>();
+ params->noise_std = noise_std;
+
+ VectorXd i_state_3D = VectorXd::Random(5);
+
+ auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 3,
+ Priors({{'P',Prior("P", p_state_3D, "fix", vector0, true)},
+ {'O',Prior("O", o_state_3D, "factor", o_std_3D, true, o_std_3D)},
+ {'I',Prior("StateBlock", i_state_3D, "initial_guess")},
+ {'A',Prior("StateQuaternion", o_state_3D, "factor", o_std_3D)}}),
params);
- ASSERT_MATRIX_APPROX(noise_std, S->getNoiseStd(), 1e-8);
- ASSERT_MATRIX_APPROX(noise_cov, S->getNoiseCov(), 1e-8);
- ASSERT_MATRIX_APPROX(p_state_3D, S->getP()->getState(), 1e-8);
- ASSERT_MATRIX_APPROX(o_state_3D, S->getO()->getState(), 1e-8);
- ASSERT_FALSE(S->getP()->isFixed());
- ASSERT_FALSE(S->getO()->isFixed());
+ // noise
+ ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS);
+ ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov, Constants::EPS);
+
+ // factors
+ ASSERT_EQ(S->getPriorFeatures().size(), 2);
+
+ // check
+ testSensor(S, 'P', p_state_3D, true, vector0, true, vector0);
+ testSensor(S, 'O', o_state_3D, false, o_std_3D, true, o_std_3D);
+ testSensor(S, 'I', i_state_3D, false, vector0, false, vector0);
+ testSensor(S, 'A', o_state_3D, false, o_std_3D, false, vector0);
+}
+
+// CONSTRUCTOR WITH PARAM SERVER and KEY_TYPES
+// 2D Fix
+TEST(SensorBase, POfix2D_server)
+{
+ 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});
+ std::vector<bool> drifts({false, true});
+
+ 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;
+
+ // P & O
+ for (auto dim : dims)
+ 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_PO_" + to_string(dim) + "D_" + mode + (dynamic ? "_dynamic" : "") + (drift ? "_drift" : "");
+ WOLF_INFO("Creating sensor from name ", name);
+
+ auto S = std::make_shared<SensorBase>("SensorBase", name, dim, server);
+
+ auto p_size = dim;
+ auto o_size = dim == 2 ? 1 : 4;
+ auto p_size_std = mode == "factor" ? dim : 0;
+ auto o_size_std = mode == "factor" ? (dim == 2 ? 1 : 3) : 0;
+ auto p_size_std_drift = drift ? dim : 0;
+ auto o_size_std_drift = drift ? (dim == 2 ? 1 : 3) : 0;
+
+ // noise
+ ASSERT_MATRIX_APPROX(S->getNoiseStd(), po_std.head<2>(), Constants::EPS);
+ ASSERT_MATRIX_APPROX(S->getNoiseCov(), MatrixXd(po_std.head<2>().cwiseAbs2().asDiagonal()), Constants::EPS);
+
+ // factors
+ ASSERT_EQ(S->getPriorFeatures().size(), mode == "factor" ? 2 : 0);
+
+ // check
+ testSensor(S, 'P', p_state.head(p_size), mode == "fix", po_std.head(p_size_std), dynamic, po_std.head(p_size_std_drift));
+ testSensor(S, 'O', o_state.head(o_size), mode == "fix", po_std.head(o_size_std), dynamic, po_std.head(o_size_std_drift));
+
+
+ // INCORRECT YAML
+ WOLF_INFO("Creating sensor from name ", name + "_wrong");
+ ASSERT_THROW(std::make_shared<SensorBase>("SensorBase", name + "_wrong", dim, server),std::runtime_error);
+ }
+
+ // P & O & I & A - 3D
+ // CORRECT YAML
+ WOLF_INFO("Creating sensor from name sensor_POIA_3D");
+
+ auto S = std::make_shared<SensorBase>("SensorBase",
+ "sensor_POIA_3D",
+ 3,
+ server,
+ std::unordered_map<char, std::string>({{'I',"StateBlock"},
+ {'A',"StateQuaternion"}}));
+
+ // noise
+ ASSERT_MATRIX_APPROX(S->getNoiseStd(), po_std.head<2>(), Constants::EPS);
+ ASSERT_MATRIX_APPROX(S->getNoiseCov(), MatrixXd(po_std.head<2>().cwiseAbs2().asDiagonal()), Constants::EPS);
+
+ // factors
+ ASSERT_EQ(S->getPriorFeatures().size(), 2);
+
+ // check
+ testSensor(S, 'P', p_state.head(3), false, po_std.head(3), true, vector0);
+ testSensor(S, 'O', o_state.head(4), true, vector0, false, vector0);
+ testSensor(S, 'I', p_state.head(4), false, vector0, true, po_std.head(4));
+ testSensor(S, 'A', o_state.head(4), false, po_std.head(3), true, po_std.head(3));
}
int main(int argc, char **argv)
diff --git a/test/yaml/params_prior.yaml b/test/yaml/params_prior.yaml
index 00c6f18916822a0f62a9436b2aff54ea14f1dcb7..eea5d70f1b1738291f2961d6fbabe61e0f1fa20f 100644
--- a/test/yaml/params_prior.yaml
+++ b/test/yaml/params_prior.yaml
@@ -1,263 +1,259 @@
-config:
- problem:
- dim: 2
-
- # P in 2D
- P2_initial_guess:
- mode: initial_guess
- state: [1, 2]
- dynamic: false
-
- P2_fix:
- mode: fix
- state: [1, 2]
- dynamic: false
-
- P2_factor:
- mode: factor
- state: [1, 2]
- noise_std: [0.1, 0.2]
- dynamic: false
-
- P2_initial_guess_dynamic_drift:
- mode: initial_guess
- state: [1, 2]
- dynamic: true
- drift_std: [0.1, 0.2]
-
- P2_fix_dynamic_drift:
- mode: fix
- state: [1, 2]
- dynamic: true
- drift_std: [0.1, 0.2]
-
- P2_factor_dynamic_drift:
- mode: factor
- state: [1, 2]
- noise_std: [0.1, 0.2]
- dynamic: true
- drift_std: [0.1, 0.2]
-
- P2_initial_guess_dynamic:
- mode: initial_guess
- state: [1, 2]
- dynamic: true
-
- P2_fix_dynamic:
- mode: fix
- state: [1, 2]
- dynamic: true
-
- P2_factor_dynamic:
- mode: factor
- state: [1, 2]
- noise_std: [0.1, 0.2]
- dynamic: true
-
- # P in 3D
- P3_initial_guess:
- mode: initial_guess
- state: [1, 2, 3]
- dynamic: false
-
- P3_fix:
- mode: fix
- state: [1, 2, 3]
- dynamic: false
-
- P3_factor:
- mode: factor
- state: [1, 2, 3]
- noise_std: [0.1, 0.2, 0.3]
- dynamic: false
-
- P3_initial_guess_dynamic_drift:
- mode: initial_guess
- state: [1, 2, 3]
- dynamic: true
- drift_std: [0.1, 0.2, 0.3]
-
- P3_fix_dynamic_drift:
- mode: fix
- state: [1, 2, 3]
- dynamic: true
- drift_std: [0.1, 0.2, 0.3]
-
- P3_factor_dynamic_drift:
- mode: factor
- state: [1, 2, 3]
- noise_std: [0.1, 0.2, 0.3]
- dynamic: true
- drift_std: [0.1, 0.2, 0.3]
-
- P3_initial_guess_dynamic:
- mode: initial_guess
- state: [1, 2, 3]
- dynamic: true
-
- P3_fix_dynamic:
- mode: fix
- state: [1, 2, 3]
- dynamic: true
-
- P3_factor_dynamic:
- mode: factor
- state: [1, 2, 3]
- noise_std: [0.1, 0.2, 0.3]
- dynamic: true
-
- # O in 2D
- O2_initial_guess:
- mode: initial_guess
- state: [1]
- dynamic: false
-
- O2_fix:
- mode: fix
- state: [1]
- dynamic: false
-
- O2_factor:
- mode: factor
- state: [1]
- noise_std: [0.1]
- dynamic: false
-
- O2_initial_guess_dynamic_drift:
- mode: initial_guess
- state: [1]
- dynamic: true
- drift_std: [0.1]
-
- O2_fix_dynamic_drift:
- mode: fix
- state: [1]
- dynamic: true
- drift_std: [0.1]
-
- O2_factor_dynamic_drift:
- mode: factor
- state: [1]
- noise_std: [0.1]
- dynamic: true
- drift_std: [0.1]
-
- O2_initial_guess_dynamic:
- mode: initial_guess
- state: [1]
- dynamic: true
-
- O2_fix_dynamic:
- mode: fix
- state: [1]
- dynamic: true
-
- O2_factor_dynamic:
- mode: factor
- state: [1]
- noise_std: [0.1]
- dynamic: true
-
- # O in 3D
- O3_initial_guess:
- mode: initial_guess
- state: [1, 0, 0, 0]
- dynamic: false
-
- O3_fix:
- mode: fix
- state: [1, 0, 0, 0]
- dynamic: false
-
- O3_factor:
- mode: factor
- state: [1, 0, 0, 0]
- noise_std: [0.1, 0.2, 0.3]
- dynamic: false
-
- O3_initial_guess_dynamic_drift:
- mode: initial_guess
- state: [1, 0, 0, 0]
- dynamic: true
- drift_std: [0.1, 0.2, 0.3]
-
- O3_fix_dynamic_drift:
- mode: fix
- state: [1, 0, 0, 0]
- dynamic: true
- drift_std: [0.1, 0.2, 0.3]
-
- O3_factor_dynamic_drift:
- 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]
-
- O3_initial_guess_dynamic:
- mode: initial_guess
- state: [1, 0, 0, 0]
- dynamic: true
-
- O3_fix_dynamic:
- mode: fix
- state: [1, 0, 0, 0]
- dynamic: true
-
- O3_factor_dynamic:
- mode: factor
- state: [1, 0, 0, 0]
- noise_std: [0.1, 0.2, 0.3]
- dynamic: true
-
- # I
- I_initial_guess:
- mode: initial_guess
- state: [1, 2, 3, 4]
- dynamic: false
-
- I_fix:
- mode: fix
- state: [1, 2, 3, 4]
- dynamic: false
-
- I_factor:
- mode: factor
- state: [1, 2, 3, 4]
- noise_std: [0.1, 0.2, 0.3, 0.4]
- dynamic: false
-
- I_initial_guess_dynamic_drift:
- mode: initial_guess
- state: [1, 2, 3, 4]
- dynamic: true
- drift_std: [0.1, 0.2, 0.3, 0.4]
-
- I_fix_dynamic_drift:
- mode: fix
- state: [1, 2, 3, 4]
- dynamic: true
- drift_std: [0.1, 0.2, 0.3, 0.4]
-
- I_factor_dynamic_drift:
- mode: factor
- state: [1, 2, 3, 4]
- noise_std: [0.1, 0.2, 0.3, 0.4]
- dynamic: true
- drift_std: [0.1, 0.2, 0.3, 0.4]
-
- I_initial_guess_dynamic:
- mode: initial_guess
- state: [1, 2, 3, 4]
- dynamic: true
-
- I_fix_dynamic:
- mode: fix
- state: [1, 2, 3, 4]
- dynamic: true
-
- I_factor_dynamic:
- mode: factor
- state: [1, 2, 3, 4]
- noise_std: [0.1, 0.2, 0.3, 0.4]
- dynamic: true
\ No newline at end of file
+# P in 2D
+P_2D_initial_guess:
+ mode: initial_guess
+ state: [1, 2]
+ dynamic: false
+
+P_2D_fix:
+ mode: fix
+ state: [1, 2]
+ dynamic: false
+
+P_2D_factor:
+ mode: factor
+ state: [1, 2]
+ noise_std: [0.1, 0.2]
+ dynamic: false
+
+P_2D_initial_guess_dynamic_drift:
+ mode: initial_guess
+ state: [1, 2]
+ dynamic: true
+ drift_std: [0.1, 0.2]
+
+P_2D_fix_dynamic_drift:
+ mode: fix
+ state: [1, 2]
+ dynamic: true
+ drift_std: [0.1, 0.2]
+
+P_2D_factor_dynamic_drift:
+ mode: factor
+ state: [1, 2]
+ noise_std: [0.1, 0.2]
+ dynamic: true
+ drift_std: [0.1, 0.2]
+
+P_2D_initial_guess_dynamic:
+ mode: initial_guess
+ state: [1, 2]
+ dynamic: true
+
+P_2D_fix_dynamic:
+ mode: fix
+ state: [1, 2]
+ dynamic: true
+
+P_2D_factor_dynamic:
+ mode: factor
+ state: [1, 2]
+ noise_std: [0.1, 0.2]
+ dynamic: true
+
+# P in 3D
+P_3D_initial_guess:
+ mode: initial_guess
+ state: [1, 2, 3]
+ dynamic: false
+
+P_3D_fix:
+ mode: fix
+ state: [1, 2, 3]
+ dynamic: false
+
+P_3D_factor:
+ mode: factor
+ state: [1, 2, 3]
+ noise_std: [0.1, 0.2, 0.3]
+ dynamic: false
+
+P_3D_initial_guess_dynamic_drift:
+ mode: initial_guess
+ state: [1, 2, 3]
+ dynamic: true
+ drift_std: [0.1, 0.2, 0.3]
+
+P_3D_fix_dynamic_drift:
+ mode: fix
+ state: [1, 2, 3]
+ dynamic: true
+ drift_std: [0.1, 0.2, 0.3]
+
+P_3D_factor_dynamic_drift:
+ mode: factor
+ state: [1, 2, 3]
+ noise_std: [0.1, 0.2, 0.3]
+ dynamic: true
+ drift_std: [0.1, 0.2, 0.3]
+
+P_3D_initial_guess_dynamic:
+ mode: initial_guess
+ state: [1, 2, 3]
+ dynamic: true
+
+P_3D_fix_dynamic:
+ mode: fix
+ state: [1, 2, 3]
+ dynamic: true
+
+P_3D_factor_dynamic:
+ mode: factor
+ state: [1, 2, 3]
+ noise_std: [0.1, 0.2, 0.3]
+ dynamic: true
+
+# O in 2D
+O_2D_initial_guess:
+ mode: initial_guess
+ state: [1]
+ dynamic: false
+
+O_2D_fix:
+ mode: fix
+ state: [1]
+ dynamic: false
+
+O_2D_factor:
+ mode: factor
+ state: [1]
+ noise_std: [0.1]
+ dynamic: false
+
+O_2D_initial_guess_dynamic_drift:
+ mode: initial_guess
+ state: [1]
+ dynamic: true
+ drift_std: [0.1]
+
+O_2D_fix_dynamic_drift:
+ mode: fix
+ state: [1]
+ dynamic: true
+ drift_std: [0.1]
+
+O_2D_factor_dynamic_drift:
+ mode: factor
+ state: [1]
+ noise_std: [0.1]
+ dynamic: true
+ drift_std: [0.1]
+
+O_2D_initial_guess_dynamic:
+ mode: initial_guess
+ state: [1]
+ dynamic: true
+
+O_2D_fix_dynamic:
+ mode: fix
+ state: [1]
+ dynamic: true
+
+O_2D_factor_dynamic:
+ mode: factor
+ state: [1]
+ noise_std: [0.1]
+ dynamic: true
+
+# O in 3D
+O_3D_initial_guess:
+ mode: initial_guess
+ state: [1, 0, 0, 0]
+ dynamic: false
+
+O_3D_fix:
+ mode: fix
+ state: [1, 0, 0, 0]
+ dynamic: false
+
+O_3D_factor:
+ mode: factor
+ state: [1, 0, 0, 0]
+ noise_std: [0.1, 0.2, 0.3]
+ dynamic: false
+
+O_3D_initial_guess_dynamic_drift:
+ mode: initial_guess
+ state: [1, 0, 0, 0]
+ dynamic: true
+ drift_std: [0.1, 0.2, 0.3]
+
+O_3D_fix_dynamic_drift:
+ mode: fix
+ state: [1, 0, 0, 0]
+ dynamic: true
+ drift_std: [0.1, 0.2, 0.3]
+
+O_3D_factor_dynamic_drift:
+ 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]
+
+O_3D_initial_guess_dynamic:
+ mode: initial_guess
+ state: [1, 0, 0, 0]
+ dynamic: true
+
+O_3D_fix_dynamic:
+ mode: fix
+ state: [1, 0, 0, 0]
+ dynamic: true
+
+O_3D_factor_dynamic:
+ mode: factor
+ state: [1, 0, 0, 0]
+ noise_std: [0.1, 0.2, 0.3]
+ dynamic: true
+
+# I
+I_initial_guess:
+ mode: initial_guess
+ state: [1, 2, 3, 4]
+ dynamic: false
+
+I_fix:
+ mode: fix
+ state: [1, 2, 3, 4]
+ dynamic: false
+
+I_factor:
+ mode: factor
+ state: [1, 2, 3, 4]
+ noise_std: [0.1, 0.2, 0.3, 0.4]
+ dynamic: false
+
+I_initial_guess_dynamic_drift:
+ mode: initial_guess
+ state: [1, 2, 3, 4]
+ dynamic: true
+ drift_std: [0.1, 0.2, 0.3, 0.4]
+
+I_fix_dynamic_drift:
+ mode: fix
+ state: [1, 2, 3, 4]
+ dynamic: true
+ drift_std: [0.1, 0.2, 0.3, 0.4]
+
+I_factor_dynamic_drift:
+ mode: factor
+ state: [1, 2, 3, 4]
+ noise_std: [0.1, 0.2, 0.3, 0.4]
+ dynamic: true
+ drift_std: [0.1, 0.2, 0.3, 0.4]
+
+I_initial_guess_dynamic:
+ mode: initial_guess
+ state: [1, 2, 3, 4]
+ dynamic: true
+
+I_fix_dynamic:
+ mode: fix
+ state: [1, 2, 3, 4]
+ dynamic: true
+
+I_factor_dynamic:
+ mode: factor
+ state: [1, 2, 3, 4]
+ noise_std: [0.1, 0.2, 0.3, 0.4]
+ dynamic: true
\ No newline at end of file
diff --git a/test/yaml/params_prior_wrong.yaml b/test/yaml/params_prior_wrong.yaml
index 31b919571be9a6e9bfa0cae6255d077ce2e037b5..afdaa97e76af653b6674fb7c62fa49fd5d514676 100644
--- a/test/yaml/params_prior_wrong.yaml
+++ b/test/yaml/params_prior_wrong.yaml
@@ -1,275 +1,260 @@
-config:
- problem:
- dim: 2
-
- # P in 2D
- P2_initial_guess:
- mode: initial_guess
- #state: [1, 2] # missing
- dynamic: false
-
- P2_fix:
- mode: fix
- #state: [1, 2] # missing
- dynamic: false
-
- P2_factor:
- mode: factor
- state: [1, 2]
- noise_std: [0.1, 0.2, 0.3] # wrong size
- dynamic: false
-
- P2_initial_guess_dynamic:
- mode: initial_guess
- state: [1, 2]
- #dynamic: true #missing
-
- P2_fix_dynamic:
- mode: fix
- state: [1] # wrong size
- dynamic: true
-
- P2_factor_dynamic:
- mode: factor
- state: [1, 2]
- #noise_std: [0.1, 0.2] # missing
- dynamic: true
-
- P2_initial_guess_dynamic_drift:
- mode: initial_guess
- state: [1, 2]
- dynamic: true
- #drift_std: [0.1, 0.2] #missing
-
- P2_fix_dynamic_drift:
- mode: fix
- state: [1, 2]
- dynamic: true
- drift_std: [0.1, 0.2, 0.3] # wrong size
-
- P2_factor_dynamic_drift:
- mode: factor
- state: [1, 2]
- #noise_std: [0.1, 0.2] # missing
- dynamic: true
- drift_std: [0.1, 0.2]
-
- # P in 3D
- P3_initial_guess:
- #mode: initial_guess # missing
- state: [1, 2, 3]
- dynamic: false
-
- P3_fix:
- mode: fix
- state: [1, 2, 3, 4] # wrong size
- dynamic: false
-
- P3_factor:
- mode: factor
- state: [1, 2, 3]
- noise_std: [0.1, 0.2] # wrong size
- dynamic: false
-
- P3_initial_guess_dynamic:
- mode: initial_guess
- state: [1, 2, 3]
- dynamic: true
- drift_std: [0.1, 0.2] # wrong size
-
- P3_fix_dynamic:
- mode: fix
- state: [1, 2, 3]
- dynamic: true
- #drift_std: [0.1, 0.2] # missing
-
- P3_factor_dynamic:
- mode: factor
- state: [1, 2, 3]
- noise_std: [0.1, 0.2] # wrong size
- dynamic: true
- drift_std: [0.1, 0.2, 0.3]
-
- P3_initial_guess_dynamic_drift:
- mode: initial_guess
- state: [1, 2, 3]
- dynamic: true
- drift_std: [0.1, 0.2] # wrong size
-
- P3_fix_dynamic_drift:
- mode: fix
- state: [1, 2, 3]
- dynamic: true
- #drift_std: [0.1, 0.2] # missing
-
- P3_factor_dynamic_drift:
- mode: factor
- state: [1, 2, 3]
- noise_std: [0.1, 0.2] # wrong size
- dynamic: true
- drift_std: [0.1, 0.2, 0.3]
-
- # O in 2D
- O2_initial_guess:
- mode: initial_guess
- state: [1, 2] # wrong size
- dynamic: false
-
- O2_fix:
- mode: fix
- state: [1]
- #dynamic: false # missing
-
- O2_factor:
- mode: factor
- state: [1]
- noise_std: [0.1, 0.2] # wrong size
- dynamic: false
-
- O2_initial_guess_dynamic:
- mode: initial_guess
- state: [1]
- dynamic: true
- #drift_std: [0.1] # missing
-
- O2_fix_dynamic:
- mode: fix
- state: [1 2] # wrong size
- dynamic: true
- drift_std: [0.1]
-
- O2_factor_dynamic:
- mode: factor
- state: [1]
- noise_std: [0.1 0.2] # wrong size
- dynamic: true
- drift_std: [0.1]
-
- O2_initial_guess_dynamic_drift:
- mode: initial_guess
- state: [1]
- dynamic: true
- #drift_std: [0.1] # missing
-
- O2_fix_dynamic_drift:
- mode: fix
- state: [1 2] # wrong size
- dynamic: true
- drift_std: [0.1]
-
- O2_factor_dynamic_drift:
- mode: factor
- state: [1]
- noise_std: [0.1 0.2] # wrong size
- dynamic: true
- drift_std: [0.1]
-
- # O in 3D
- O3_initial_guess:
- mode: initial_guess
- #state: [1, 0, 0, 0] # missing
- dynamic: false
-
- O3_fix:
- mode: fix
- state: [1, 0, 0, 0]
- #dynamic: false # missing
-
- O3_factor:
- mode: factor
- state: [1, 0, 0, 0]
- noise_std: [0.1, 0.2] # wrong size
- dynamic: false
-
- O3_initial_guess_dynamic:
- mode: initial_guess
- state: [1, 0, 0] # wrong size
- dynamic: true
- drift_std: [0.1, 0.2, 0.3]
-
- O3_fix_dynamic:
- mode: fix
- state: [1, 0, 0, 0]
- dynamic: true
- #drift_std: [0.1, 0.2, 0.3] # missing
-
- O3_factor_dynamic:
- mode: factor
- state: [1, 0, 0, 0]
- noise_std: [0.1, 0.2, 0.3]
- dynamic: true
- drift_std: [0.1, 0.2] # wrong size
-
- O3_initial_guess_dynamic_drift:
- mode: initial_guess
- state: [1, 0, 0] # wrong size
- dynamic: true
- drift_std: [0.1, 0.2, 0.3]
-
- O3_fix_dynamic_drift:
- mode: fix
- state: [1, 0, 0, 0]
- dynamic: true
- #drift_std: [0.1, 0.2, 0.3] # missing
-
- O3_factor_dynamic_drift:
- mode: factor
- state: [1, 0, 0, 0]
- noise_std: [0.1, 0.2, 0.3]
- dynamic: true
- drift_std: [0.1, 0.2] # wrong size
-
- # I
- I_initial_guess:
- #mode: initial_guess # missing
- state: [1, 2, 3, 4]
- dynamic: false
-
- I_fix:
- mode: fix
- #state: [1, 2, 3, 4] # missing
- dynamic: false
-
- I_factor:
- mode: factor
- state: [1, 2, 3, 4]
- noise_std: [0.1, 0.2, 0.3] # wrong size
- dynamic: false
-
- I_initial_guess_dynamic:
- mode: initial_guess
- state: [1, 2, 3, 4]
- dynamic: true
- drift_std: [0.1, 0.2, 0.3] # wrong size
-
- I_fix_dynamic:
- mode: fix
- state: [1, 2, 3] # wrong size
- dynamic: true
- drift_std: [0.1, 0.2, 0.3, 0.4]
-
- I_factor_dynamic:
- mode: factor
- state: [1, 2, 3, 4]
- noise_std: [0.1, 0.2, 0.3] # wrong size
- dynamic: true
- drift_std: [0.1, 0.2, 0.3, 0.4]
-
- I_initial_guess_dynamic_drift:
- mode: initial_guess
- state: [1, 2, 3, 4]
- dynamic: true
- drift_std: [0.1, 0.2, 0.3] # wrong size
-
- I_fix_dynamic_drift:
- mode: fix
- state: [1, 2, 3] # wrong size
- dynamic: true
- drift_std: [0.1, 0.2, 0.3, 0.4]
-
- I_factor_dynamic_drift:
- mode: factor
- state: [1, 2, 3, 4]
- noise_std: [0.1, 0.2, 0.3] # wrong size
- dynamic: true
- drift_std: [0.1, 0.2, 0.3, 0.4]
\ No newline at end of file
+# P in _2D_
+P_2D_initial_guess:
+ mode: initial_guess
+ #state: [1, 2] # missing
+ dynamic: false
+
+P_2D_fix:
+ mode: fix
+ #state: [1, 2] # missing
+ dynamic: false
+
+P_2D_factor:
+ mode: factor
+ state: [1, 2]
+ noise_std: [0.1, 0.2, 0.3] # wrong size
+ dynamic: false
+
+P_2D_initial_guess_dynamic:
+ mode: initial_guess
+ state: [1, 2]
+ #dynamic: true #missing
+
+P_2D_fix_dynamic:
+ mode: fix
+ state: [1] # wrong size
+ dynamic: true
+
+P_2D_factor_dynamic:
+ mode: factor
+ state: [1, 2]
+ #noise_std: [0.1, 0.2] # missing
+ dynamic: true
+
+P_2D_initial_guess_dynamic_drift:
+ mode: initial_guess
+ state: [1, 2]
+ dynamic: true
+ drift_std: [0.1, 0.2, 0.3] # wrong size
+
+P_2D_fix_dynamic_drift:
+ mode: fix
+ state: [1, 2]
+ dynamic: true
+ drift_std: [0.1] # wrong size
+
+P_2D_factor_dynamic_drift:
+ mode: factor
+ state: [1, 2]
+ #noise_std: [0.1, 0.2] # missing
+ dynamic: true
+ drift_std: [0.1, 0.2]
+
+# P in _3D_
+P_3D_initial_guess:
+ #mode: initial_guess # missing
+ state: [1, 2, 3]
+ dynamic: false
+
+P_3D_fix:
+ mode: fix
+ state: [1, 2, 3, 4] # wrong size
+ dynamic: false
+
+P_3D_factor:
+ mode: factor
+ state: [1, 2, 3]
+ noise_std: [0.1, 0.2] # wrong size
+ dynamic: false
+
+P_3D_initial_guess_dynamic:
+ mode: initial_guess
+ state: [1, 2, 3, 4] # wrong size
+ dynamic: true
+
+P_3D_fix_dynamic:
+ mode: fix
+ state: [1, 2, 3]
+ # dynamic: true # missing
+
+P_3D_factor_dynamic:
+ mode: factor
+ state: [1, 2, 3]
+ noise_std: [0.1, 0.2] # wrong size
+ dynamic: true
+ drift_std: [0.1, 0.2, 0.3]
+
+P_3D_initial_guess_dynamic_drift:
+ mode: initial_guess
+ state: [1, 2, 3]
+ dynamic: true
+ drift_std: [0.1, 0.2] # wrong size
+
+P_3D_fix_dynamic_drift:
+ #mode: fix
+ state: [1, 2, 3]
+ dynamic: true
+ drift_std: [0.1, 0.2]
+
+P_3D_factor_dynamic_drift:
+ mode: factor
+ state: [1, 2, 3]
+ noise_std: [0.1, 0.2] # wrong size
+ dynamic: true
+ drift_std: [0.1, 0.2, 0.3]
+
+# O in _2D_
+O_2D_initial_guess:
+ mode: initial_guess
+ state: [1, 2] # wrong size
+ dynamic: false
+
+O_2D_fix:
+ mode: fix
+ state: [1]
+ #dynamic: false # missing
+
+O_2D_factor:
+ mode: factor
+ state: [1]
+ noise_std: [0.1, 0.2] # wrong size
+ dynamic: false
+
+O_2D_initial_guess_dynamic:
+ mode: initial_guess
+ state: [1, 2] #wrong size
+ dynamic: true
+
+O_2D_fix_dynamic:
+ mode: fix
+ state: [1 2] # wrong size
+ dynamic: true
+
+O_2D_factor_dynamic:
+ mode: factor
+ state: [1]
+ noise_std: [0.1 0.2] # wrong size
+ dynamic: true
+
+O_2D_initial_guess_dynamic_drift:
+ mode: initial_guess
+ state: [1]
+ dynamic: true
+ drift_std: [0.1, 0.2] # wrong size
+
+O_2D_fix_dynamic_drift:
+ mode: fix
+ state: [1 2] # wrong size
+ dynamic: true
+ drift_std: [0.1]
+
+O_2D_factor_dynamic_drift:
+ mode: factor
+ state: [1]
+ noise_std: [0.1 0.2] # wrong size
+ dynamic: true
+ drift_std: [0.1]
+
+# O in _3D_
+O_3D_initial_guess:
+ mode: initial_guess
+ #state: [1, 0, 0, 0] # missing
+ dynamic: false
+
+O_3D_fix:
+ mode: fix
+ state: [1, 0, 0, 0]
+ #dynamic: false # missing
+
+O_3D_factor:
+ mode: factor
+ state: [1, 0, 0, 0]
+ noise_std: [0.1, 0.2] # wrong size
+ dynamic: false
+
+O_3D_initial_guess_dynamic:
+ mode: initial_guess
+ state: [1, 0, 0] # wrong size
+ dynamic: true
+
+O_3D_fix_dynamic:
+ mode: fix
+ state: [1, 0, 0, 0]
+ #dynamic: true
+
+O_3D_factor_dynamic:
+ mode: factor
+ state: [1, 0, 0, 0]
+ noise_std: [0.1, 0.2, 0.3, 0.4] # wrong size
+ dynamic: true
+
+O_3D_initial_guess_dynamic_drift:
+ mode: initial_guess
+ state: [1, 0, 0] # wrong size
+ dynamic: true
+ drift_std: [0.1, 0.2, 0.3]
+
+O_3D_fix_dynamic_drift:
+ mode: fix
+ state: [1, 0, 0, 0]
+ dynamic: true
+ drift_std: [0.1, 0.2, 0.3, 0.4] #wrong size
+
+O_3D_factor_dynamic_drift:
+ mode: factor
+ state: [1, 0, 0, 0]
+ noise_std: [0.1, 0.2, 0.3]
+ dynamic: true
+ drift_std: [0.1, 0.2] # wrong size
+
+# I
+I_initial_guess:
+ #mode: initial_guess # missing
+ state: [1, 2, 3, 4]
+ dynamic: false
+
+I_fix:
+ mode: fix
+ #state: [1, 2, 3, 4] # missing
+ dynamic: false
+
+I_factor:
+ mode: factor
+ state: [1, 2, 3, 4]
+ noise_std: [0.1, 0.2, 0.3] # wrong size
+ dynamic: false
+
+I_initial_guess_dynamic:
+ #mode: initial_guess # missing
+ state: [1, 2, 3, 4]
+ dynamic: true
+
+I_fix_dynamic:
+ mode: fix
+ state: [] # wrong size
+ dynamic: true
+
+I_factor_dynamic:
+ mode: factor
+ state: [1, 2, 3, 4]
+ noise_std: [0.1, 0.2, 0.3] # wrong size
+ dynamic: true
+
+I_initial_guess_dynamic_drift:
+ mode: initial_guess
+ state: [1, 2, 3, 4]
+ dynamic: true
+ drift_std: [0.1, 0.2, 0.3] # wrong size
+
+I_fix_dynamic_drift:
+ mode: fix
+ state: [1, 2, 3] # wrong size
+ dynamic: true
+ drift_std: [0.1, 0.2, 0.3, 0.4]
+
+I_factor_dynamic_drift:
+ mode: factor
+ state: [1, 2, 3, 4]
+ noise_std: [0.1, 0.2, 0.3] # wrong size
+ dynamic: true
+ drift_std: [0.1, 0.2, 0.3, 0.4]
\ No newline at end of file
diff --git a/test/yaml/sensor_base.yaml b/test/yaml/sensor_base.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..469dbd637fc87f3b381d6e339f87eb13c55852e8
--- /dev/null
+++ b/test/yaml/sensor_base.yaml
@@ -0,0 +1,476 @@
+sensor:
+
+ #############################################################################################
+ ########################################## CORRECT ##########################################
+ #############################################################################################
+
+ sensor_PO_2D_fix:
+ P:
+ mode: fix
+ state: [1, 2]
+ dynamic: false
+ O:
+ mode: fix
+ state: [1]
+ dynamic: false
+ noise_std: [0.1, 0.2]
+
+ sensor_PO_3D_fix:
+ 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]
+
+ sensor_PO_2D_initial_guess:
+ P:
+ mode: initial_guess
+ state: [1, 2]
+ dynamic: false
+ O:
+ mode: initial_guess
+ state: [1]
+ dynamic: false
+ noise_std: [0.1, 0.2]
+
+ sensor_PO_3D_initial_guess:
+ 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]
+
+ sensor_PO_2D_factor:
+ 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]
+
+ sensor_PO_3D_factor:
+ 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]
+
+ sensor_PO_2D_fix_dynamic:
+ P:
+ mode: fix
+ state: [1, 2]
+ dynamic: true
+ O:
+ mode: fix
+ state: [1]
+ dynamic: true
+ noise_std: [0.1, 0.2]
+
+ sensor_PO_3D_fix_dynamic:
+ 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]
+
+ sensor_PO_2D_initial_guess_dynamic:
+ P:
+ mode: initial_guess
+ state: [1, 2]
+ dynamic: true
+ O:
+ mode: initial_guess
+ state: [1]
+ dynamic: true
+ noise_std: [0.1, 0.2]
+
+ sensor_PO_3D_initial_guess_dynamic:
+ 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]
+
+ sensor_PO_2D_factor_dynamic:
+ 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]
+
+ sensor_PO_3D_factor_dynamic:
+ 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]
+
+ sensor_PO_2D_fix_dynamic_drift:
+ 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]
+
+ sensor_PO_3D_fix_dynamic_drift:
+ 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]
+
+ sensor_PO_2D_initial_guess_dynamic_drift:
+ 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]
+
+ sensor_PO_3D_initial_guess_dynamic_drift:
+ 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]
+
+ sensor_PO_2D_factor_dynamic_drift:
+ 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]
+
+ sensor_PO_3D_factor_dynamic_drift:
+ 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]
+
+ sensor_POIA_3D:
+ 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]
+
+ #############################################################################################
+ ######################################### INCORRECT #########################################
+ #############################################################################################
+
+ sensor_PO_2D_fix_wrong:
+ P:
+ mode: fix
+ state: [1, 2]
+ #dynamic: false #missing
+ O:
+ mode: fix
+ state: [1]
+ dynamic: false
+ noise_std: [0.1, 0.2]
+
+ sensor_PO_3D_fix_wrong:
+ 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
+
+ sensor_PO_2D_initial_guess_wrong:
+ P:
+ mode: initial_guess
+ state: [1, 2]
+ dynamic: false
+ O:
+ #mode: initial_guess #missing
+ state: [1]
+ dynamic: false
+ noise_std: [0.1, 0.2]
+
+ sensor_PO_3D_initial_guess_wrong:
+ 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]
+
+ sensor_PO_2D_factor_wrong:
+ 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]
+
+ sensor_PO_3D_factor_wrong:
+ 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]
+
+ sensor_PO_2D_fix_dynamic_wrong:
+ P:
+ mode: fix
+ state: [1, 2, 3] # wrong size
+ dynamic: true
+ O:
+ mode: fix
+ state: [1]
+ dynamic: true
+ noise_std: [0.1, 0.2]
+
+ sensor_PO_3D_fix_dynamic_wrong:
+ 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]
+
+ sensor_PO_2D_initial_guess_dynamic_wrong:
+ # P: #missing
+ # mode: initial_guess
+ # state: [1, 2]
+ # dynamic: true
+ O:
+ mode: initial_guess
+ state: [1]
+ dynamic: true
+ noise_std: [0.1, 0.2]
+
+ sensor_PO_3D_initial_guess_dynamic_wrong:
+ 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]
+
+ sensor_PO_2D_factor_dynamic_wrong:
+ 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]
+
+ sensor_PO_3D_factor_dynamic_wrong:
+ 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]
+
+ sensor_PO_2D_fix_dynamic_drift_wrong:
+ 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]
+
+ sensor_PO_3D_fix_dynamic_drift_wrong:
+ 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]
+
+ sensor_PO_2D_initial_guess_dynamic_drift_wrong:
+ 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]
+
+ sensor_PO_3D_initial_guess_dynamic_drift_wrong:
+ 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]
+
+ sensor_PO_2D_factor_dynamic_drift_wrong:
+ 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]
+
+ sensor_PO_3D_factor_dynamic_drift_wrong:
+ 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