From 6701ddc49e5604896e31ba551ac36f8889b6bab4 Mon Sep 17 00:00:00 2001 From: jvallve <jvallve@iri.upc.edu> Date: Thu, 7 Jul 2022 13:52:12 +0200 Subject: [PATCH] [skip ci] still fixing gtests --- schema/processor/ProcessorTracker.schema | 12 +++ .../processor/ProcessorTrackerFeature.schema | 1 + .../processor/ProcessorTrackerLandmark.schema | 1 + schema/sensor/SensorDiffDrive.schema | 4 +- src/problem/problem.cpp | 22 ++-- src/state_block/state_block.cpp | 2 +- src/state_block/state_block_derived.cpp | 14 +-- test/CMakeLists.txt | 4 +- .../dummy/ProcessorTrackerFeatureDummy.schema | 6 ++ test/dummy/SensorDummyPoia3d.schema | 4 +- test/dummy/TreeManagerDummy.schema | 6 ++ test/gtest_factory_state_block.cpp | 10 +- test/gtest_prior.cpp | 54 +++++----- test/gtest_problem.cpp | 102 +++++++----------- test/gtest_processor_base.cpp | 8 +- test/gtest_processor_motion.cpp | 2 +- test/gtest_sensor_base.cpp | 2 +- test/gtest_tree_manager.cpp | 18 ++-- test/yaml/params3.yaml | 3 +- test/yaml/params_prior.yaml | 18 ++-- ...ree_manager_sliding_window_dual_rate1.yaml | 3 +- ...ree_manager_sliding_window_dual_rate3.yaml | 3 +- ...ger_sliding_window_dual_rate_baseline.yaml | 3 +- .../yaml/processor_tracker_feature_dummy.yaml | 4 + test/yaml/sensor_odom_3d.yaml | 1 + test/yaml/sensor_odom_3d_other.yaml | 2 + 26 files changed, 168 insertions(+), 141 deletions(-) create mode 100644 schema/processor/ProcessorTracker.schema create mode 100644 schema/processor/ProcessorTrackerFeature.schema create mode 100644 schema/processor/ProcessorTrackerLandmark.schema create mode 100644 test/dummy/ProcessorTrackerFeatureDummy.schema create mode 100644 test/dummy/TreeManagerDummy.schema create mode 100644 test/yaml/processor_tracker_feature_dummy.yaml create mode 100644 test/yaml/sensor_odom_3d_other.yaml diff --git a/schema/processor/ProcessorTracker.schema b/schema/processor/ProcessorTracker.schema new file mode 100644 index 000000000..73323ef25 --- /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 000000000..4a3bf2eb5 --- /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 000000000..4a3bf2eb5 --- /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 e42352f97..d685842ed 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 6f1ace470..ee4631c8d 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 a1d831c46..c061ea96b 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 c5bc71bbc..e1eca5d66 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 34537dbc5..ad07f3044 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 000000000..f5dcdeed9 --- /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 3c54a371c..180e9e842 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 000000000..32f690f19 --- /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 7468d09e3..ea1f4be3c 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 b82a64b48..c8abf4fa5 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 fbc835ae1..b73e36e12 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 613857d10..81c3ac989 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 c4b37a73f..8b157d5ee 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 5280195fd..1ec8217d3 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 463b82987..ea5ab4946 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 4d0487bde..14c6271cd 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 dbdc6c608..e6cea5317 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 6da054449..12d338a33 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 79f9a97fa..35cd9bc7d 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 46e4eeff5..769d7376a 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 000000000..ea6f0af32 --- /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 ea22ca723..d3965dfe4 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 000000000..98b573c9e --- /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 -- GitLab