From 5bad6f27b74e5453358c3a9256b92702c943afff Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu> Date: Fri, 22 Jul 2022 16:46:04 +0200 Subject: [PATCH] [skip ci] wip --- include/core/sensor/sensor_odom.h | 2 +- include/core/sensor/sensor_pose.h | 2 +- include/core/state_block/composite.h | 41 ++++++++++++--- schema/sensor/SpecStateSensorO2d.schema | 12 ++++- schema/sensor/SpecStateSensorO3d.schema | 12 ++++- schema/sensor/SpecStateSensorP2d.schema | 12 ++++- schema/sensor/SpecStateSensorP3d.schema | 12 ++++- src/problem/problem.cpp | 2 +- src/sensor/sensor_diff_drive.cpp | 2 +- src/sensor/sensor_motion_model.cpp | 2 +- test/CMakeLists.txt | 4 +- test/dummy/sensor_dummy.h | 35 ++++++++++++- test/gtest_processor_diff_drive.cpp | 67 +++++++++---------------- test/gtest_sensor_base.cpp | 11 ++-- 14 files changed, 149 insertions(+), 67 deletions(-) diff --git a/include/core/sensor/sensor_odom.h b/include/core/sensor/sensor_odom.h index 8efa6ae80..9817b4235 100644 --- a/include/core/sensor/sensor_odom.h +++ b/include/core/sensor/sensor_odom.h @@ -72,7 +72,7 @@ class SensorOdom : public SensorBase SensorBase("SensorOdom"+toString(DIM)+"d", DIM, _params, - _priors), + _priors("PO")), params_odom_(_params) { static_assert(DIM == 2 or DIM == 3); diff --git a/include/core/sensor/sensor_pose.h b/include/core/sensor/sensor_pose.h index 6a3d8adef..4bafeb5ff 100644 --- a/include/core/sensor/sensor_pose.h +++ b/include/core/sensor/sensor_pose.h @@ -61,7 +61,7 @@ class SensorPose : public SensorBase SensorBase("SensorPose"+toString(DIM)+"d", DIM, _params, - _priors), + _priors("PO")), params_pose_(_params) { static_assert(DIM == 2 or DIM == 3); diff --git a/include/core/state_block/composite.h b/include/core/state_block/composite.h index 6fec7d76a..6697d6e0b 100644 --- a/include/core/state_block/composite.h +++ b/include/core/state_block/composite.h @@ -59,6 +59,8 @@ class Composite : public map<char, T> return _os; } + Composite operator()(const std::string& _keys) const; + YAML::Node toYaml() const; }; @@ -77,22 +79,30 @@ inline CompositeOther Composite<T>::cast() const template <typename T> inline Composite<T>::Composite(const YAML::Node& _n) { - WOLF_INFO("constructor composite..."); if (_n.IsDefined()) { - WOLF_INFO("defined...", _n); if (not _n.IsMap()) { - throw std::runtime_error("SpecComposite: constructor with a non-map yaml node"); + throw std::runtime_error("Composite: constructor with a non-map yaml node"); } for (auto spec_pair : _n) { - WOLF_INFO("iterating...", spec_pair.first, spec_pair.second); - this->emplace(spec_pair.first.as<char>(), T(spec_pair.second)); + try + { + this->emplace(spec_pair.first.as<char>(), T(spec_pair.second)); + } + catch(const std::exception& e) + { + WOLF_WARN("Composite: In constructor. When emplacing the entry for key '", spec_pair.first.as<std::string>(), + "' got the error: ", e.what(), " skipping this key..."); + } } } - WOLF_INFO("constructor composite done"); + else + { + WOLF_WARN("Composite: In constructor. The YAML node is not defined. Constructing an empty composite"); + } } template <typename T> @@ -124,6 +134,25 @@ inline StateKeys Composite<T>::getKeys() const return keys; } +template <typename T> +inline Composite<T> Composite<T>::operator()(const std::string& _keys) const +{ + if (not this->has(_keys)) + { + throw std::runtime_error("Composite<T>::operator() required keys " + + _keys + + " are not available, only have " + + getKeys()); + } + + Composite<T> output; + for (auto key : _keys) + { + output.emplace(key, this->at(key)); + } + return output; +} + template <typename T> inline YAML::Node Composite<T>::toYaml() const { diff --git a/schema/sensor/SpecStateSensorO2d.schema b/schema/sensor/SpecStateSensorO2d.schema index 519ca225b..a6378199a 100644 --- a/schema/sensor/SpecStateSensorO2d.schema +++ b/schema/sensor/SpecStateSensorO2d.schema @@ -10,4 +10,14 @@ state: type: Vector1d yaml_type: scalar mandatory: true - doc: A vector containing the state values \ No newline at end of file + doc: A vector containing the state values +noise_std: + type: Vector1d + yaml_type: scalar + mandatory: false + doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. +drift_std: + type: Vector1d + yaml_type: scalar + mandatory: false + doc: A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix. \ No newline at end of file diff --git a/schema/sensor/SpecStateSensorO3d.schema b/schema/sensor/SpecStateSensorO3d.schema index c248eb442..560d94b1d 100644 --- a/schema/sensor/SpecStateSensorO3d.schema +++ b/schema/sensor/SpecStateSensorO3d.schema @@ -10,4 +10,14 @@ state: type: Vector4d yaml_type: scalar mandatory: true - doc: A vector containing the state values. It should be a quaternion (i.e. four values and normalized) \ No newline at end of file + doc: A vector containing the state values. It should be a quaternion (i.e. four values and normalized) +noise_std: + type: Vector3d + yaml_type: scalar + mandatory: false + doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. +drift_std: + type: Vector3d + yaml_type: scalar + mandatory: false + doc: A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix. \ No newline at end of file diff --git a/schema/sensor/SpecStateSensorP2d.schema b/schema/sensor/SpecStateSensorP2d.schema index 4d3ea92c2..8315a52bd 100644 --- a/schema/sensor/SpecStateSensorP2d.schema +++ b/schema/sensor/SpecStateSensorP2d.schema @@ -10,4 +10,14 @@ state: type: Vector2d yaml_type: scalar mandatory: true - doc: A vector containing the state values \ No newline at end of file + doc: A vector containing the state values +noise_std: + type: Vector2d + yaml_type: scalar + mandatory: false + doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. +drift_std: + type: Vector2d + yaml_type: scalar + mandatory: false + doc: A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix. \ No newline at end of file diff --git a/schema/sensor/SpecStateSensorP3d.schema b/schema/sensor/SpecStateSensorP3d.schema index c7d681d7f..83b76542b 100644 --- a/schema/sensor/SpecStateSensorP3d.schema +++ b/schema/sensor/SpecStateSensorP3d.schema @@ -10,4 +10,14 @@ state: type: Vector3d yaml_type: scalar mandatory: true - doc: A vector containing the state 'P' values \ No newline at end of file + doc: A vector containing the state 'P' values +noise_std: + type: Vector3d + yaml_type: scalar + mandatory: false + doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix. +drift_std: + type: Vector3d + yaml_type: scalar + mandatory: false + doc: A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix. \ No newline at end of file diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp index 8712f0da9..c4fd5ef64 100644 --- a/src/problem/problem.cpp +++ b/src/problem/problem.cpp @@ -158,7 +158,7 @@ ProblemPtr Problem::autoSetup(const std::string& _input_yaml_file, const std::ve bool is2D = server.getNode()["problem"]["dimension"].as<int>() == 2; if (not server.applySchema(is2D ? "Problem2d.schema" : "Problem3d.schema")) { - WOLF_ERROR(server.getLog().str()); + WOLF_ERROR(server.getLog().str(), "\nNode:\n", server.getNode()); return nullptr; } WOLF_INFO("schema applied"); diff --git a/src/sensor/sensor_diff_drive.cpp b/src/sensor/sensor_diff_drive.cpp index 4a3b7401b..b17b318f9 100644 --- a/src/sensor/sensor_diff_drive.cpp +++ b/src/sensor/sensor_diff_drive.cpp @@ -38,7 +38,7 @@ SensorDiffDrive::SensorDiffDrive(ParamsSensorDiffDrivePtr _params, SensorBase("SensorDiffDrive", 2, _params, - _priors), + _priors("POI")), params_diff_drive_(_params) { } diff --git a/src/sensor/sensor_motion_model.cpp b/src/sensor/sensor_motion_model.cpp index 58a28fa17..c2a0ea42c 100644 --- a/src/sensor/sensor_motion_model.cpp +++ b/src/sensor/sensor_motion_model.cpp @@ -28,7 +28,7 @@ SensorMotionModel::SensorMotionModel(ParamsSensorBasePtr _params, SensorBase("SensorMotionModel", -1, _params, - _priors) + _priors("")) { } diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 782a06f05..a61ad8265 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -186,8 +186,8 @@ wolf_add_gtest(gtest_param_prior gtest_param_prior.cpp) # ProcessorFixedWingModel class test wolf_add_gtest(gtest_processor_fixed_wing_model gtest_processor_fixed_wing_model.cpp) -# # ProcessorDiffDrive class test -# wolf_add_gtest(gtest_processor_diff_drive gtest_processor_diff_drive.cpp) +# ProcessorDiffDrive class test +wolf_add_gtest(gtest_processor_diff_drive gtest_processor_diff_drive.cpp) # # ProcessorLoopClosure class test # wolf_add_gtest(gtest_processor_loop_closure gtest_processor_loop_closure.cpp) diff --git a/test/dummy/sensor_dummy.h b/test/dummy/sensor_dummy.h index bca6fed74..5cb186c87 100644 --- a/test/dummy/sensor_dummy.h +++ b/test/dummy/sensor_dummy.h @@ -63,7 +63,7 @@ class SensorDummy : public SensorBase SensorBase("SensorDummy"+toString(DIM)+"d", DIM, _params, - _priors), + _priors("PO")), params_dummy_(_params) { static_assert(DIM == 2 or DIM == 3); @@ -79,11 +79,41 @@ class SensorDummy : public SensorBase } }; +template <unsigned int DIM> +class SensorDummyPoia : public SensorBase +{ + private: + ParamsSensorDummyPtr params_dummy_; + + public: + SensorDummyPoia(ParamsSensorDummyPtr _params, + const SpecSensorComposite& _priors) : + SensorBase("SensorDummyPoia"+toString(DIM)+"d", + DIM, + _params, + _priors("POIA")), + params_dummy_(_params) + { + static_assert(DIM == 2 or DIM == 3); + } + WOLF_SENSOR_CREATE(SensorDummyPoia<DIM>, ParamsSensorDummy); + + virtual ~SensorDummyPoia() = default; + + Eigen::MatrixXd computeNoiseCov(const Eigen::VectorXd & _data) const override + { + return (Eigen::Vector2d() << params_dummy_->noise_p_std, + params_dummy_->noise_o_std).finished().cwiseAbs2().asDiagonal(); + } +}; + typedef SensorDummy<2> SensorDummy2d; typedef SensorDummy<3> SensorDummy3d; -typedef SensorDummy<3> SensorDummyPoia3d; +typedef SensorDummyPoia<2> SensorDummyPoia2d; +typedef SensorDummyPoia<3> SensorDummyPoia3d; WOLF_DECLARED_PTR_TYPEDEFS(SensorDummy2d); WOLF_DECLARED_PTR_TYPEDEFS(SensorDummy3d); +WOLF_DECLARED_PTR_TYPEDEFS(SensorDummyPoia2d); WOLF_DECLARED_PTR_TYPEDEFS(SensorDummyPoia3d); } // namespace wolf @@ -93,5 +123,6 @@ WOLF_DECLARED_PTR_TYPEDEFS(SensorDummyPoia3d); namespace wolf { WOLF_REGISTER_SENSOR(SensorDummy2d); WOLF_REGISTER_SENSOR(SensorDummy3d); +WOLF_REGISTER_SENSOR(SensorDummyPoia2d); WOLF_REGISTER_SENSOR(SensorDummyPoia3d); } \ No newline at end of file diff --git a/test/gtest_processor_diff_drive.cpp b/test/gtest_processor_diff_drive.cpp index c67f9294f..0712b09ba 100644 --- a/test/gtest_processor_diff_drive.cpp +++ b/test/gtest_processor_diff_drive.cpp @@ -142,16 +142,15 @@ class ProcessorDiffDriveTest : public testing::Test // make a sensor first params_sen = std::make_shared<ParamsSensorDiffDrive>(); + params_sen->name = "cool sensor"; params_sen->ticks_per_wheel_revolution = 100; // DO NOT MODIFY THESE VALUES !!! - SpecSensorComposite priors{{'P',SpecStateSensor("P", Vector2d::Zero())}, //default "fix", not dynamic - {'O',SpecStateSensor("O", Vector1d::Zero())}, //default "fix", not dynamic - {'I',SpecStateSensor("StateBlock", Vector3d::Ones(), "initial_guess")}}; // not dynamic DO NOT MODIFY THESE VALUES !!! + SpecSensorComposite priors{{'P',SpecStateSensor("StatePoint2d", Vector2d::Zero())}, // default "fix", not dynamic + {'O',SpecStateSensor("StateAngle", Vector1d::Zero())}, // default "fix", not dynamic + {'I',SpecStateSensor("StateBlock", Vector3d::Ones(), "initial_guess")}}; // not dynamic DO NOT MODIFY THESE VALUES !!! - sensor = std::static_pointer_cast<SensorDiffDrive>(problem->installSensor("SensorDiffDrive", - "sensor", - params_sen, - priors)); // DO NOT MODIFY THESE VALUES !!! + sensor = SensorBase::emplace<SensorDiffDrive>(problem->getHardware(), params_sen, priors); + // params and processor params = std::make_shared<ParamsProcessorDiffDrive>(); params->voting_active = true; @@ -160,7 +159,7 @@ class ProcessorDiffDriveTest : public testing::Test params->max_buff_length = 3; params->max_time_span = 2.5; params->unmeasured_perturbation_std = 1e-4; - processor = std::static_pointer_cast<ProcessorDiffDrivePublic>(problem->installProcessor("ProcessorDiffDrive", "processor diff drive", sensor, params)); + processor = ProcessorBase::emplace<ProcessorDiffDrivePublic>(sensor, params); } void TearDown() override{} @@ -180,21 +179,6 @@ TEST(ProcessorDiffDrive, constructor) ASSERT_EQ(prc->getType(), "ProcessorDiffDrive"); } -TEST(ProcessorDiffDrive, create) -{ - // params - auto params = std::make_shared<ParamsProcessorDiffDrive>(); - - // processor - auto prc_base = ProcessorDiffDrive::create("prc", params); - - auto prc = std::static_pointer_cast<ProcessorDiffDrive>(prc_base); - - ASSERT_NE(prc, nullptr); - - ASSERT_EQ(prc->getType(), "ProcessorDiffDrive"); -} - TEST_F(ProcessorDiffDriveTest, params) { // read @@ -288,7 +272,7 @@ TEST_F(ProcessorDiffDriveTest, statePlusDelta) Vector3d calib(1,1,1); double dt = 1.0; VectorXd delta(3); - VectorComposite x1("PO", {2,1}), x2("PO", {2,1}); + VectorComposite x1("PO", Vector3d::Zero(), {2,1}), x2("PO", Vector3d::Zero(), {2,1}); MatrixXd delta_cov(3,3), J_delta_calib(3,3); // 1. left turn 90 deg in 3 steps of 30 deg --> ends up in (1.5, 1.5, pi/2) @@ -325,15 +309,14 @@ TEST_F(ProcessorDiffDriveTest, statePlusDelta) TEST_F(ProcessorDiffDriveTest, process) { Vector2d data; - Matrix2d data_cov; data_cov . setIdentity(); + Matrix2d data_cov; data_cov.setIdentity(); TimeStamp t = 0.0; double dt = 1.0; - // Vector3d x(0,0,0); - VectorComposite x(Vector3d(0,0,0), "PO", {2,1}); - // Matrix3d P; P.setIdentity(); - VectorComposite P(Vector3d(1,1,1), "PO", {2,1}); - auto F0 = problem->setPriorFactor(x, P, t); + SpecComposite prior; + prior.emplace('P', SpecState("StatePoint2d", Vector2d::Zero(), "factor", Vector2d::Constant(1))); + prior.emplace('O', SpecState("StateAngle", Vector1d::Zero(), "factor", Vector1d::Constant(1))); + auto F0 = problem->setPrior(prior, t); processor->setOrigin(F0); // 1. left turn 90 deg in N steps of 90/N deg --> ends up in (1.5, 1.5, pi/2) @@ -359,12 +342,11 @@ TEST_F(ProcessorDiffDriveTest, linear) Vector2d data; Matrix2d data_cov; data_cov . setIdentity(); TimeStamp t = 0.0; - // Vector3d x(0,0,0); - VectorComposite x(Vector3d(0,0,0), "PO", {2,1}); - // Matrix3d P; P.setIdentity(); - VectorComposite P(Vector3d(1,1,1), "PO", {2,1}); - - auto F0 = problem->setPriorFactor(x, P, t); + + SpecComposite prior; + prior.emplace('P', SpecState("StatePoint2d", Vector2d::Zero(), "factor", Vector2d::Constant(1))); + prior.emplace('O', SpecState("StateAngle", Vector1d::Zero(), "factor", Vector1d::Constant(1))); + auto F0 = problem->setPrior(prior, t); processor->setOrigin(F0); // Straight one turn of the wheels, in one go @@ -388,13 +370,12 @@ TEST_F(ProcessorDiffDriveTest, angular) { Vector2d data; Matrix2d data_cov; data_cov . setIdentity(); - TimeStamp t = 0.0; - // Vector3d x(0,0,0); - VectorComposite x(Vector3d(0,0,0), "PO", {2,1}); - // Matrix3d P; P.setIdentity(); - VectorComposite P(Vector3d(1,1,1), "PO", {2,1}); - - auto F0 = problem->setPriorFactor(x, P, t); + TimeStamp t = 0.0; + + SpecComposite prior; + prior.emplace('P', SpecState("StatePoint2d", Vector2d::Zero(), "factor", Vector2d::Constant(1))); + prior.emplace('O', SpecState("StateAngle", Vector1d::Zero(), "factor", Vector1d::Constant(1))); + auto F0 = problem->setPrior(prior, t); processor->setOrigin(F0); // Straight one turn of the wheels, in one go diff --git a/test/gtest_sensor_base.cpp b/test/gtest_sensor_base.cpp index d8d5fcc40..b8cbc3bc0 100644 --- a/test/gtest_sensor_base.cpp +++ b/test/gtest_sensor_base.cpp @@ -332,11 +332,11 @@ TEST(SensorBase, makeshared_priors_POIA_mixed) VectorXd i_state_3D = VectorXd::Random(5); - auto S = std::make_shared<SensorDummy3d>(params, - SpecSensorComposite({{'P',SpecStateSensor("StatePoint3d", p_state_3D, "fix", vector0, true)}, - {'O',SpecStateSensor("StateQuaternion", o_state_3D, "factor", o_std_3D, true, o_std_3D)}, - {'I',SpecStateSensor("StateParams5", i_state_3D, "initial_guess")}, - {'A',SpecStateSensor("StateQuaternion", o_state_3D, "factor", o_std_3D)}})); + auto S = std::make_shared<SensorDummyPoia3d>(params, + SpecSensorComposite({{'P',SpecStateSensor("StatePoint3d", p_state_3D, "fix", vector0, true)}, + {'O',SpecStateSensor("StateQuaternion", o_state_3D, "factor", o_std_3D, true, o_std_3D)}, + {'I',SpecStateSensor("StateParams5", i_state_3D, "initial_guess")}, + {'A',SpecStateSensor("StateQuaternion", o_state_3D, "factor", o_std_3D)}})); // noise ASSERT_MATRIX_APPROX(S->computeNoiseCov(vector0), noise_cov_dummy, Constants::EPS); @@ -394,6 +394,7 @@ TEST(SensorBase, factory) auto valid = server.applySchema("SensorDummy"+toString(dim)+"d"); WOLF_WARN_COND(not valid and not wrong, server.getLog().str()); + WOLF_WARN_COND(valid and wrong, "Schema didn't detect any problem in a wrong yaml."); // CORRECT YAML if (not wrong) -- GitLab