diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h index 931db3689e731d433bf1686b04c00e66bcddbd66..f57725decd14bc50d4bec57936a8d2de5bf8c316 100644 --- a/include/core/sensor/sensor_base.h +++ b/include/core/sensor/sensor_base.h @@ -104,7 +104,7 @@ struct ParamsSensorBase: public ParamsBase std::string print() const override { - return "noise_std: ";// + to_string(noise_std) + "\n"; + return "noise_std: " + to_string(noise_std) + "\n"; } }; @@ -163,7 +163,7 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh const std::string& _unique_name, const SizeEigen& _dim, const ParamsServer& _server, - std::string _keys); + std::unordered_map<char, std::string> _keys_types); ~SensorBase() override; diff --git a/include/core/state_block/state_angle.h b/include/core/state_block/state_angle.h index 3cc093665df69b32ed10d2d49921fdacedea7871..1a307ce72310bcee0bf7d5b21b833aec8caf9340 100644 --- a/include/core/state_block/state_angle.h +++ b/include/core/state_block/state_angle.h @@ -58,7 +58,9 @@ inline StateAngle::~StateAngle() inline StateBlockPtr StateAngle::create (const Eigen::VectorXd& _state, bool _fixed) { - MatrixSizeCheck<1,1>::check(_state); + if(_state.size() != 1) + throw std::runtime_error("The angle state vector must be of size 1!"); + return std::make_shared<StateAngle>(_state(0), _fixed); } diff --git a/include/core/state_block/state_quaternion.h b/include/core/state_block/state_quaternion.h index a1317fd2936dedf39d79301526fa3d0829b509a5..d5278e1a6f4e3580c22056db7cc2c9ef073c21fb 100644 --- a/include/core/state_block/state_quaternion.h +++ b/include/core/state_block/state_quaternion.h @@ -61,7 +61,8 @@ inline StateQuaternion::StateQuaternion(const Eigen::Quaterniond& _quaternion, b inline StateQuaternion::StateQuaternion(const Eigen::VectorXd& _state, bool _fixed) : StateBlock(_state, _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>()) { - assert(state_.size() == 4 && "The quaternion state vector must be of size 4"); + if(state_.size() != 4) + throw std::runtime_error("The quaternion state vector must be of size 4!"); // TODO: remove these lines after issue #381 is addressed if(not isValid(1e-5)) @@ -95,7 +96,6 @@ inline Eigen::VectorXd StateQuaternion::identity() const inline StateBlockPtr StateQuaternion::create (const Eigen::VectorXd& _state, bool _fixed) { - MatrixSizeCheck<4,1>::check(_state); return std::make_shared<StateQuaternion>(_state, _fixed); } diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp index 64860f706a2e9f709230758f26bee39eec093ffc..dc92cbe1d59a68c106d041fa0049e07f65a08903 100644 --- a/src/sensor/sensor_base.cpp +++ b/src/sensor/sensor_base.cpp @@ -99,30 +99,31 @@ void SensorBase::loadPriors(const Priors& _priors, SizeEigen _dim) for (auto&& prior_pair : _priors) { + auto key = prior_pair.first; const Prior& prior = prior_pair.second; // Checks - if (prior.getKey() == 'P' and prior.getState().size() != _dim) + if (key == 'P' and prior.getState().size() != _dim) throw std::runtime_error("Prior state for P has wrong size"); - if (prior.getKey() == '0' and prior.getState().size() != (_dim == 2 ? 1 : 4)) + if (key == '0' and prior.getState().size() != (_dim == 2 ? 1 : 4)) throw std::runtime_error("Prior state for O has wrong size"); // create state block - auto sb = FactoryStateBlock::create(std::string(1, prior.getKey()), prior.getState(), prior.isFixed()); + auto sb = FactoryStateBlock::create(prior.getType(), prior.getState(), prior.isFixed()); // check local param if (not sb->isValid()) { - WOLF_ERROR("The created stateblock ", std::string(1, prior.getKey()), " is not valid (local param violation)"); + WOLF_ERROR("The created stateblock ", prior.getType(), " for key ", std::string(1,key), " is not valid (local param violation)"); throw std::runtime_error("StateBlock not valid"); } // Add state block - addStateBlock(prior.getKey(), sb, prior.isDynamic()); + addStateBlock(key, sb, prior.isDynamic()); // Factor if (prior.isFactor()) - addPriorParameter(prior.getKey(), prior.getState(), prior.getSigma()); + addPriorParameter(key, prior.getState(), prior.getSigma()); } } diff --git a/src/state_block/prior.cpp b/src/state_block/prior.cpp index 1d7fdbbdb8b8b29dcdb752e2b2c7d07d61429209..172d49aaa05c816cbf19bacac25c8a372118fbd7 100644 --- a/src/state_block/prior.cpp +++ b/src/state_block/prior.cpp @@ -42,10 +42,10 @@ Prior::Prior(const std::string& _type, check(); } -Prior::Prior(const std::string& _prefix, const ParamsServer& _server, const std::string& _type) +Prior::Prior(const std::string& _prefix, const std::string& _type, const ParamsServer& _server) { type_ = _type; - mode_ = _server.getParam<double>(_prefix + "/mode"); + mode_ = _server.getParam<std::string>(_prefix + "/mode"); if (mode_ != "nothing") state_ = _server.getParam<Eigen::VectorXd>(_prefix + "/state"); @@ -70,42 +70,47 @@ Prior::Prior(const std::string& _prefix, const ParamsServer& _server, const std: void Prior::check() const { if (mode_ != "nothing" and mode_ != "initial_guess" and mode_ != "fix" and mode_ != "factor") - throw std::runtime_error("wrong mode value: " + mode_ + ", it should be: 'nothing', 'initial_guess', 'fix' or 'factor'"); + throw std::runtime_error("Prior::check() wrong mode value: " + mode_ + ", it should be: 'nothing', 'initial_guess', 'fix' or 'factor'. " + print()); - if ( mode_ == "nothing" and (type_ == "P" or type_ == "O")) - throw std::runtime_error("For P and O keys, mode 'nothing' is not valid"); - - if (dynamic_ and (type_ == "P" or type_ == "O") ) - throw std::runtime_error("Dynamic state blocks not implemented for extrinsics"); + if (mode_ == "nothing" and state_.size() == 0) + return; // try to create a state block and check for local parameterization and dimensions consistency - auto sb = FactoryStateBlock::create(type_, state_, mode_ == "fix"); + StateBlockPtr sb; + try + { + sb = FactoryStateBlock::create(type_, state_, mode_ == "fix"); + } + catch(const std::exception& e) + { + throw std::runtime_error("Prior::check() could not create state block from prior with error: " + std::string(e.what()) + print()); + } // check local param if (not sb->isValid()) - throw std::runtime_error("Prior " + type_ + " state is not valid (local param violation)"); + throw std::runtime_error("Prior::check() Prior " + type_ + " state is not valid (local param violation). " + print()); // check state size if (mode_ != "nothing" and state_.size() != sb->getSize()) - throw std::runtime_error("Prior " + type_ + " state size different of state size"); + throw std::runtime_error("Prior::check() Prior " + type_ + " state size different of state size. " + print()); // check sigma size if (mode_ == "factor" and sigma_.size() != sb->getLocalSize()) - throw std::runtime_error("Prior " + type_ + " sigma size different of state local size"); + throw std::runtime_error("Prior::check() Prior " + type_ + " sigma size different of state local size. " + print()); // check sigma size if (dynamic_ and sigma_drift_.size() != sb->getLocalSize()) - throw std::runtime_error("Prior " + type_ + " sigma_drift size different of state local size"); + throw std::runtime_error("Prior::check() Prior " + type_ + " sigma_drift size different of state local size. " + print()); } std::string Prior::print() const { return "Prior " + type_ + "\n" + "mode: " + mode_ + "\n" - //+ "state: " + to_string(state_) + "\n" - //+ (mode_ == "factor" ? "sigma: " + to_string(sigma_) + "\n" : "") - + "dynamic: " + to_string(dynamic_) + "\n"; - //+ (dynamic_ ? "sigma_drift: "+ to_string(sigma_drift_) + "\n" : ""); + + "state: " + to_string(state_) + "\n" + + (mode_ == "factor" ? "sigma: " + to_string(sigma_) + "\n" : "") + + "dynamic: " + to_string(dynamic_) + "\n" + + (dynamic_ ? "sigma_drift: "+ to_string(sigma_drift_) + "\n" : ""); } } // namespace wolf \ No newline at end of file diff --git a/test/gtest_prior.cpp b/test/gtest_prior.cpp index 974600d199321f38b815a3b0c0c1ca206bd8141f..43a507e274269f9ce19cbc07fa2c42c6a1670592 100644 --- a/test/gtest_prior.cpp +++ b/test/gtest_prior.cpp @@ -23,13 +23,24 @@ #include "core/utils/utils_gtest.h" #include "core/common/wolf.h" #include "core/state_block/prior.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; + +auto vector0 = VectorXd(0); +auto vector1 = VectorXd::Random(1); +auto vector2 = VectorXd::Random(2); +auto vector3 = VectorXd::Random(3); +auto vector4 = VectorXd::Random(4); +auto vectorq = VectorXd::Random(4).normalized(); + struct PriorAsStruct { - char key; + std::string type; std::string mode; VectorXd state; VectorXd sigma; @@ -40,7 +51,7 @@ struct PriorAsStruct void testPrior(const Prior& P, const PriorAsStruct& pstruct) { - ASSERT_EQ(pstruct.key, P.getKey()); + ASSERT_EQ(pstruct.type, P.getType()); ASSERT_EQ(pstruct.mode, P.getMode()); ASSERT_MATRIX_APPROX(pstruct.state, P.getState(), 1e-8); @@ -70,18 +81,18 @@ void testPriors(const std::vector<PriorAsStruct>& setups, bool should_work) WOLF_INFO("Testing setups that should ", (should_work ? "" : "NOT"), " work..."); for (auto setup : setups) { - WOLF_DEBUG("prior setup: ", - "\n\tkey: ", std::string(1,setup.key), + WOLF_INFO("prior setup: ", + "\n\ttype: ", setup.type, "\n\tmode: ", setup.mode, "\n\tstate: ", setup.state.transpose(), "\n\tsigma: ", setup.sigma.transpose(), "\n\tdynamic: ", setup.dynamic, "\n\tsigma_drift: ", setup.sigma_drift.transpose()); if (should_work) - testPrior(Prior(setup.key, setup.mode, setup.state, setup.sigma, setup.dynamic, setup.sigma_drift), setup); + testPrior(Prior(setup.type, setup.mode, setup.state, setup.sigma, setup.dynamic, setup.sigma_drift), setup); else { - ASSERT_THROW(testPrior(Prior(setup.key, setup.mode, setup.state, setup.sigma, setup.dynamic, setup.sigma_drift), setup),std::runtime_error); + ASSERT_THROW(testPrior(Prior(setup.type, setup.mode, setup.state, setup.sigma, setup.dynamic, setup.sigma_drift), setup),std::runtime_error); } } } @@ -90,41 +101,51 @@ TEST(Prior, P) { std::vector<PriorAsStruct> setups_ok, setups_death; - // SHOULD FAIL: Nothing - not dynamic (not allowed for P and O) - setups_death.push_back(PriorAsStruct({'P',"nothing",VectorXd::Random(2),VectorXd::Random(0),false})); - setups_death.push_back(PriorAsStruct({'P',"nothing",VectorXd::Random(3),VectorXd::Random(0),false})); + // Nothing - not dynamic (not allowed for P and O) + setups_ok .push_back(PriorAsStruct({"P","nothing",vector2,vector0,false})); + setups_ok .push_back(PriorAsStruct({"P","nothing",vector3,vector0,false})); + setups_death.push_back(PriorAsStruct({"P","nothing",vector4,vector0,false})); // wrong state size - // SHOULD FAIL: Nothing - dynamic (not implemented) - setups_death.push_back(PriorAsStruct({'P',"nothing",VectorXd::Random(2),VectorXd::Random(0),true})); - setups_death.push_back(PriorAsStruct({'P',"nothing",VectorXd::Random(3),VectorXd::Random(0),true})); + // Nothing - dynamic + setups_ok .push_back(PriorAsStruct({"P","nothing",vector2,vector0,true,vector2})); + setups_ok .push_back(PriorAsStruct({"P","nothing",vector3,vector0,true,vector3})); + setups_death.push_back(PriorAsStruct({"P","nothing",vector4,vector0,true,vector4})); // wrong state size + setups_death.push_back(PriorAsStruct({"P","nothing",vector3,vector0,true,vector4})); // inconsistent size // Initial guess - not dynamic - setups_ok .push_back(PriorAsStruct({'P',"initial_guess",VectorXd::Random(2),VectorXd::Random(0),false})); - setups_ok .push_back(PriorAsStruct({'P',"initial_guess",VectorXd::Random(3),VectorXd::Random(0),false})); - setups_death.push_back(PriorAsStruct({'P',"initial_guess",VectorXd::Random(4),VectorXd::Random(0),false})); // wrong size + setups_ok .push_back(PriorAsStruct({"P","initial_guess",vector2,vector0,false})); + setups_ok .push_back(PriorAsStruct({"P","initial_guess",vector3,vector0,false})); + setups_death.push_back(PriorAsStruct({"P","initial_guess",vector4,vector0,false})); // wrong state size - // SHOULD FAIL: Initial guess - dynamic (not implemented) - setups_death.push_back(PriorAsStruct({'P',"initial_guess",VectorXd::Random(2),VectorXd::Random(0),true})); - setups_death.push_back(PriorAsStruct({'P',"initial_guess",VectorXd::Random(3),VectorXd::Random(0),true})); + // Initial guess - dynamic + setups_ok .push_back(PriorAsStruct({"P","initial_guess",vector2,vector0,true,vector2})); + setups_ok .push_back(PriorAsStruct({"P","initial_guess",vector3,vector0,true,vector3})); + setups_death.push_back(PriorAsStruct({"P","initial_guess",vector4,vector0,true,vector4})); // wrong state size + setups_death.push_back(PriorAsStruct({"P","initial_guess",vector3,vector0,true,vector4})); // inconsistent size // Fix - not dynamic - setups_ok .push_back(PriorAsStruct({'P',"fix",VectorXd::Random(2),VectorXd::Random(0),false})); - setups_ok .push_back(PriorAsStruct({'P',"fix",VectorXd::Random(3),VectorXd::Random(0),false})); - setups_death.push_back(PriorAsStruct({'P',"fix",VectorXd::Random(4),VectorXd::Random(0),false})); // wrong size - - // SHOULD FAIL: Fix - dynamic (not implemented) - setups_death.push_back(PriorAsStruct({'P',"fix",VectorXd::Random(2),VectorXd::Random(0),true})); - setups_death.push_back(PriorAsStruct({'P',"fix",VectorXd::Random(3),VectorXd::Random(0),true})); + setups_ok .push_back(PriorAsStruct({"P","fix",vector2,vector0,false})); + setups_ok .push_back(PriorAsStruct({"P","fix",vector3,vector0,false})); + setups_death.push_back(PriorAsStruct({"P","fix",vector4,vector0,false})); // wrong size + // Fix - dynamic + setups_ok .push_back(PriorAsStruct({"P","fix",vector2,vector0,true,vector2})); + setups_ok .push_back(PriorAsStruct({"P","fix",vector3,vector0,true,vector3})); + setups_death.push_back(PriorAsStruct({"P","fix",vector4,vector0,true,vector4})); // wrong state size + setups_death.push_back(PriorAsStruct({"P","fix",vector3,vector0,true,vector4})); // inconsistent size + // Factor - not dynamic - setups_ok .push_back(PriorAsStruct({'P',"factor",VectorXd::Random(2),VectorXd::Random(2),false})); - setups_ok .push_back(PriorAsStruct({'P',"factor",VectorXd::Random(3),VectorXd::Random(3),false})); - setups_death.push_back(PriorAsStruct({'P',"factor",VectorXd::Random(4),VectorXd::Random(4),false})); // wrong size - setups_death.push_back(PriorAsStruct({'P',"factor",VectorXd::Random(2),VectorXd::Random(3),false})); // inconsistent size + setups_ok .push_back(PriorAsStruct({"P","factor",vector2,vector2,false})); + setups_ok .push_back(PriorAsStruct({"P","factor",vector3,vector3,false})); + setups_death.push_back(PriorAsStruct({"P","factor",vector4,vector4,false})); // wrong state size + setups_death.push_back(PriorAsStruct({"P","factor",vector2,vector3,false})); // inconsistent size - // SHOULD FAIL: Factor - dynamic (not implemented) - setups_death.push_back(PriorAsStruct({'P',"factor",VectorXd::Random(2),VectorXd::Random(2),true})); - setups_death.push_back(PriorAsStruct({'P',"factor",VectorXd::Random(3),VectorXd::Random(3),true})); + // Factor - dynamic + setups_ok .push_back(PriorAsStruct({"P","factor",vector2,vector2,true,vector2})); + setups_ok .push_back(PriorAsStruct({"P","factor",vector3,vector3,true,vector3})); + setups_death.push_back(PriorAsStruct({"P","factor",vector4,vector4,true,vector4})); // wrong state size + setups_death.push_back(PriorAsStruct({"P","factor",vector3,vector3,true,vector4})); // inconsistent size + setups_death.push_back(PriorAsStruct({"P","factor",vector3,vector4,true,vector3})); // inconsistent size // TEST SETUPS testPriors(setups_ok, true); @@ -135,103 +156,336 @@ TEST(Prior, O) { std::vector<PriorAsStruct> setups_ok, setups_death; - // SHOULD FAIL: Nothing - not dynamic (not allowed for P and O) - setups_death.push_back(PriorAsStruct({'O',"nothing",VectorXd::Random(1),VectorXd::Random(0),false})); - setups_death.push_back(PriorAsStruct({'O',"nothing",VectorXd::Random(4).normalized(),VectorXd::Random(0),false})); + // nothing - not dynamic + setups_ok .push_back(PriorAsStruct({"O","nothing",vector1,vector0,false})); + setups_ok .push_back(PriorAsStruct({"O","nothing",vectorq,vector0,false})); + setups_death.push_back(PriorAsStruct({"O","nothing",vector4,vector0,false})); // not normalized + setups_death.push_back(PriorAsStruct({"O","nothing",vector3,vector0,false})); // wrong size - // SHOULD FAIL: Nothing - dynamic (not implemented) - setups_death.push_back(PriorAsStruct({'O',"nothing",VectorXd::Random(1),VectorXd::Random(0),true})); - setups_death.push_back(PriorAsStruct({'O',"nothing",VectorXd::Random(4).normalized(),VectorXd::Random(0),true})); + // Nothing - dynamic + setups_ok .push_back(PriorAsStruct({"O","nothing",vector1,vector0,true,vector1})); + setups_ok .push_back(PriorAsStruct({"O","nothing",vectorq,vector0,true,vector3})); + setups_death.push_back(PriorAsStruct({"O","nothing",vector4,vector0,true,vector3})); // not normalized + setups_death.push_back(PriorAsStruct({"O","nothing",vector3,vector0,true,vector3})); // wrong size + setups_death.push_back(PriorAsStruct({"O","nothing",vectorq,vector0,true,vector4})); // inconsistent size // Initial guess - not dynamic - setups_ok .push_back(PriorAsStruct({'O',"initial_guess",VectorXd::Random(1),VectorXd::Random(0),false})); - setups_ok .push_back(PriorAsStruct({'O',"initial_guess",VectorXd::Random(4).normalized(),VectorXd::Random(0),false})); - setups_death.push_back(PriorAsStruct({'O',"initial_guess",VectorXd::Random(3),VectorXd::Random(0),false})); // wrong size - setups_death.push_back(PriorAsStruct({'O',"initial_guess",VectorXd::Random(4),VectorXd::Random(0),false})); // not normalized - - // SHOULD FAIL: Initial guess - dynamic (not implemented) - setups_death.push_back(PriorAsStruct({'O',"initial_guess",VectorXd::Random(1),VectorXd::Random(0),true})); - setups_death.push_back(PriorAsStruct({'O',"initial_guess",VectorXd::Random(4).normalized(),VectorXd::Random(0),true})); + setups_ok .push_back(PriorAsStruct({"O","initial_guess",vector1,vector0,false})); + setups_ok .push_back(PriorAsStruct({"O","initial_guess",vectorq,vector0,false})); + setups_death.push_back(PriorAsStruct({"O","initial_guess",vector4,vector0,false})); // not normalized + setups_death.push_back(PriorAsStruct({"O","initial_guess",vector3,vector0,false})); // wrong size + + // Initial guess - dynamic + setups_ok .push_back(PriorAsStruct({"O","initial_guess",vector1,vector0,true,vector1})); + setups_ok .push_back(PriorAsStruct({"O","initial_guess",vectorq,vector0,true,vector3})); + setups_death.push_back(PriorAsStruct({"O","initial_guess",vector4,vector0,true,vector3})); // not normalized + setups_death.push_back(PriorAsStruct({"O","initial_guess",vector3,vector0,true,vector3})); // wrong size + setups_death.push_back(PriorAsStruct({"O","initial_guess",vectorq,vector0,true,vector4})); // inconsistent size // Fix - not dynamic - setups_ok .push_back(PriorAsStruct({'O',"fix",VectorXd::Random(1),VectorXd::Random(0),false})); - setups_ok .push_back(PriorAsStruct({'O',"fix",VectorXd::Random(4).normalized(),VectorXd::Random(0),false})); - setups_death.push_back(PriorAsStruct({'O',"fix",VectorXd::Random(3),VectorXd::Random(0),false})); // wrong size - setups_death.push_back(PriorAsStruct({'O',"fix",VectorXd::Random(4),VectorXd::Random(0),false})); // not normalized + setups_ok .push_back(PriorAsStruct({"O","fix",vector1,vector0,false})); + setups_ok .push_back(PriorAsStruct({"O","fix",vectorq,vector0,false})); + setups_death.push_back(PriorAsStruct({"O","fix",vector3,vector0,false})); // wrong size + setups_death.push_back(PriorAsStruct({"O","fix",vector4,vector0,false})); // not normalized - // SHOULD FAIL: Fix - dynamic (not implemented) - setups_death.push_back(PriorAsStruct({'O',"fix",VectorXd::Random(1),VectorXd::Random(0),true})); - setups_death.push_back(PriorAsStruct({'O',"fix",VectorXd::Random(4).normalized(),VectorXd::Random(0),true})); + // Fix - dynamic + setups_ok .push_back(PriorAsStruct({"O","fix",vector1,vector0,true,vector1})); + setups_ok .push_back(PriorAsStruct({"O","fix",vectorq,vector0,true,vector3})); + setups_death.push_back(PriorAsStruct({"O","fix",vector4,vector0,true,vector3})); // not normalized + setups_death.push_back(PriorAsStruct({"O","fix",vector3,vector0,true,vector3})); // wrong size + setups_death.push_back(PriorAsStruct({"O","fix",vectorq,vector0,true,vector4})); // inconsistent size // Factor - not dynamic - setups_ok .push_back(PriorAsStruct({'O',"factor",VectorXd::Random(1),VectorXd::Random(1),false})); - setups_ok .push_back(PriorAsStruct({'O',"factor",VectorXd::Random(4).normalized(),VectorXd::Random(3),false})); - setups_death.push_back(PriorAsStruct({'O',"factor",VectorXd::Random(3).normalized(),VectorXd::Random(3),false})); // wrong state size - setups_death.push_back(PriorAsStruct({'O',"factor",VectorXd::Random(4).normalized(),VectorXd::Random(4),false})); // wrong sigma size - setups_death.push_back(PriorAsStruct({'O',"factor",VectorXd::Random(4),VectorXd::Random(3),false})); // not normalized + setups_ok .push_back(PriorAsStruct({"O","factor",vector1,vector1,false})); + setups_ok .push_back(PriorAsStruct({"O","factor",vectorq,vector3,false})); + setups_death.push_back(PriorAsStruct({"O","factor",vector4,vector3,false})); // not normalized + setups_death.push_back(PriorAsStruct({"O","factor",vector3,vector3,false})); // wrong size + setups_death.push_back(PriorAsStruct({"O","factor",vectorq,vector4,false})); // inconsistent size - // SHOULD FAIL: Factor - dynamic (not implemented) - setups_death.push_back(PriorAsStruct({'O',"factor",VectorXd::Random(1),VectorXd::Random(2),true})); - setups_death.push_back(PriorAsStruct({'O',"factor",VectorXd::Random(4).normalized(),VectorXd::Random(3),true})); + // Factor - dynamic + setups_ok .push_back(PriorAsStruct({"O","factor",vector1,vector1,true,vector1})); + setups_ok .push_back(PriorAsStruct({"O","factor",vectorq,vector3,true,vector3})); + setups_death.push_back(PriorAsStruct({"O","factor",vector4,vector3,true,vector3})); // not normalized + setups_death.push_back(PriorAsStruct({"O","factor",vector3,vector3,true,vector3})); // wrong size + setups_death.push_back(PriorAsStruct({"O","factor",vectorq,vector3,true,vector4})); // inconsistent size + setups_death.push_back(PriorAsStruct({"O","factor",vectorq,vector4,true,vector3})); // inconsistent size // TEST SETUPS testPriors(setups_ok, true); testPriors(setups_death, false); } -TEST(Prior, K) +TEST(Prior, StateBlock) { std::vector<PriorAsStruct> setups_ok, setups_death; - // Nothing - not dynamic - setups_ok .push_back(PriorAsStruct({'K',"nothing",VectorXd::Random(1),VectorXd::Random(0),false})); - setups_ok .push_back(PriorAsStruct({'K',"nothing",VectorXd::Random(3),VectorXd::Random(0),false})); - setups_ok .push_back(PriorAsStruct({'K',"nothing",VectorXd::Random(6),VectorXd::Random(0),false})); + // Unknown type -> StateBlock + setups_ok .push_back(PriorAsStruct({"K","nothing",vector1,vector0,false})); + + // Nothing - not dynamic (not allowed for P and O) + setups_ok .push_back(PriorAsStruct({"StateBlock","nothing",vector2,vector0,false})); + setups_ok .push_back(PriorAsStruct({"StateBlock","nothing",vector3,vector0,false})); + setups_ok .push_back(PriorAsStruct({"StateBlock","nothing",vector4,vector0,false})); // Nothing - dynamic - setups_ok .push_back(PriorAsStruct({'K',"nothing",VectorXd::Random(1),VectorXd::Random(0),true,VectorXd::Random(1)})); - setups_ok .push_back(PriorAsStruct({'K',"nothing",VectorXd::Random(3),VectorXd::Random(0),true,VectorXd::Random(3)})); - setups_death.push_back(PriorAsStruct({'K',"nothing",VectorXd::Random(3),VectorXd::Random(0),true,VectorXd::Random(2)})); // wrong size + setups_ok .push_back(PriorAsStruct({"StateBlock","nothing",vector2,vector0,true,vector2})); + setups_ok .push_back(PriorAsStruct({"StateBlock","nothing",vector3,vector0,true,vector3})); + setups_ok .push_back(PriorAsStruct({"StateBlock","nothing",vector4,vector0,true,vector4})); + setups_death.push_back(PriorAsStruct({"StateBlock","nothing",vector3,vector0,true,vector4})); // inconsistent size // Initial guess - not dynamic - setups_ok .push_back(PriorAsStruct({'K',"initial_guess",VectorXd::Random(1),VectorXd::Random(0),false})); - setups_ok .push_back(PriorAsStruct({'K',"initial_guess",VectorXd::Random(3),VectorXd::Random(0),false})); - setups_ok .push_back(PriorAsStruct({'K',"initial_guess",VectorXd::Random(6),VectorXd::Random(0),false})); + setups_ok .push_back(PriorAsStruct({"StateBlock","initial_guess",vector2,vector0,false})); + setups_ok .push_back(PriorAsStruct({"StateBlock","initial_guess",vector3,vector0,false})); + setups_ok .push_back(PriorAsStruct({"StateBlock","initial_guess",vector4,vector0,false})); // Initial guess - dynamic - setups_ok .push_back(PriorAsStruct({'K',"initial_guess",VectorXd::Random(1),VectorXd::Random(0),true,VectorXd::Random(1)})); - setups_ok .push_back(PriorAsStruct({'K',"initial_guess",VectorXd::Random(3),VectorXd::Random(0),true,VectorXd::Random(3)})); - setups_death.push_back(PriorAsStruct({'K',"initial_guess",VectorXd::Random(3),VectorXd::Random(0),true,VectorXd::Random(2)})); // wrong size + setups_ok .push_back(PriorAsStruct({"StateBlock","initial_guess",vector2,vector0,true,vector2})); + setups_ok .push_back(PriorAsStruct({"StateBlock","initial_guess",vector3,vector0,true,vector3})); + setups_ok .push_back(PriorAsStruct({"StateBlock","initial_guess",vector4,vector0,true,vector4})); + setups_death.push_back(PriorAsStruct({"StateBlock","initial_guess",vector3,vector0,true,vector4})); // inconsistent size // Fix - not dynamic - setups_ok .push_back(PriorAsStruct({'K',"fix",VectorXd::Random(1),VectorXd::Random(0),false})); - setups_ok .push_back(PriorAsStruct({'K',"fix",VectorXd::Random(3),VectorXd::Random(0),false})); - setups_ok .push_back(PriorAsStruct({'K',"fix",VectorXd::Random(6),VectorXd::Random(0),false})); + setups_ok .push_back(PriorAsStruct({"StateBlock","fix",vector2,vector0,false})); + setups_ok .push_back(PriorAsStruct({"StateBlock","fix",vector3,vector0,false})); + setups_ok .push_back(PriorAsStruct({"StateBlock","fix",vector4,vector0,false})); // wrong size // Fix - dynamic - setups_ok .push_back(PriorAsStruct({'K',"fix",VectorXd::Random(1),VectorXd::Random(0),true,VectorXd::Random(1)})); - setups_ok .push_back(PriorAsStruct({'K',"fix",VectorXd::Random(3),VectorXd::Random(0),true,VectorXd::Random(3)})); - setups_death.push_back(PriorAsStruct({'K',"fix",VectorXd::Random(3),VectorXd::Random(0),true,VectorXd::Random(2)})); // wrong size + setups_ok .push_back(PriorAsStruct({"StateBlock","fix",vector2,vector0,true,vector2})); + setups_ok .push_back(PriorAsStruct({"StateBlock","fix",vector3,vector0,true,vector3})); + setups_ok .push_back(PriorAsStruct({"StateBlock","fix",vector4,vector0,true,vector4})); + setups_death.push_back(PriorAsStruct({"StateBlock","fix",vector3,vector0,true,vector4})); // inconsistent size + + // Factor - not dynamic + setups_ok .push_back(PriorAsStruct({"StateBlock","factor",vector2,vector2,false})); + setups_ok .push_back(PriorAsStruct({"StateBlock","factor",vector3,vector3,false})); + setups_ok .push_back(PriorAsStruct({"StateBlock","factor",vector4,vector4,false})); + setups_death.push_back(PriorAsStruct({"StateBlock","factor",vector2,vector3,false})); // inconsistent size + + // Factor - dynamic + setups_ok .push_back(PriorAsStruct({"StateBlock","factor",vector2,vector2,true,vector2})); + setups_ok .push_back(PriorAsStruct({"StateBlock","factor",vector3,vector3,true,vector3})); + setups_ok .push_back(PriorAsStruct({"StateBlock","factor",vector4,vector4,true,vector4})); + setups_death.push_back(PriorAsStruct({"StateBlock","factor",vector3,vector3,true,vector4})); // inconsistent size + setups_death.push_back(PriorAsStruct({"StateBlock","factor",vector3,vector4,true,vector3})); // inconsistent size + + // TEST SETUPS + testPriors(setups_ok, true); + testPriors(setups_death, false); +} +TEST(Prior, StateAngle) +{ + std::vector<PriorAsStruct> setups_ok, setups_death; + + // nothing - not dynamic + setups_ok .push_back(PriorAsStruct({"StateAngle","nothing",vector1,vector0,false})); + setups_death.push_back(PriorAsStruct({"StateAngle","nothing",vector3,vector0,false})); // wrong size + + // Nothing - dynamic + setups_ok .push_back(PriorAsStruct({"StateAngle","nothing",vector1,vector0,true,vector1})); + setups_death.push_back(PriorAsStruct({"StateAngle","nothing",vector3,vector0,true,vector3})); // wrong size + setups_death.push_back(PriorAsStruct({"StateAngle","nothing",vector1,vector0,true,vector4})); // inconsistent size + + // Initial guess - not dynamic + setups_ok .push_back(PriorAsStruct({"StateAngle","initial_guess",vector1,vector0,false})); + setups_death.push_back(PriorAsStruct({"StateAngle","initial_guess",vector3,vector0,false})); // wrong size + + // Initial guess - dynamic + setups_ok .push_back(PriorAsStruct({"StateAngle","initial_guess",vector1,vector0,true,vector1})); + setups_death.push_back(PriorAsStruct({"StateAngle","initial_guess",vector3,vector0,true,vector3})); // wrong size + setups_death.push_back(PriorAsStruct({"StateAngle","initial_guess",vector1,vector0,true,vector4})); // inconsistent size + + // Fix - not dynamic + setups_ok .push_back(PriorAsStruct({"StateAngle","fix",vector1,vector0,false})); + setups_death.push_back(PriorAsStruct({"StateAngle","fix",vector3,vector0,false})); // wrong size + + // Fix - dynamic + setups_ok .push_back(PriorAsStruct({"StateAngle","fix",vector1,vector0,true,vector1})); + setups_death.push_back(PriorAsStruct({"StateAngle","fix",vector3,vector0,true,vector3})); // wrong size + setups_death.push_back(PriorAsStruct({"StateAngle","fix",vector1,vector0,true,vector4})); // inconsistent size // Factor - not dynamic - setups_ok .push_back(PriorAsStruct({'K',"factor",VectorXd::Random(1),VectorXd::Random(1),false})); - setups_ok .push_back(PriorAsStruct({'K',"factor",VectorXd::Random(3),VectorXd::Random(3),false})); - setups_ok .push_back(PriorAsStruct({'K',"factor",VectorXd::Random(6),VectorXd::Random(6),false})); - setups_death.push_back(PriorAsStruct({'K',"factor",VectorXd::Random(4),VectorXd::Random(3),false})); // wrong sigma size + setups_ok .push_back(PriorAsStruct({"StateAngle","factor",vector1,vector1,false})); + setups_death.push_back(PriorAsStruct({"StateAngle","factor",vector3,vector3,false})); // wrong size + setups_death.push_back(PriorAsStruct({"StateAngle","factor",vector1,vector4,false})); // inconsistent size // Factor - dynamic - setups_ok .push_back(PriorAsStruct({'K',"factor",VectorXd::Random(1),VectorXd::Random(1),true,VectorXd::Random(1)})); - setups_ok .push_back(PriorAsStruct({'K',"factor",VectorXd::Random(3),VectorXd::Random(3),true,VectorXd::Random(3)})); - setups_death.push_back(PriorAsStruct({'K',"factor",VectorXd::Random(3),VectorXd::Random(3),true,VectorXd::Random(2)})); // wrong sigma_drift size - setups_death.push_back(PriorAsStruct({'K',"factor",VectorXd::Random(3),VectorXd::Random(2),true,VectorXd::Random(3)})); // wrong sigma size + setups_ok .push_back(PriorAsStruct({"StateAngle","factor",vector1,vector1,true,vector1})); + setups_death.push_back(PriorAsStruct({"StateAngle","factor",vector3,vector3,true,vector3})); // wrong size + setups_death.push_back(PriorAsStruct({"StateAngle","factor",vector1,vector3,true,vector1})); // inconsistent size + setups_death.push_back(PriorAsStruct({"StateAngle","factor",vector1,vector1,true,vector3})); // inconsistent size // TEST SETUPS testPriors(setups_ok, true); testPriors(setups_death, false); } -// TODO: param_server constructor! +TEST(Prior, StateQuaternion) +{ + std::vector<PriorAsStruct> setups_ok, setups_death; + + // nothing - not dynamic + setups_ok .push_back(PriorAsStruct({"StateQuaternion","nothing",vectorq,vector0,false})); + setups_death.push_back(PriorAsStruct({"StateQuaternion","nothing",vector4,vector0,false})); // not normalized + setups_death.push_back(PriorAsStruct({"StateQuaternion","nothing",vector3,vector0,false})); // wrong size + + // Nothing - dynamic + setups_ok .push_back(PriorAsStruct({"StateQuaternion","nothing",vectorq,vector0,true,vector3})); + setups_death.push_back(PriorAsStruct({"StateQuaternion","nothing",vector4,vector0,true,vector3})); // not normalized + setups_death.push_back(PriorAsStruct({"StateQuaternion","nothing",vector3,vector0,true,vector3})); // wrong size + setups_death.push_back(PriorAsStruct({"StateQuaternion","nothing",vectorq,vector0,true,vector4})); // inconsistent size + + // Initial guess - not dynamic + setups_ok .push_back(PriorAsStruct({"StateQuaternion","initial_guess",vectorq,vector0,false})); + setups_death.push_back(PriorAsStruct({"StateQuaternion","initial_guess",vector4,vector0,false})); // not normalized + setups_death.push_back(PriorAsStruct({"StateQuaternion","initial_guess",vector3,vector0,false})); // wrong size + + // Initial guess - dynamic + setups_ok .push_back(PriorAsStruct({"StateQuaternion","initial_guess",vectorq,vector0,true,vector3})); + setups_death.push_back(PriorAsStruct({"StateQuaternion","initial_guess",vector4,vector0,true,vector3})); // not normalized + setups_death.push_back(PriorAsStruct({"StateQuaternion","initial_guess",vector3,vector0,true,vector3})); // wrong size + setups_death.push_back(PriorAsStruct({"StateQuaternion","initial_guess",vectorq,vector0,true,vector4})); // inconsistent size + + // Fix - not dynamic + setups_ok .push_back(PriorAsStruct({"StateQuaternion","fix",vectorq,vector0,false})); + setups_death.push_back(PriorAsStruct({"StateQuaternion","fix",vector3,vector0,false})); // wrong size + setups_death.push_back(PriorAsStruct({"StateQuaternion","fix",vector4,vector0,false})); // not normalized + + // Fix - dynamic + setups_ok .push_back(PriorAsStruct({"StateQuaternion","fix",vectorq,vector0,true,vector3})); + setups_death.push_back(PriorAsStruct({"StateQuaternion","fix",vector4,vector0,true,vector3})); // not normalized + setups_death.push_back(PriorAsStruct({"StateQuaternion","fix",vector3,vector0,true,vector3})); // wrong size + setups_death.push_back(PriorAsStruct({"StateQuaternion","fix",vectorq,vector0,true,vector4})); // inconsistent size + + // Factor - not dynamic + setups_ok .push_back(PriorAsStruct({"StateQuaternion","factor",vectorq,vector3,false})); + setups_death.push_back(PriorAsStruct({"StateQuaternion","factor",vector4,vector3,false})); // not normalized + setups_death.push_back(PriorAsStruct({"StateQuaternion","factor",vector3,vector3,false})); // wrong size + setups_death.push_back(PriorAsStruct({"StateQuaternion","factor",vectorq,vector4,false})); // inconsistent size + // Factor - dynamic + setups_ok .push_back(PriorAsStruct({"StateQuaternion","factor",vectorq,vector3,true,vector3})); + setups_death.push_back(PriorAsStruct({"StateQuaternion","factor",vector4,vector3,true,vector3})); // not normalized + setups_death.push_back(PriorAsStruct({"StateQuaternion","factor",vector3,vector3,true,vector3})); // wrong size + setups_death.push_back(PriorAsStruct({"StateQuaternion","factor",vectorq,vector3,true,vector4})); // inconsistent size + setups_death.push_back(PriorAsStruct({"StateQuaternion","factor",vectorq,vector4,true,vector3})); // inconsistent size + + // TEST SETUPS + testPriors(setups_ok, true); + testPriors(setups_death, false); +} + +TEST(Prior, ParamsServer) +{ + ParserYaml parser = ParserYaml("test/yaml/params_prior.yaml", wolf_root); + ParamsServer server = ParamsServer(parser.getParams()); + + std::vector<std::string> keys({"P","O"}); + std::vector<int> dims({2,3}); + std::vector<std::string> modes({"nothing","initial_guess","fix","factor"}); + std::vector<bool> dynamics({false,true}); + + VectorXd p_state(4), o_state(4), po_sigma(4); + p_state << 1, 2, 3, 4; + o_state << 1, 0, 0, 0; + po_sigma << 0.1, 0.2, 0.3, 0.4; + + // P & O + for (auto key : keys) + for (auto dim : dims) + for (auto mode : modes) + for (auto dynamic : dynamics) + { + std::string prefix = key + to_string(dim) + "_" + mode + (dynamic ? "_dynamic" : ""); + + WOLF_INFO("Creating prior from prefix ", prefix); + auto P = Prior(prefix, key, server); + + // Checks + ASSERT_EQ(P.getMode(), mode); + ASSERT_EQ(P.isDynamic(), dynamic); + + VectorXd state = p_state.head(dim); + VectorXd sigma = po_sigma.head(dim); + if (key == "O") + { + state = o_state.head(dim == 2 ? 1 : 4); + sigma = po_sigma.head(dim == 2 ? 1 : 3); + } + + if (mode != "nothing") + { + ASSERT_MATRIX_APPROX(P.getState(),state,wolf::Constants::EPS); + } + if (dynamic) + { + ASSERT_MATRIX_APPROX(P.getSigmaDrift(),sigma,wolf::Constants::EPS); + } + if (mode == "factor") + { + ASSERT_MATRIX_APPROX(P.getSigma(),sigma,wolf::Constants::EPS); + } + } + + // I + for (auto mode : modes) + for (auto dynamic : dynamics) + { + std::string prefix = "I_" + mode + (dynamic ? "_dynamic" : ""); + WOLF_INFO("Creating prior from prefix ", prefix); + auto P = Prior(prefix, "StateBlock", server); + ASSERT_EQ(P.getMode(), mode); + ASSERT_EQ(P.isDynamic(), dynamic); + if (mode != "nothing") + { + ASSERT_MATRIX_APPROX(P.getState(),p_state,wolf::Constants::EPS); + } + if (dynamic) + { + ASSERT_MATRIX_APPROX(P.getSigmaDrift(),po_sigma,wolf::Constants::EPS); + } + if (mode == "factor") + { + ASSERT_MATRIX_APPROX(P.getSigma(),po_sigma,wolf::Constants::EPS); + } + } +} + +TEST(Prior, ParamsServerWrong) +{ + ParserYaml parser = ParserYaml("test/yaml/params_prior_wrong.yaml", wolf_root); + ParamsServer server = ParamsServer(parser.getParams()); + + std::vector<std::string> keys({"P","O"}); + std::vector<int> dims({2,3}); + std::vector<std::string> modes({"nothing","initial_guess","fix","factor"}); + std::vector<bool> dynamics({false,true}); + + VectorXd p_state(4), o_state(4), po_sigma(4); + p_state << 1, 2, 3, 4; + o_state << 1, 0, 0, 0; + po_sigma << 0.1, 0.2, 0.3, 0.4; + + // P & O + for (auto key : keys) + for (auto dim : dims) + for (auto mode : modes) + for (auto dynamic : dynamics) + { + std::string prefix = key + to_string(dim) + "_" + mode + (dynamic ? "_dynamic" : ""); + + WOLF_INFO("Creating prior from prefix ", prefix); + ASSERT_THROW(auto P = Prior(prefix, key, server),std::runtime_error); + } + + // I + for (auto mode : modes) + for (auto dynamic : dynamics) + { + std::string prefix = "I_" + mode + (dynamic ? "_dynamic" : ""); + WOLF_INFO("Creating prior from prefix ", prefix); + ASSERT_THROW(auto P = Prior(prefix, "StateBlock", server),std::runtime_error); + } +} int main(int argc, char **argv) { diff --git a/test/gtest_sensor_base.cpp b/test/gtest_sensor_base.cpp index 5f2e785f147ed275ce48c73ac6d922d028fd2f73..0a367cd3047d706bbf46ce4697a4ea9c67fbd388 100644 --- a/test/gtest_sensor_base.cpp +++ b/test/gtest_sensor_base.cpp @@ -43,8 +43,8 @@ TEST(SensorBase, POfix) auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 2, - Priors({{'P',Prior('P',"fix", Eigen::Vector2d::Zero())}, - {'O',Prior('O',"fix", Eigen::Vector1d::Zero())}}), + Priors({{'P',Prior("P","fix", Eigen::Vector2d::Zero())}, + {'O',Prior("O","fix", Eigen::Vector1d::Zero())}}), params); ASSERT_MATRIX_APPROX(noise_std, S->getNoiseStd(), 1e-8); diff --git a/test/yaml/params_prior.yaml b/test/yaml/params_prior.yaml new file mode 100644 index 0000000000000000000000000000000000000000..1b1955cdd541043243419ea56ed0e4be17201404 --- /dev/null +++ b/test/yaml/params_prior.yaml @@ -0,0 +1,228 @@ +config: + problem: + dim: 2 + + # P in 2D + P2_nothing: + mode: nothing + dynamic: false + + 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] + sigma: [0.1, 0.2] + dynamic: false + + P2_nothing_dynamic: + mode: nothing + dynamic: true + sigma_drift: [0.1, 0.2] + + P2_initial_guess_dynamic: + mode: initial_guess + state: [1, 2] + dynamic: true + sigma_drift: [0.1, 0.2] + + P2_fix_dynamic: + mode: fix + state: [1, 2] + dynamic: true + sigma_drift: [0.1, 0.2] + + P2_factor_dynamic: + mode: factor + state: [1, 2] + sigma: [0.1, 0.2] + dynamic: true + sigma_drift: [0.1, 0.2] + + # P in 3D + P3_nothing: + mode: nothing + dynamic: false + + 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] + sigma: [0.1, 0.2, 0.3] + dynamic: false + + P3_nothing_dynamic: + mode: nothing + dynamic: true + sigma_drift: [0.1, 0.2, 0.3] + + P3_initial_guess_dynamic: + mode: initial_guess + state: [1, 2, 3] + dynamic: true + sigma_drift: [0.1, 0.2, 0.3] + + P3_fix_dynamic: + mode: fix + state: [1, 2, 3] + dynamic: true + sigma_drift: [0.1, 0.2, 0.3] + + P3_factor_dynamic: + mode: factor + state: [1, 2, 3] + sigma: [0.1, 0.2, 0.3] + dynamic: true + sigma_drift: [0.1, 0.2, 0.3] + + # O in 2D + O2_nothing: + mode: nothing + dynamic: false + + O2_initial_guess: + mode: initial_guess + state: [1] + dynamic: false + + O2_fix: + mode: fix + state: [1] + dynamic: false + + O2_factor: + mode: factor + state: [1] + sigma: [0.1] + dynamic: false + + O2_nothing_dynamic: + mode: nothing + dynamic: true + sigma_drift: [0.1] + + O2_initial_guess_dynamic: + mode: initial_guess + state: [1] + dynamic: true + sigma_drift: [0.1] + + O2_fix_dynamic: + mode: fix + state: [1] + dynamic: true + sigma_drift: [0.1] + + O2_factor_dynamic: + mode: factor + state: [1] + sigma: [0.1] + dynamic: true + sigma_drift: [0.1] + + # O in 3D + O3_nothing: + mode: nothing + dynamic: false + + 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] + sigma: [0.1, 0.2, 0.3] + dynamic: false + + O3_nothing_dynamic: + mode: nothing + dynamic: true + sigma_drift: [0.1, 0.2, 0.3] + + O3_initial_guess_dynamic: + mode: initial_guess + state: [1, 0, 0, 0] + dynamic: true + sigma_drift: [0.1, 0.2, 0.3] + + O3_fix_dynamic: + mode: fix + state: [1, 0, 0, 0] + dynamic: true + sigma_drift: [0.1, 0.2, 0.3] + + O3_factor_dynamic: + mode: factor + state: [1, 0, 0, 0] + sigma: [0.1, 0.2, 0.3] + dynamic: true + sigma_drift: [0.1, 0.2, 0.3] + + # I + I_nothing: + mode: nothing + dynamic: false + + 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] + sigma: [0.1, 0.2, 0.3, 0.4] + dynamic: false + + I_nothing_dynamic: + mode: nothing + dynamic: true + sigma_drift: [0.1, 0.2, 0.3, 0.4] + + I_initial_guess_dynamic: + mode: initial_guess + state: [1, 2, 3, 4] + dynamic: true + sigma_drift: [0.1, 0.2, 0.3, 0.4] + + I_fix_dynamic: + mode: fix + state: [1, 2, 3, 4] + dynamic: true + sigma_drift: [0.1, 0.2, 0.3, 0.4] + + I_factor_dynamic: + mode: factor + state: [1, 2, 3, 4] + sigma: [0.1, 0.2, 0.3, 0.4] + dynamic: true + sigma_drift: [0.1, 0.2, 0.3, 0.4] \ No newline at end of file diff --git a/test/yaml/params_prior_wrong.yaml b/test/yaml/params_prior_wrong.yaml new file mode 100644 index 0000000000000000000000000000000000000000..5a40d4743123e8e777016d86cbcb7aa178a0ed66 --- /dev/null +++ b/test/yaml/params_prior_wrong.yaml @@ -0,0 +1,228 @@ +config: + problem: + dim: 2 + + # P in 2D + P2_nothing: + mode: no # wrong mode + dynamic: false + + 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] + sigma: [0.1, 0.2, 0.3] # wrong size + dynamic: false + + P2_nothing_dynamic: + mode: nothing + dynamic: true + #sigma_drift: [0.1, 0.2] # missing + + P2_initial_guess_dynamic: + mode: initial_guess + state: [1, 2] + dynamic: true + #sigma_drift: [0.1, 0.2] #missing + + P2_fix_dynamic: + mode: fix + state: [1, 2] + dynamic: true + sigma_drift: [0.1, 0.2, 0.3] # wrong size + + P2_factor_dynamic: + mode: factor + state: [1, 2] + #sigma: [0.1, 0.2] # missing + dynamic: true + sigma_drift: [0.1, 0.2] + + # P in 3D + P3_nothing: + mode: nothing + #dynamic: false # missing + + 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] + sigma: [0.1, 0.2] # wrong size + dynamic: false + + P3_nothing_dynamic: + mode: no # wrong mode + dynamic: true + sigma_drift: [0.1, 0.2, 0.3] + + P3_initial_guess_dynamic: + mode: initial_guess + state: [1, 2, 3] + dynamic: true + sigma_drift: [0.1, 0.2] # wrong size + + P3_fix_dynamic: + mode: fix + state: [1, 2, 3] + dynamic: true + #sigma_drift: [0.1, 0.2] # missing + + P3_factor_dynamic: + mode: factor + state: [1, 2, 3] + sigma: [0.1, 0.2] # wrong size + dynamic: true + sigma_drift: [0.1, 0.2, 0.3] + + # O in 2D + O2_nothing: + mode: no # wrong mode + dynamic: false + + 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] + sigma: [0.1, 0.2] # wrong size + dynamic: false + + O2_nothing_dynamic: + mode: nothing + dynamic: true + sigma_drift: [0.1 0.2] # wrong size + + O2_initial_guess_dynamic: + mode: initial_guess + state: [1] + dynamic: true + #sigma_drift: [0.1] # missing + + O2_fix_dynamic: + mode: fix + state: [1 2] # wrong size + dynamic: true + sigma_drift: [0.1] + + O2_factor_dynamic: + mode: factor + state: [1] + sigma: [0.1 0.2] # wrong size + dynamic: true + sigma_drift: [0.1] + + # O in 3D + O3_nothing: + mode: no # wrong mode + dynamic: false + + 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] + sigma: [0.1, 0.2] # wrong size + dynamic: false + + O3_nothing_dynamic: + mode: nothing + #dynamic: true # missing + sigma_drift: [0.1, 0.2, 0.3] + + O3_initial_guess_dynamic: + mode: initial_guess + state: [1, 0, 0] # wrong size + dynamic: true + sigma_drift: [0.1, 0.2, 0.3] + + O3_fix_dynamic: + mode: fix + state: [1, 0, 0, 0] + dynamic: true + #sigma_drift: [0.1, 0.2, 0.3] # missing + + O3_factor_dynamic: + mode: factor + state: [1, 0, 0, 0] + sigma: [0.1, 0.2, 0.3] + dynamic: true + sigma_drift: [0.1, 0.2] # wrong size + + # I + I_nothing: + mode: nothing + #dynamic: false # missing + + I_initial_guess: + #mode: initial_guess # missing + state: [1, 2, 3, 4] + dynamic: false + + I_fix: + #mode: fix # missing + state: [1, 2, 3, 4] + dynamic: false + + I_factor: + mode: factor + state: [1, 2, 3, 4] + sigma: [0.1, 0.2, 0.3] # wrong size + dynamic: false + + I_nothing_dynamic: + mode: nothing + dynamic: true + #sigma_drift: [0.1, 0.2, 0.3, 0.4] # missing + + I_initial_guess_dynamic: + mode: initial_guess + state: [1, 2, 3, 4] + dynamic: true + sigma_drift: [0.1, 0.2, 0.3] # wrong size + + I_fix_dynamic: + mode: fix + state: [1, 2, 3] # wrong size + dynamic: true + sigma_drift: [0.1, 0.2, 0.3, 0.4] + + I_factor_dynamic: + mode: factor + state: [1, 2, 3, 4] + sigma: [0.1, 0.2, 0.3] # wrong size + dynamic: true + sigma_drift: [0.1, 0.2, 0.3, 0.4] \ No newline at end of file