diff --git a/include/core/sensor/sensor_odom.h b/include/core/sensor/sensor_odom.h
index 8efa6ae80be7acdf4def33a41ddcb99466738a3b..9817b4235e1476741d068b9d14e743eb08d8d602 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 6a3d8adefcd3156f11be904fdd0cb5517e1a2e8f..4bafeb5ff67cfc36d5eccea46fa4c549ec275542 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 6fec7d76a82fed670bbf367237213e1fc195d8fe..6697d6e0bc3a21c8ca9fe6abbf283b4f0ab3a1c4 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 519ca225baf3b4b15d688162dcced59d7dc1b356..a6378199ab198dc9baf33a1afe687e70801baadc 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 c248eb442a986415e6cddc4cc745447d5b358e1f..560d94b1d70f559ebf0a1e18aefb6f3d2a663f1b 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 4d3ea92c2bad73ccf9d33c340150e8a51ed31457..8315a52bde28ec279121b66d218b39bacddf0c1e 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 c7d681d7f101c068a11d37b6df462d4b41b92255..83b76542b67aa10a262d30bf5d5d9af3f950a977 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 8712f0da95f856e2f21853924bc58a31e62ed1bc..c4fd5ef64fe46b942c9ab088802005adac47e1c9 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 4a3b7401b3caad2a905e6a19f67f0294534f9a16..b17b318f9be4506f9eecbf27669defd2b3fd282c 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 58a28fa17532374b8fb035c9f644a20cf5eec49d..c2a0ea42c635cfc8f2dcd51c9c17cb39a32f5318 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 782a06f05f0cd39dac17e2bdf74047d8161b878d..a61ad826553734812553b483b5f0178f06c0a7e5 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 bca6fed749f1158fc4898f113a5878db01366dd5..5cb186c8706002cc7fa469981fd21bde19121e2f 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 c67f9294fb9bff2ac5d1e82f827642b459f54a6a..0712b09ba6d49f1908e5b39cf5db53171bbdec6c 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 d8d5fcc40c1947bdc27a2bf565f29ad2ca68bf30..b8cbc3bc0e1c85fc76e55d8ca2128ae7031aec1f 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)