diff --git a/schema/processor/ProcessorTracker.schema b/schema/processor/ProcessorTracker.schema new file mode 100644 index 0000000000000000000000000000000000000000..73323ef255ce9d5c11a705b1dc1d8852dc4e8817 --- /dev/null +++ b/schema/processor/ProcessorTracker.schema @@ -0,0 +1,12 @@ +follow: ProcessorBase.schema +min_features_for_keyframe: + mandatory: true + type: unsigned int + yaml_type: scalar + doc: Minimum number of features to vote for keyframe. + +max_new_features: + mandatory: true + type: int + yaml_type: scalar + doc: Maximum number of new features to be processed when adding a keyframe (-1=unlimited. 0=none.) diff --git a/schema/processor/ProcessorTrackerFeature.schema b/schema/processor/ProcessorTrackerFeature.schema new file mode 100644 index 0000000000000000000000000000000000000000..4a3bf2eb5042fd3da9fe79df6f76a586ac7e6439 --- /dev/null +++ b/schema/processor/ProcessorTrackerFeature.schema @@ -0,0 +1 @@ +follow: ProcessorTracker.schema \ No newline at end of file diff --git a/schema/processor/ProcessorTrackerLandmark.schema b/schema/processor/ProcessorTrackerLandmark.schema new file mode 100644 index 0000000000000000000000000000000000000000..4a3bf2eb5042fd3da9fe79df6f76a586ac7e6439 --- /dev/null +++ b/schema/processor/ProcessorTrackerLandmark.schema @@ -0,0 +1 @@ +follow: ProcessorTracker.schema \ No newline at end of file diff --git a/schema/sensor/SensorDiffDrive.schema b/schema/sensor/SensorDiffDrive.schema index e42352f9749376252bd19a306013ad5e0f4c4b13..d685842ed43b0d67433853acb006e1227e696b33 100644 --- a/schema/sensor/SensorDiffDrive.schema +++ b/schema/sensor/SensorDiffDrive.schema @@ -21,8 +21,8 @@ states: type: string yaml_type: scalar mandatory: false - default: StateParam3 - options: [StateParam3] + default: StateParams3 + options: [StateParams3] doc: The type of the SensorDiffDrive intrinsic parameters is StateParam3. state: type: Vector3d diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp index 6f1ace470ddf2bf636b8474212baa871515341a0..ee4631c8d62b868edce40b2aedb9f1c5aa6d2bdd 100644 --- a/src/problem/problem.cpp +++ b/src/problem/problem.cpp @@ -293,14 +293,22 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, ProcessorBasePtr prc_ptr = FactoryProcessorYaml::create(_prc_type, _params_yaml_filename, _folders_schema); - //Dimension check - int prc_dim = prc_ptr->getDim(); - auto prb = this; - assert(((prc_dim == 0) or (prc_dim == prb->getDim())) && "Processor and Problem do not agree on dimension"); - - prc_ptr->configure(sen_ptr); - prc_ptr->link(sen_ptr); + if (not prc_ptr) + { + throw std::runtime_error("Processor could not be created."); + } + else + { + //Dimension check + if (prc_ptr->getDim() != 0 and prc_ptr->getDim() != this->getDim()) + { + throw std::runtime_error("Processor not compatible with the Problem dimension."); + } + // add processor + prc_ptr->configure(sen_ptr); + prc_ptr->link(sen_ptr); + } return prc_ptr; } diff --git a/src/state_block/state_block.cpp b/src/state_block/state_block.cpp index a1d831c460e628d1a4571e287bd3fd1c942117d5..c061ea96beff00f3ac0a7c102c06f7e57dd4e349 100644 --- a/src/state_block/state_block.cpp +++ b/src/state_block/state_block.cpp @@ -86,7 +86,7 @@ StateBlockPtr create_orientation(const Eigen::VectorXd& _state, bool _fixed) if (_state.size() == 4) return StateQuaternion::create(_state, _fixed); - throw std::length_error("Wrong vector size for orientation. Must be 4 for a quaternion in 3D, or 1 for an angle in 2D."); + throw std::runtime_error("Wrong vector size for orientation. Must be 4 for a quaternion in 3D, or 1 for an angle in 2D."); return nullptr; } diff --git a/src/state_block/state_block_derived.cpp b/src/state_block/state_block_derived.cpp index c5bc71bbc0252beb8c951a6bc37d5f5a66884ea0..e1eca5d666cc70fbd1ed3074dbc981bddbcdd872 100644 --- a/src/state_block/state_block_derived.cpp +++ b/src/state_block/state_block_derived.cpp @@ -29,28 +29,28 @@ StateBlockPtr StatePoint2d::create(const Eigen::VectorXd& _state, bool _fixed) { if (_state.size() == 2) return std::make_shared<StatePoint2d>(_state, _fixed); - throw std::length_error("Wrong vector size for Point2d."); + throw std::runtime_error("Wrong vector size for Point2d."); } StateBlockPtr StatePoint3d::create(const Eigen::VectorXd& _state, bool _fixed) { if (_state.size() == 3) return std::make_shared<StatePoint3d>(_state, _fixed); - throw std::length_error("Wrong vector size for Point3d."); + throw std::runtime_error("Wrong vector size for Point3d."); } StateBlockPtr StateVector2d::create(const Eigen::VectorXd& _state, bool _fixed) { if (_state.size() == 2) return std::make_shared<StateVector2d>(_state, _fixed); - throw std::length_error("Wrong vector size for Vector2d."); + throw std::runtime_error("Wrong vector size for Vector2d."); } StateBlockPtr StateVector3d::create(const Eigen::VectorXd& _state, bool _fixed) { if (_state.size() == 3) return std::make_shared<StateVector3d>(_state, _fixed); - throw std::length_error("Wrong vector size for Vector3d."); + throw std::runtime_error("Wrong vector size for Vector3d."); } StateBlockPtr create_point(const Eigen::VectorXd& _state, bool _fixed) @@ -60,7 +60,7 @@ StateBlockPtr create_point(const Eigen::VectorXd& _state, bool _fixed) else if (_state.size() == 3) return std::make_shared<StatePoint3d>(_state, _fixed); - throw std::length_error("Wrong vector size for Point."); + throw std::runtime_error("Wrong vector size for Point."); } StateBlockPtr create_vector(const Eigen::VectorXd& _state, bool _fixed) @@ -70,7 +70,7 @@ StateBlockPtr create_vector(const Eigen::VectorXd& _state, bool _fixed) else if (_state.size() == 3) return std::make_shared<StateVector3d>(_state, _fixed); - throw std::length_error("Wrong vector size for Vector."); + throw std::runtime_error("Wrong vector size for Vector."); } template <size_t size> @@ -78,7 +78,7 @@ StateBlockPtr StateParams<size>::create(const Eigen::VectorXd& _state, bool _fix { if (_state.size() == size) return std::make_shared<StateParams<size>>(_state, _fixed); - throw std::length_error("Wrong vector size for Params."); + throw std::runtime_error("Wrong vector size for Params."); } namespace diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 34537dbc560271c4ffae5e374275d41d8f2897f7..ad07f30443ed9cb9049bdd656d288a9ca1ff6eb7 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -92,8 +92,8 @@ wolf_add_gtest(gtest_prior gtest_prior.cpp) # wolf_add_gtest(gtest_parser_yaml gtest_parser_yaml.cpp) # Problem class test -# wolf_add_gtest(gtest_problem gtest_problem.cpp) -# target_link_libraries(gtest_problem PUBLIC dummy) +wolf_add_gtest(gtest_problem gtest_problem.cpp) +target_link_libraries(gtest_problem PUBLIC dummy) # ProcessorBase class test wolf_add_gtest(gtest_processor_base gtest_processor_base.cpp) diff --git a/test/dummy/ProcessorTrackerFeatureDummy.schema b/test/dummy/ProcessorTrackerFeatureDummy.schema new file mode 100644 index 0000000000000000000000000000000000000000..f5dcdeed9b298aa72b768b4d27e218152d64bb39 --- /dev/null +++ b/test/dummy/ProcessorTrackerFeatureDummy.schema @@ -0,0 +1,6 @@ +follow: ProcessorTrackerFeature.schema +n_tracks_lost: + mandatory: true + type: unsigned int + yaml_type: scalar + doc: dummy parameter. \ No newline at end of file diff --git a/test/dummy/SensorDummyPoia3d.schema b/test/dummy/SensorDummyPoia3d.schema index 3c54a371cade72b795c8cb9156e08c2a109de6ee..180e9e84230c364548c5d0c8aa5206db961d1135 100644 --- a/test/dummy/SensorDummyPoia3d.schema +++ b/test/dummy/SensorDummyPoia3d.schema @@ -10,8 +10,8 @@ states: type: string yaml_type: scalar mandatory: false - default: StateParam5 - options: [StateParam5] + default: StateParams5 + options: [StateParams5] doc: The derived type of the state in 'I' state: type: Vector5d diff --git a/test/dummy/TreeManagerDummy.schema b/test/dummy/TreeManagerDummy.schema new file mode 100644 index 0000000000000000000000000000000000000000..32f690f192bb96f341919b8e495cfe4a71d91557 --- /dev/null +++ b/test/dummy/TreeManagerDummy.schema @@ -0,0 +1,6 @@ +follow: TreeManagerBase.schema +toy_param: + mandatory: true + type: double + yaml_type: scalar + doc: a toy parameter. \ No newline at end of file diff --git a/test/gtest_factory_state_block.cpp b/test/gtest_factory_state_block.cpp index 7468d09e36469d1ad8ee49d61bc97bc8e8774201..ea1f4be3c9258238ba927e95196e9d0e2781cfb9 100644 --- a/test/gtest_factory_state_block.cpp +++ b/test/gtest_factory_state_block.cpp @@ -122,7 +122,7 @@ TEST(FactoryStateBlock, creator_Point) ASSERT_FALSE(sba->hasLocalParametrization()); // fails - ASSERT_THROW(sba = FactoryStateBlock::create("StatePoint2d", Vector1d(1), false) , std::length_error); + ASSERT_THROW(sba = FactoryStateBlock::create("StatePoint2d", Vector1d(1), false) , std::runtime_error); } TEST(FactoryStateBlock, creator_P) @@ -140,7 +140,7 @@ TEST(FactoryStateBlock, creator_P) ASSERT_FALSE(sba->hasLocalParametrization()); // fails - ASSERT_THROW(sba = FactoryStateBlock::create("P", Vector1d(1), false) , std::length_error); + ASSERT_THROW(sba = FactoryStateBlock::create("P", Vector1d(1), false) , std::runtime_error); } TEST(FactoryStateBlock, creator_Vector) @@ -158,7 +158,7 @@ TEST(FactoryStateBlock, creator_Vector) ASSERT_FALSE(sba->hasLocalParametrization()); // fails - ASSERT_THROW(sba = FactoryStateBlock::create("StatePoint2d", Vector1d(1), false) , std::length_error); + ASSERT_THROW(sba = FactoryStateBlock::create("StatePoint2d", Vector1d(1), false) , std::runtime_error); } TEST(FactoryStateBlock, creator_V) @@ -176,7 +176,7 @@ TEST(FactoryStateBlock, creator_V) ASSERT_FALSE(sba->hasLocalParametrization()); // fails - ASSERT_THROW(sba = FactoryStateBlock::create("V", Vector1d(1), false) , std::length_error); + ASSERT_THROW(sba = FactoryStateBlock::create("V", Vector1d(1), false) , std::runtime_error); } TEST(FactoryStateBlock, creator_Params) @@ -207,7 +207,7 @@ TEST(FactoryStateBlock, creator_Params) ASSERT_FALSE(sb1->hasLocalParametrization()); // fails - ASSERT_THROW(auto sba = FactoryStateBlock::create("StateParams2", Vector1d(1), false) , std::length_error); + ASSERT_THROW(auto sba = FactoryStateBlock::create("StateParams2", Vector1d(1), false) , std::runtime_error); } int main(int argc, char **argv) diff --git a/test/gtest_prior.cpp b/test/gtest_prior.cpp index b82a64b48beeeb4c29a85dfd4bc80917c59f19c0..c8abf4fa57bf34dbbbeee8546d025bff062b5650 100644 --- a/test/gtest_prior.cpp +++ b/test/gtest_prior.cpp @@ -203,45 +203,45 @@ TEST(Prior, StateBlock) std::vector<PriorAsStruct> setups_ok, setups_death; // Unknown type -> StateBlock - setups_ok .push_back(PriorAsStruct({"K","initial_guess",vector1,vector0,false})); + setups_death.push_back(PriorAsStruct({"K","initial_guess",vector1,vector0,false})); // unknown K // Initial guess - not dynamic - 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})); + setups_ok .push_back(PriorAsStruct({"StateParams2","initial_guess",vector2,vector0,false})); + setups_ok .push_back(PriorAsStruct({"StateParams3","initial_guess",vector3,vector0,false})); + setups_ok .push_back(PriorAsStruct({"StateParams4","initial_guess",vector4,vector0,false})); // Initial guess - dynamic - setups_ok .push_back(PriorAsStruct({"StateBlock","initial_guess",vector2,vector0,true})); - 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 + setups_ok .push_back(PriorAsStruct({"StateParams2","initial_guess",vector2,vector0,true})); + setups_ok .push_back(PriorAsStruct({"StateParams2","initial_guess",vector2,vector0,true,vector2})); + setups_ok .push_back(PriorAsStruct({"StateParams3","initial_guess",vector3,vector0,true,vector3})); + setups_ok .push_back(PriorAsStruct({"StateParams4","initial_guess",vector4,vector0,true,vector4})); + setups_death.push_back(PriorAsStruct({"StateParams3","initial_guess",vector3,vector0,true,vector4})); // inconsistent size // Fix - not dynamic - 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 + setups_ok .push_back(PriorAsStruct({"StateParams2","fix",vector2,vector0,false})); + setups_ok .push_back(PriorAsStruct({"StateParams3","fix",vector3,vector0,false})); + setups_death.push_back(PriorAsStruct({"StateParams3","fix",vector4,vector0,false})); // wrong size // Fix - dynamic - setups_ok .push_back(PriorAsStruct({"StateBlock","fix",vector2,vector0,true,vector0})); - 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 + setups_ok .push_back(PriorAsStruct({"StateParams2","fix",vector2,vector0,true,vector0})); + setups_ok .push_back(PriorAsStruct({"StateParams2","fix",vector2,vector0,true,vector2})); + setups_ok .push_back(PriorAsStruct({"StateParams3","fix",vector3,vector0,true,vector3})); + setups_ok .push_back(PriorAsStruct({"StateParams4","fix",vector4,vector0,true,vector4})); + setups_death.push_back(PriorAsStruct({"StateParams3","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 + setups_ok .push_back(PriorAsStruct({"StateParams2","factor",vector2,vector2,false})); + setups_ok .push_back(PriorAsStruct({"StateParams3","factor",vector3,vector3,false})); + setups_ok .push_back(PriorAsStruct({"StateParams4","factor",vector4,vector4,false})); + setups_death.push_back(PriorAsStruct({"StateParams2","factor",vector2,vector3,false})); // inconsistent size // Factor - dynamic - setups_ok .push_back(PriorAsStruct({"StateBlock","factor",vector2,vector2,true})); - 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 + setups_ok .push_back(PriorAsStruct({"StateParams2","factor",vector2,vector2,true})); + setups_ok .push_back(PriorAsStruct({"StateParams2","factor",vector2,vector2,true,vector2})); + setups_ok .push_back(PriorAsStruct({"StateParams3","factor",vector3,vector3,true,vector3})); + setups_ok .push_back(PriorAsStruct({"StateParams4","factor",vector4,vector4,true,vector4})); + setups_death.push_back(PriorAsStruct({"StateParams3","factor",vector3,vector3,true,vector4})); // inconsistent size + setups_death.push_back(PriorAsStruct({"StateParams3","factor",vector3,vector4,true,vector3})); // inconsistent size // TEST SETUPS testPriors(setups_ok, true); diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp index fbc835ae1624f9dda8c139c9f8bedf131cb54787..b73e36e1262465f06ea1c9d3e965e75829e1ce55 100644 --- a/test/gtest_problem.cpp +++ b/test/gtest_problem.cpp @@ -76,12 +76,12 @@ TEST(Problem, Sensors) ProblemPtr P = Problem::create("POV", 3); // add a dummy sensor - auto S = SensorBase::emplace<SensorDummy>(P->getHardware(), - "dummy_name", - 3, - std::make_shared<ParamsSensorDummy>(), - Priors({{'P',Prior("P",Vector3d::Zero())}, - {'O',Prior("O",Vector4d::Random().normalized())}})); + auto params = std::make_shared<ParamsSensorDummy>(); + params->name = "dummy_name"; + auto S = SensorBase::emplace<SensorDummy3d>(P->getHardware(), + params, + Priors({{'P',Prior("P",Vector3d::Zero())}, + {'O',Prior("O",Vector4d::Random().normalized())}})); // check pointers ASSERT_EQ(P, S->getProblem()); ASSERT_EQ(P->getHardware(), S->getHardware()); @@ -96,12 +96,12 @@ TEST(Problem, Processor) ASSERT_TRUE(P->getMotionProviderMap().empty()); // add a motion sensor and processor - auto Sm = SensorBase::emplace<SensorOdom>(P->getHardware(), - "odometer", - 3, - std::make_shared<ParamsSensorOdom>(), - Priors({{'P',Prior("P",Vector3d::Zero())}, - {'O',Prior("O",Vector4d::Random().normalized())}})); + auto params = std::make_shared<ParamsSensorOdom>(); + params->name = "dummy_name"; + auto Sm = SensorBase::emplace<SensorOdom3d>(P->getHardware(), + params, + Priors({{'P',Prior("P",Vector3d::Zero())}, + {'O',Prior("O",Vector4d::Random().normalized())}})); // add motion processor auto Pm = ProcessorBase::emplace<ProcessorOdom3d>(Sm, std::make_shared<ParamsProcessorOdom3d>()); @@ -116,16 +116,16 @@ TEST(Problem, Installers) ProblemPtr P = Problem::create("PO", 3); Eigen::Vector7d xs; xs.setRandom(); xs.tail(4).normalize(); - SensorBasePtr S = P->installSensor ("SensorOdom", "odometer", wolf_root + "/test/yaml/sensor_odom_3d.yaml"); + SensorBasePtr S = P->installSensor("SensorOdom3d", wolf_root + "/test/yaml/sensor_odom_3d.yaml", {wolf_root}); // install processor tracker (dummy installation under an Odometry sensor -- it's OK for this test) - auto pt = P->installProcessor("ProcessorTrackerFeatureDummy", "dummy", "odometer"); + auto pt = P->installProcessor("ProcessorTrackerFeatureDummy", S->getName(), wolf_root + "/test/yaml/processor_tracker_feature_dummy.yaml", {wolf_root}); // check motion processor IS NOT set ASSERT_TRUE(P->getMotionProviderMap().empty()); // install processor motion - ProcessorBasePtr pm = P->installProcessor("ProcessorOdom3d", "odom integrator", "odometer", wolf_root + "/test/yaml/processor_odom_3d.yaml"); + ProcessorBasePtr pm = P->installProcessor("ProcessorOdom3d", S->getName(), wolf_root + "/test/yaml/processor_odom_3d.yaml", {wolf_root}); // check motion processor is set ASSERT_FALSE(P->getMotionProviderMap().empty()); @@ -292,11 +292,11 @@ TEST(Problem, StateBlocks) Eigen::Vector3d xs2d; // 2 state blocks, fixed - SensorBasePtr Sm = P->installSensor ("SensorOdom", "odometer", wolf_root + "/test/yaml/sensor_odom_3d.yaml"); + SensorBasePtr Sm = P->installSensor("SensorOdom3d", wolf_root + "/test/yaml/sensor_odom_3d.yaml", {wolf_root}); ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 2); - auto pt = P->installProcessor("ProcessorTrackerFeatureDummy", "dummy", "odometer"); - auto pm = P->installProcessor("ProcessorOdom3d", "odom integrator", "odometer", wolf_root + "/test/yaml/processor_odom_3d.yaml"); + auto pt = P->installProcessor("ProcessorTrackerFeatureDummy", Sm->getName(), wolf_root + "/test/yaml/processor_tracker_feature_dummy.yaml", {wolf_root}); + auto pm = P->installProcessor("ProcessorOdom3d", Sm->getName(), wolf_root + "/test/yaml/processor_odom_3d.yaml", {wolf_root}); // 2 state blocks, estimated auto KF = P->emplaceFrame(0, "PO", 3, xs3d ); @@ -345,16 +345,17 @@ TEST(Problem, Covariances) Eigen::Vector3d xs2d; - SensorBasePtr Sm = P->installSensor ("SensorOdom", "odometer", wolf_root + "/test/yaml/sensor_odom_3d.yaml"); - SensorBasePtr St = P->installSensor ("SensorOdom", "other odometer", wolf_root + "/test/yaml/sensor_odom_3d.yaml"); + SensorBasePtr Sm = P->installSensor("SensorOdom3d", wolf_root + "/test/yaml/sensor_odom_3d.yaml", {wolf_root}); + SensorBasePtr St = P->installSensor("SensorOdom3d", wolf_root + "/test/yaml/sensor_odom_3d_other.yaml", {wolf_root}); ParamsProcessorTrackerFeaturePtr params = std::make_shared<ParamsProcessorTrackerFeature>(); params->time_tolerance = 0.1; params->max_new_features = 5; params->min_features_for_keyframe = 10; - auto pt = P->installProcessor("ProcessorTrackerFeatureDummy", "dummy", Sm, params); - auto pm = P->installProcessor("ProcessorOdom3d", "odom integrator", "other odometer", wolf_root + "/test/yaml/processor_odom_3d.yaml"); + + auto pt = P->installProcessor("ProcessorTrackerFeatureDummy", Sm->getName(), wolf_root + "/test/yaml/processor_tracker_feature_dummy.yaml", {wolf_root}); + auto pm = P->installProcessor("ProcessorOdom3d", St->getName(), wolf_root + "/test/yaml/processor_odom_3d.yaml", {wolf_root}); // 4 state blocks, estimated St->unfixExtrinsics(); @@ -381,13 +382,13 @@ TEST(Problem, perturb) // make a sensor first auto param = std::make_shared<ParamsSensorDiffDrive>(); - param->ticks_per_wheel_revolution = 100; - auto sensor = std::static_pointer_cast<SensorDiffDrive>(problem->installSensor("SensorDiffDrive", - "sensor diff drive", - param, - Priors({{'P',Prior("P",Vector2d::Zero())}, - {'O',Prior("O",Vector1d::Zero())}, - {'I',Prior("StateBlock",Vector3d::Ones(),"initial_guess", VectorXd(0),true)}}))); + param->name = "sensor diff drive 1"; + param->ticks_per_wheel_revolution = 100; + auto sensor = SensorBase::emplace<SensorDiffDrive>(problem->getHardware(), + param, + Priors({{'P',Prior("P",Vector2d::Zero())}, + {'O',Prior("O",Vector1d::Zero())}, + {'I',Prior("StateParams3",Vector3d::Ones(),"initial_guess", VectorXd(0),true)}})); Vector3d pose; pose << 0,0,0; @@ -467,14 +468,13 @@ TEST(Problem, check) // make a sensor first auto param = std::make_shared<ParamsSensorDiffDrive>(); - param->ticks_per_wheel_revolution = 100; - auto sensor = std::static_pointer_cast<SensorDiffDrive>(problem->installSensor("SensorDiffDrive", - "sensor diff drive", - param, - Priors({{'P',Prior("P",Vector2d::Zero())}, - {'O',Prior("O",Vector1d::Zero())}, - {'I',Prior("StateBlock",Vector3d::Ones(),"initial_guess", VectorXd(0),true)}}))); - + param->name = "sensor diff drive 1"; + param->ticks_per_wheel_revolution = 100; + auto sensor = SensorBase::emplace<SensorDiffDrive>(problem->getHardware(), + param, + Priors({{'P',Prior("P",Vector2d::Zero())}, + {'O',Prior("O",Vector1d::Zero())}, + {'I',Prior("StateParams3",Vector3d::Ones(),"initial_guess", VectorXd(0),true)}})); Vector3d pose; pose << 0,0,0; int i = 0; @@ -526,36 +526,21 @@ TEST(Problem, check) TEST(Problem, autoSetupMap) { - std::string wolf_root = _WOLF_ROOT_DIR; - - auto parser = ParserYaml(wolf_root + "/test/yaml/params_problem_autosetup.yaml"); - auto server = ParamsServer(parser.getParams()); - - auto P = Problem::autoSetup(server); + auto P = Problem::autoSetup(wolf_root + "/test/yaml/params_problem_autosetup.yaml", {wolf_root}); ASSERT_TRUE(P->check(true, std::cout)); } TEST(Problem, autoSetupNoMap) { - std::string wolf_root = _WOLF_ROOT_DIR; - - auto parser = ParserYaml(wolf_root + "/test/yaml/params_problem_autosetup_no_map.yaml"); - auto server = ParamsServer(parser.getParams()); - - auto P = Problem::autoSetup(server); + auto P = Problem::autoSetup(wolf_root + "/test/yaml/params_problem_autosetup_no_map.yaml", {wolf_root}); ASSERT_TRUE(P->check(true, std::cout)); } TEST(Problem, getState) { - std::string wolf_root = _WOLF_ROOT_DIR; - - auto parser = ParserYaml(wolf_root + "/test/yaml/params_problem_autosetup.yaml"); - auto server = ParamsServer(parser.getParams()); - - auto P = Problem::autoSetup(server); + auto P = Problem::autoSetup(wolf_root + "/test/yaml/params_problem_autosetup.yaml", {wolf_root}); auto S = P->getHardware()->getSensorList().front(); @@ -620,12 +605,7 @@ TEST(Problem, getState) TEST(Problem, transform) { - std::string wolf_root = _WOLF_ROOT_DIR; - - auto parser = ParserYaml("test/yaml/params_problem_autosetup.yaml", wolf_root); - auto server = ParamsServer(parser.getParams()); - - auto P = Problem::autoSetup(server); + auto P = Problem::autoSetup(wolf_root + "/test/yaml/params_problem_autosetup.yaml", {wolf_root}); auto S = P->getHardware()->getSensorList().front(); diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp index 613857d108325657276fdaadc138650b543f5540..81c3ac989141fd8d65bb351baafeed15c9373be8 100644 --- a/test/gtest_processor_base.cpp +++ b/test/gtest_processor_base.cpp @@ -63,7 +63,7 @@ TEST(ProcessorBase, MotionProvider) ProblemPtr problem = Problem::create("PO", 2); // Install tracker (sensor and processor) - auto sens_trk = problem->installSensor("SensorDummy", + auto sens_trk = problem->installSensor("SensorDummy2d", wolf_root + "/test/yaml/sensor_tests/sensor_PO_2D_fix.yaml", {wolf_root}); auto proc_trk_params = make_shared<ParamsProcessorTrackerFeatureDummy>(); @@ -71,7 +71,7 @@ TEST(ProcessorBase, MotionProvider) auto proc_trk = ProcessorBase::emplace<ProcessorTrackerFeatureDummy>(sens_trk, proc_trk_params); // Install odometer (sensor and processor) - auto sens_odo = problem->installSensor("SensorOdom", + auto sens_odo = problem->installSensor("SensorOdom2d", wolf_root + "/test/yaml/sensor_odom_2d.yaml", {wolf_root}); auto proc_odo_params = make_shared<ParamsProcessorOdom2d>(); @@ -101,7 +101,7 @@ TEST(ProcessorBase, KeyFrameCallback) ProblemPtr problem = Problem::create("PO", 2); // Install tracker (sensor and processor) - auto sens_trk = problem->installSensor("SensorDummy", + auto sens_trk = problem->installSensor("SensorDummy2d", wolf_root + "/test/yaml/sensor_tests/sensor_PO_2D_fix.yaml", {wolf_root}); auto proc_trk_params = make_shared<ParamsProcessorTrackerFeatureDummy>(); @@ -109,7 +109,7 @@ TEST(ProcessorBase, KeyFrameCallback) auto proc_trk = ProcessorBase::emplace<ProcessorTrackerFeatureDummy>(sens_trk, proc_trk_params); // Install odometer (sensor and processor) - auto sens_odo = problem->installSensor("SensorOdom", + auto sens_odo = problem->installSensor("SensorOdom2d", wolf_root + "/test/yaml/sensor_odom_2d.yaml", {wolf_root}); auto proc_odo_params = make_shared<ParamsProcessorOdom2d>(); diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp index c4b37a73ff7bbfc2b613a2b137990581afce9627..8b157d5ee90eee6ec002855695e960b5edec9bf7 100644 --- a/test/gtest_processor_motion.cpp +++ b/test/gtest_processor_motion.cpp @@ -69,7 +69,7 @@ class ProcessorMotion_test : public testing::Test{ dt = 1.0; problem = Problem::create("PO", 2); - sensor = static_pointer_cast<SensorOdom2d>(problem->installSensor("SensorOdom", + sensor = static_pointer_cast<SensorOdom2d>(problem->installSensor("SensorOdom2d", wolf_root + "/test/yaml/sensor_odom_2d.yaml", {wolf_root})); ParamsProcessorOdom2dPtr params(std::make_shared<ParamsProcessorOdom2d>()); diff --git a/test/gtest_sensor_base.cpp b/test/gtest_sensor_base.cpp index 5280195fd62b8fcaf5050f34a5915321c34772a5..1ec8217d3e920ecf944eb457f615faca7dbf351b 100644 --- a/test/gtest_sensor_base.cpp +++ b/test/gtest_sensor_base.cpp @@ -335,7 +335,7 @@ TEST(SensorBase, makeshared_priors_POIA_mixed) auto S = std::make_shared<SensorDummy3d>(params, 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")}, + {'I',Prior("StateParams5", i_state_3D, "initial_guess")}, {'A',Prior("StateQuaternion", o_state_3D, "factor", o_std_3D)}})); // noise diff --git a/test/gtest_tree_manager.cpp b/test/gtest_tree_manager.cpp index 463b8298727332612945a168a69e0646db47a15a..ea5ab494606c188a5130a15b01eeb610b01cd9e4 100644 --- a/test/gtest_tree_manager.cpp +++ b/test/gtest_tree_manager.cpp @@ -48,8 +48,10 @@ TEST(TreeManager, createNode) { ProblemPtr P = Problem::create("PO", 2); - auto yaml_server = yaml_schema_cpp::YamlServer({wolf_root + "/schemas"}, - wolf_root + "/test/yaml/params_tree_manager1.yaml"); + auto yaml_server = yaml_schema_cpp::YamlServer({wolf_root}, + wolf_root + "/test/yaml/tree_manager_dummy.yaml"); + + WOLF_INFO_COND(not yaml_server.applySchema("TreeManagerDummy"), yaml_server.getLog().str()); ASSERT_TRUE(yaml_server.applySchema("TreeManagerDummy")); auto GM = TreeManagerDummy::create(yaml_server.getNode()); @@ -66,7 +68,7 @@ TEST(TreeManager, createYaml) { ProblemPtr P = Problem::create("PO", 2); - auto GM = TreeManagerDummy::create(wolf_root + "/test/yaml/tree_manager_dummy.yaml", {wolf_root + "/schemas"}); + auto GM = TreeManagerDummy::create(wolf_root + "/test/yaml/tree_manager_dummy.yaml", {wolf_root}); ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerDummy>(GM) != nullptr); @@ -80,8 +82,8 @@ TEST(TreeManager, FactoryParam) { ProblemPtr P = Problem::create("PO", 2); - auto yaml_server = yaml_schema_cpp::YamlServer({wolf_root + "/schemas"}, - wolf_root + "/test/yaml/params_tree_manager1.yaml"); + auto yaml_server = yaml_schema_cpp::YamlServer({wolf_root}, + wolf_root + "/test/yaml/tree_manager_dummy.yaml"); ASSERT_TRUE(yaml_server.applySchema("TreeManagerDummy")); auto GM = FactoryTreeManager::create("TreeManagerDummy", yaml_server.getNode()); @@ -100,7 +102,7 @@ TEST(TreeManager, FactoryYaml) auto GM = FactoryTreeManagerYaml::create("TreeManagerDummy", wolf_root + "/test/yaml/tree_manager_dummy.yaml", - {wolf_root + "/schemas"}); + {wolf_root}); ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerDummy>(GM) != nullptr); @@ -112,7 +114,7 @@ TEST(TreeManager, FactoryYaml) TEST(TreeManager, autoConf) { - ProblemPtr P = Problem::autoSetup(wolf_root + "/test/yaml/params_tree_manager1.yaml", {wolf_root + "/schemas"}); + ProblemPtr P = Problem::autoSetup(wolf_root + "/test/yaml/params_tree_manager1.yaml", {wolf_root}); P->applyPriorOptions(0); ASSERT_TRUE(std::dynamic_pointer_cast<TreeManagerDummy>(P->getTreeManager()) != nullptr); @@ -121,7 +123,7 @@ TEST(TreeManager, autoConf) TEST(TreeManager, autoConfNone) { - ProblemPtr P = Problem::autoSetup(wolf_root + "/test/yaml/params_tree_manager2.yaml", {wolf_root + "/schemas"}); + ProblemPtr P = Problem::autoSetup(wolf_root + "/test/yaml/params_tree_manager2.yaml", {wolf_root}); P->applyPriorOptions(0); ASSERT_TRUE(P->getTreeManager() == nullptr); // params_tree_manager2.yaml problem/tree_manager/type: None diff --git a/test/yaml/params3.yaml b/test/yaml/params3.yaml index 4d0487bde4dfe76d6ea3fb9fe1bbf629ec525bee..14c6271cd071c4d6d2f525e3c1fc8d0341d052d0 100644 --- a/test/yaml/params3.yaml +++ b/test/yaml/params3.yaml @@ -18,4 +18,5 @@ config: name: "my_proc_test" plugin: "core" sensor_name: "odom" - extern_params: "@params3.1.yaml" \ No newline at end of file + extern_params: + follow: "params3.1.yaml" \ No newline at end of file diff --git a/test/yaml/params_prior.yaml b/test/yaml/params_prior.yaml index dbdc6c608c75f3c8a62d96034a5fb825aba3f3e5..e6cea5317bc1f90ba67a948c465b31c31f1a78e1 100644 --- a/test/yaml/params_prior.yaml +++ b/test/yaml/params_prior.yaml @@ -244,40 +244,40 @@ O_3D_factor_dynamic: # I I_initial_guess: - type: StateParam4 + type: StateParams4 mode: initial_guess state: [1, 2, 3, 4] dynamic: false I_fix: - type: StateParam4 + type: StateParams4 mode: fix state: [1, 2, 3, 4] dynamic: false I_factor: - type: StateParam4 + type: StateParams4 mode: factor state: [1, 2, 3, 4] noise_std: [0.1, 0.2, 0.3, 0.4] dynamic: false I_initial_guess_dynamic_drift: - type: StateParam4 + type: StateParams4 mode: initial_guess state: [1, 2, 3, 4] dynamic: true drift_std: [0.1, 0.2, 0.3, 0.4] I_fix_dynamic_drift: - type: StateParam4 + type: StateParams4 mode: fix state: [1, 2, 3, 4] dynamic: true drift_std: [0.1, 0.2, 0.3, 0.4] I_factor_dynamic_drift: - type: StateParam4 + type: StateParams4 mode: factor state: [1, 2, 3, 4] noise_std: [0.1, 0.2, 0.3, 0.4] @@ -285,19 +285,19 @@ I_factor_dynamic_drift: drift_std: [0.1, 0.2, 0.3, 0.4] I_initial_guess_dynamic: - type: StateParam4 + type: StateParams4 mode: initial_guess state: [1, 2, 3, 4] dynamic: true I_fix_dynamic: - type: StateParam4 + type: StateParams4 mode: fix state: [1, 2, 3, 4] dynamic: true I_factor_dynamic: - type: StateParam4 + type: StateParams4 mode: factor state: [1, 2, 3, 4] noise_std: [0.1, 0.2, 0.3, 0.4] diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml index 6da05444971111e75df87f3dae914a050f80c841..12d338a330ee4eb3112cbd46a9347b0bb41af258 100644 --- a/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml +++ b/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml @@ -11,4 +11,5 @@ config: P: [0.31, 0.31, 0.31] O: [0.31, 0.31, 0.31] time_tolerance: 0.1 - tree_manager: "@tree_manager_sliding_window_dual_rate1.yaml" \ No newline at end of file + tree_manager: + follow: "tree_manager_sliding_window_dual_rate1.yaml" \ No newline at end of file diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml index 79f9a97fa861fec5ae9885c57d8319049ad88512..35cd9bc7da15a010b912df30d195c7be0f563ec2 100644 --- a/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml +++ b/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml @@ -1,5 +1,6 @@ config: - solver: "@solver.yaml" + solver: + follow: "solver.yaml" problem: frame_structure: "PO" dimension: 3 diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml index 46e4eeff543983841949c467dc6a7473e6cfd911..769d7376a5a5f988a5d817068d589710a49c0afc 100644 --- a/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml +++ b/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml @@ -1,5 +1,6 @@ config: - solver: "@solver.yaml" + solver: + follow: "solver.yaml" problem: frame_structure: "PO" dimension: 3 diff --git a/test/yaml/processor_tracker_feature_dummy.yaml b/test/yaml/processor_tracker_feature_dummy.yaml new file mode 100644 index 0000000000000000000000000000000000000000..ea6f0af325782f950dced66d09f7bb08d0de32bf --- /dev/null +++ b/test/yaml/processor_tracker_feature_dummy.yaml @@ -0,0 +1,4 @@ +name: cool dummy processor +min_features_for_keyframe: 1 +max_new_features: 0 +n_tracks_lost: 1 \ No newline at end of file diff --git a/test/yaml/sensor_odom_3d.yaml b/test/yaml/sensor_odom_3d.yaml index ea22ca723fbf7fedaf121ded4d041866bc0f65a1..d3965dfe44e370d3a35563a244774cfb3bd5d20b 100644 --- a/test/yaml/sensor_odom_3d.yaml +++ b/test/yaml/sensor_odom_3d.yaml @@ -1,3 +1,4 @@ +name: odometer 3d states: P: mode: fix diff --git a/test/yaml/sensor_odom_3d_other.yaml b/test/yaml/sensor_odom_3d_other.yaml new file mode 100644 index 0000000000000000000000000000000000000000..98b573c9e1f9111fbb39efd17f28daca8fe04d9a --- /dev/null +++ b/test/yaml/sensor_odom_3d_other.yaml @@ -0,0 +1,2 @@ +follow: sensor_odom_3d.yaml +name: another odometer 3d \ No newline at end of file