diff --git a/demos/hello_wolf/hello_wolf.cpp b/demos/hello_wolf/hello_wolf.cpp
index 793d409e4c7522ee2ebcc18ad01ec61cbbf1bbbc..b2b8b4fb6add295a6196e5992a729f6d96f3b8d2 100644
--- a/demos/hello_wolf/hello_wolf.cpp
+++ b/demos/hello_wolf/hello_wolf.cpp
@@ -130,7 +130,9 @@ int main()
// sensor odometer 2d
ParamsSensorOdom2dPtr intrinsics_odo = std::make_shared<ParamsSensorOdom2d>();
- SensorBasePtr sensor_odo = problem->installSensor("SensorOdom2d", "sensor odo", Vector3d(0,0,0), intrinsics_odo);
+ Priors priors_odo = {{'P',Prior("P", Vector2d::Zero())},
+ {'O',Prior("O", Vector1d::Zero())}};
+ SensorBasePtr sensor_odo = problem->installSensor("SensorOdom2d", "sensor odo", intrinsics_odo, priors_odo);
// processor odometer 2d
ParamsProcessorOdom2dPtr params_odo = std::make_shared<ParamsProcessorOdom2d>();
@@ -145,9 +147,11 @@ int main()
// sensor Range and Bearing
ParamsSensorRangeBearingPtr intrinsics_rb = std::make_shared<ParamsSensorRangeBearing>();
- intrinsics_rb->noise_range_metres_std = 0.1;
- intrinsics_rb->noise_bearing_degrees_std = 1.0;
- SensorBasePtr sensor_rb = problem->installSensor("SensorRangeBearing", "sensor RB", Vector3d(1,1,0), intrinsics_rb);
+ Priors priors_rb = {{'P',Prior("P", Vector2d(1,1))},
+ {'O',Prior("O", Vector1d::Zero())}};
+ intrinsics_rb->noise_range_metres_std = 0.1;
+ intrinsics_rb->noise_bearing_degrees_std = 1.0;
+ SensorBasePtr sensor_rb = problem->installSensor("SensorRangeBearing", "sensor RB", intrinsics_rb, priors_rb);
// processor Range and Bearing
ParamsProcessorRangeBearingPtr params_rb = std::make_shared<ParamsProcessorRangeBearing>();
diff --git a/demos/hello_wolf/sensor_range_bearing.cpp b/demos/hello_wolf/sensor_range_bearing.cpp
index 7073082e34be71b45e60b7671211e1617432a124..24dfc14b97a20d5fc9e4c9861da8cf32f0305f45 100644
--- a/demos/hello_wolf/sensor_range_bearing.cpp
+++ b/demos/hello_wolf/sensor_range_bearing.cpp
@@ -25,29 +25,28 @@
* Created on: Nov 30, 2017
* Author: jsola
*/
-
#include "sensor_range_bearing.h"
-#include "core/state_block/state_angle.h"
-namespace wolf
-{
+namespace wolf{
WOLF_PTR_TYPEDEFS(SensorRangeBearing);
-SensorRangeBearing::SensorRangeBearing(const Eigen::VectorXd& _extrinsics, const Eigen::Vector2d& _noise_std) :
- SensorBase("SensorRangeBearing",
- std::make_shared<StateBlock>(_extrinsics.head<2>(), true),
- std::make_shared<StateAngle>(_extrinsics(2), true),
- nullptr,
- _noise_std)
+SensorRangeBearing::SensorRangeBearing(const std::string& _unique_name,
+ const SizeEigen& _dim,
+ ParamsSensorRangeBearingPtr _params,
+ const Priors& _priors) :
+ SensorBase("SensorRangeBearing",_unique_name, _dim, _params, _priors)
{
- assert(_extrinsics.size() == 3 && "Bad extrinsics vector size. Must be 3 for 2d");
+ assert(_dim == 2 && "SensorRangeBearing only for 2D");
}
-SensorRangeBearing::SensorRangeBearing(const Eigen::VectorXd& _extrinsics, const ParamsSensorRangeBearingPtr _intr) :
- SensorRangeBearing(_extrinsics, Eigen::Vector2d(_intr->noise_range_metres_std, toRad(_intr->noise_bearing_degrees_std)))
+SensorRangeBearing::SensorRangeBearing(const std::string& _unique_name,
+ const SizeEigen& _dim,
+ ParamsSensorRangeBearingPtr _params,
+ const ParamsServer& _server) :
+ SensorBase("SensorRangeBearing",_unique_name, _dim, _params, _server)
{
- //
+ assert(_dim == 2 && "SensorRangeBearing only for 2D");
}
SensorRangeBearing::~SensorRangeBearing()
@@ -55,7 +54,6 @@ SensorRangeBearing::~SensorRangeBearing()
//
}
-
} /* namespace wolf */
// Register in the SensorFactory
@@ -63,5 +61,5 @@ SensorRangeBearing::~SensorRangeBearing()
namespace wolf
{
WOLF_REGISTER_SENSOR(SensorRangeBearing)
-WOLF_REGISTER_SENSOR_AUTO(SensorRangeBearing)
+WOLF_REGISTER_SENSOR_YAML(SensorRangeBearing)
} // namespace wolf
diff --git a/demos/hello_wolf/sensor_range_bearing.h b/demos/hello_wolf/sensor_range_bearing.h
index 80f55753b30364216fc22cdf34b1e7bfc2425168..76170f9b1d8801022b0a5eee5f2cace8bfce70b5 100644
--- a/demos/hello_wolf/sensor_range_bearing.h
+++ b/demos/hello_wolf/sensor_range_bearing.h
@@ -38,12 +38,10 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorRangeBearing);
struct ParamsSensorRangeBearing : public ParamsSensorBase
{
- double noise_range_metres_std = 0.05;
- double noise_bearing_degrees_std = 0.5;
- ParamsSensorRangeBearing()
- {
- //DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE.
- }
+ double noise_range_metres_std = 0.05;
+ double noise_bearing_degrees_std = 0.5;
+
+ ParamsSensorRangeBearing() = default;
ParamsSensorRangeBearing(std::string _unique_name, const ParamsServer& _server):
ParamsSensorBase(_unique_name, _server)
{
@@ -52,9 +50,9 @@ struct ParamsSensorRangeBearing : public ParamsSensorBase
}
std::string print() const
{
- return ParamsSensorBase::print() + "\n"
- + "noise_range_metres_std: " + std::to_string(noise_range_metres_std) + "\n"
- + "noise_bearing_degrees_std: " + std::to_string(noise_bearing_degrees_std) + "\n";
+ return ParamsSensorBase::print() + "\n"
+ + "noise_range_metres_std: " + toString(noise_range_metres_std) + "\n"
+ + "noise_bearing_degrees_std: " + toString(noise_bearing_degrees_std) + "\n";
}
};
@@ -63,11 +61,16 @@ WOLF_PTR_TYPEDEFS(SensorRangeBearing)
class SensorRangeBearing : public SensorBase
{
public:
- SensorRangeBearing(const Eigen::VectorXd& _extrinsics, const Eigen::Vector2d& _noise_std);
- SensorRangeBearing(const Eigen::VectorXd& _extrinsics, const ParamsSensorRangeBearingPtr _intr);
- WOLF_SENSOR_CREATE(SensorRangeBearing, ParamsSensorRangeBearing, 3);
+ SensorRangeBearing(const std::string& _unique_name,
+ const SizeEigen& _dim,
+ ParamsSensorRangeBearingPtr _params,
+ const Priors& _priors);
+ SensorRangeBearing(const std::string& _unique_name,
+ const SizeEigen& _dim,
+ ParamsSensorRangeBearingPtr _params,
+ const ParamsServer& _server);
+ WOLF_SENSOR_CREATE(SensorRangeBearing, ParamsSensorRangeBearing);
~SensorRangeBearing() override;
-
};
} /* namespace wolf */
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 9dfb7cae3e62adac06662e9414c75764045148e2..9c70f894f700413c035462e449dc9be64579d760 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -120,32 +120,31 @@ class Problem : public std::enable_shared_from_this<Problem>
// Hardware branch ------------------------------------
HardwareBaseConstPtr getHardware() const;
HardwareBasePtr getHardware();
-
+
/** \brief Factory method to install (create and add) sensors only from its properties
* \param _sen_type type of sensor
* \param _unique_sensor_name unique sensor name, used to identify the particular instance of the sensor
- * \param _extrinsics a vector of extrinsic parameters: size 2 for 2d position, 3 for 2d pose, 3 for 3d position, 7 for 3d pose.
- * \param _intrinsics a base-pointer to a derived struct defining the intrinsic parameters.
+ * \param _params a base-pointer to a derived struct defining the intrinsic parameters.
+ * \param _priors an unordered map of the states priors indexed by their key.
*/
- SensorBasePtr installSensor(const std::string& _sen_type, //
- const std::string& _unique_sensor_name, //
- const Eigen::VectorXd& _extrinsics, //
- ParamsSensorBasePtr _intrinsics = nullptr);
-
+ SensorBasePtr installSensor(const std::string& _sen_type,
+ const std::string& _unique_sensor_name,
+ ParamsSensorBasePtr _params,
+ const Priors& _priors);
/** \brief Factory method to install (create and add) sensors only from its properties -- Helper method loading parameters from file
* \param _sen_type type of sensor
* \param _unique_sensor_name unique sensor name, used to identify the particular instance of the sensor
- * \param _extrinsics a vector of extrinsic parameters: size 2 for 2d position, 3 for 2d pose, 3 for 3d position, 7 for 3d pose.
- * \param _intrinsics_filename the name of a file containing the intrinsic parameters in a format compatible with the intrinsics creator registered in FactoryParamsSensor under the key _sen_type.
+ * \param _params_yaml_filename the name of a file containing the parameters in a format compatible with the intrinsics creator registered in FactoryParamsSensor under the key _sen_type.
*/
- SensorBasePtr installSensor(const std::string& _sen_type, //
- const std::string& _unique_sensor_name, //
- const Eigen::VectorXd& _extrinsics, //
- const std::string& _intrinsics_filename);
- /**
- Custom installSensor using the parameters server
- */
- SensorBasePtr installSensor(const std::string& _sen_type, //
+ SensorBasePtr installSensor(const std::string& _sen_type,
+ const std::string& _unique_sensor_name,
+ const std::string& _params_yaml_filename);
+ /**\brief Factory method to install (create and add) sensors only from its properties -- Helper method loading parameters from file
+ * \param _sen_type type of sensor
+ * \param _unique_sensor_name unique sensor name, used to identify the particular instance of the sensor
+ * \param _server the parameter server.
+ */
+ SensorBasePtr installSensor(const std::string& _sen_type,
const std::string& _unique_sensor_name,
const ParamsServer& _server);
/** \brief get a sensor pointer by its name
diff --git a/include/core/sensor/factory_sensor.h b/include/core/sensor/factory_sensor.h
index 13968e370e83326135ef5958e3abcd9e457a92b3..a1c7b77a4a4abce394410335cd16607a5fc265be 100644
--- a/include/core/sensor/factory_sensor.h
+++ b/include/core/sensor/factory_sensor.h
@@ -221,19 +221,25 @@ typedef Factory<SensorBase,
SizeEigen,
const std::string&> FactorySensorYaml;
+typedef Factory<SensorBase,
+ const std::string&,
+ SizeEigen,
+ ParamsSensorBasePtr,
+ const Priors&> FactorySensorPriors;
+
template<>
inline std::string FactorySensor::getClass() const
{
return "FactorySensor";
}
-#define WOLF_REGISTER_SENSOR(SensorType) \
- namespace{ const bool WOLF_UNUSED SensorType##Registered = \
- FactorySensor::registerCreator(#SensorType, SensorType::create); } \
-
-#define WOLF_REGISTER_SENSOR_YAML(SensorType) \
- namespace{ const bool WOLF_UNUSED SensorType##YamlRegistered = \
- FactorySensorYaml::registerCreator(#SensorType, SensorType::create); } \
+#define WOLF_REGISTER_SENSOR(SensorType) \
+ namespace{ const bool WOLF_UNUSED SensorType##Registered = \
+ FactorySensor::registerCreator(#SensorType, SensorType::create);} \
+ namespace{ const bool WOLF_UNUSED SensorType##YamlRegistered = \
+ FactorySensorYaml::registerCreator(#SensorType, SensorType::create);} \
+ namespace{ const bool WOLF_UNUSED SensorType##PriorsRegistered = \
+ FactorySensorPriors::registerCreator(#SensorType, SensorType::create);} \
} /* namespace wolf */
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 48eddca9fe75da37f142e5c84af507c5d5f98df4..13c5a77d60bac6100d13f7e0d06cc4f90870b595 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -50,35 +50,47 @@ namespace wolf {
* preferably just after the constructors.
*
* In order to use this macro, the derived sensor class, SensorClass,
- * must have a constructor available with the API:
+ * must have two constructors available with the API:
*
* SensorClass(const std::string& _unique_name,
* SizeEigen _dim,
* ParamsSensorClassPtr _params,
* const ParamsServer& _server)
*
+ * SensorClass(const std::string& _unique_name,
+ * SizeEigen _dim,
+ * ParamsSensorClassPtr _params,
+ * const Priors& _priors)
+ *
*/
-#define WOLF_SENSOR_CREATE(SensorClass, ParamsSensorClass) \
-static SensorBasePtr create(const std::string& _unique_name, SizeEigen _dim, const ParamsServer& _server) \
-{ \
- auto params = std::make_shared<ParamsSensorClass>(_unique_name, _server); \
- \
- auto sensor = std::make_shared<SensorClass>(_unique_name, _dim, params, _server); \
- \
- return sensor; \
-} \
-static SensorBasePtr create(const std::string& _unique_name, SizeEigen _dim, const std::string& _yaml_filepath) \
-{ \
- auto parser = ParserYaml(_yaml_filepath, "", true); \
- \
- auto server = ParamsServer(parser.getParams(), "sensor/" + _unique_name); \
- \
- auto params = std::make_shared<ParamsSensorClass>(_unique_name, server); \
- \
- auto sensor = std::make_shared<SensorClass>(_unique_name, _dim, params, server); \
- \
- return sensor; \
-} \
+#define WOLF_SENSOR_CREATE(SensorClass, ParamsSensorClass) \
+static SensorBasePtr create(const std::string& _unique_name, \
+ SizeEigen _dim, \
+ const ParamsServer& _server) \
+{ \
+ auto params = std::make_shared<ParamsSensorClass>(_unique_name, _server); \
+ \
+ return std::make_shared<SensorClass>(_unique_name, _dim, params, _server); \
+} \
+static SensorBasePtr create(const std::string& _unique_name, \
+ SizeEigen _dim, \
+ const std::string& _yaml_filepath) \
+{ \
+ auto parser = ParserYaml(_yaml_filepath, "", true); \
+ auto server = ParamsServer(parser.getParams(), "sensor/" + _unique_name); \
+ auto params = std::make_shared<ParamsSensorClass>(_unique_name, server); \
+ \
+ return std::make_shared<SensorClass>(_unique_name, _dim, params, server); \
+} \
+static SensorBasePtr create(const std::string& _unique_name, \
+ SizeEigen _dim, \
+ ParamsSensorBasePtr _params, \
+ const Priors& _priors) \
+{ \
+ auto params_derived = std::static_pointer_cast<ParamsSensorClass>(_params); \
+ \
+ return std::make_shared<SensorClass>(_unique_name, _dim, params_derived, _priors); \
+} \
/** \brief base struct for intrinsic sensor parameters
*
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index f624a36dfced0a59a0f26e9aa3bac6d3df3240fc..e551fd69290476c1efcbbd9a9c903b69d5f16124 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -209,35 +209,38 @@ Problem::~Problem()
// WOLF_DEBUG("destructed -P");
}
-SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
- const std::string& _unique_sensor_name, //
- const Eigen::VectorXd& _extrinsics, //
- ParamsSensorBasePtr _intrinsics)
-{
- //SensorBasePtr sen_ptr = FactorySensor::create(_sen_type, _unique_sensor_name, _extrinsics, _intrinsics);
- // sen_ptr->link(getHardware());
- // return sen_ptr;
- return nullptr;
+SensorBasePtr Problem::installSensor(const std::string& _sen_type,
+ const std::string& _unique_sensor_name,
+ ParamsSensorBasePtr _params,
+ const Priors& _priors)
+{
+ SensorBasePtr sen_ptr = FactorySensorPriors::create(_sen_type,
+ _unique_sensor_name,
+ dim_,
+ _params,
+ _priors);
+ sen_ptr->link(getHardware());
+ return sen_ptr;
}
-SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
- const std::string& _unique_sensor_name, //
- const Eigen::VectorXd& _extrinsics, //
+SensorBasePtr Problem::installSensor(const std::string& _sen_type,
+ const std::string& _unique_sensor_name,
const std::string& _params_yaml_filename)
{
- if (_params_yaml_filename != "")
+ if (_params_yaml_filename == "" or not file_exists(_params_yaml_filename))
{
- assert(file_exists(_params_yaml_filename) && "Cannot install sensor: intrinsics' YAML file does not exist.");
- //ParamsSensorBasePtr intr_ptr = FactoryParamsSensor::create(_sen_type, _intrinsics_filename);
- return installSensor(_sen_type, _unique_sensor_name, _extrinsics, ParamsSensorBasePtr());
+ throw std::runtime_error("Cannot install sensor: provided params YAML file: '" + _params_yaml_filename + "' does not exist.");
}
- else
- return installSensor(_sen_type, _unique_sensor_name, _extrinsics, ParamsSensorBasePtr());
-
+ SensorBasePtr sen_ptr = FactorySensorYaml::create(_sen_type,
+ _unique_sensor_name,
+ dim_,
+ _params_yaml_filename);
+ sen_ptr->link(getHardware());
+ return sen_ptr;
}
-SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
- const std::string& _unique_sensor_name, //
+SensorBasePtr Problem::installSensor(const std::string& _sen_type,
+ const std::string& _unique_sensor_name,
const ParamsServer& _server)
{
SensorBasePtr sen_ptr = FactorySensor::create(_sen_type, _unique_sensor_name, dim_, _server);
@@ -245,9 +248,9 @@ SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
return sen_ptr;
}
-ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
- const std::string& _unique_processor_name, //
- SensorBasePtr _corresponding_sensor_ptr, //
+ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type,
+ const std::string& _unique_processor_name,
+ SensorBasePtr _corresponding_sensor_ptr,
ParamsProcessorBasePtr _prc_params)
{
if (_corresponding_sensor_ptr == nullptr)
diff --git a/test/gtest_processor_tracker_feature_dummy.cpp b/test/gtest_processor_tracker_feature_dummy.cpp
index b1dbdfee8a84823106a59e699da1cd28bb532982..95af593518002badf85ec15a337b439b6a1703ed 100644
--- a/test/gtest_processor_tracker_feature_dummy.cpp
+++ b/test/gtest_processor_tracker_feature_dummy.cpp
@@ -105,7 +105,7 @@ class ProcessorTrackerFeatureDummyTest : public testing::Test
problem = Problem::create("PO", 2);
// Install camera
- sensor = problem->installSensor("SensorOdom2d", "auxiliar sensor", (Eigen::Vector3d() << 0,0,0).finished(), std::make_shared<ParamsSensorBase>());
+ sensor = problem->installSensor("SensorOdom2d", "auxiliar sensor", wolf_root + "/test/yaml/sensor_odom_2d.yaml");
// Install processor
params = std::make_shared<ParamsProcessorTrackerFeatureDummy>();
diff --git a/test/gtest_processor_tracker_landmark_dummy.cpp b/test/gtest_processor_tracker_landmark_dummy.cpp
index 4ed3c0846017e15fe5882607ff1807fbbbbc93f5..68f8147d38f23a8dc3a712b46fc2207d393baa71 100644
--- a/test/gtest_processor_tracker_landmark_dummy.cpp
+++ b/test/gtest_processor_tracker_landmark_dummy.cpp
@@ -124,7 +124,7 @@ class ProcessorTrackerLandmarkDummyTest : public testing::Test
problem = Problem::create("PO", 2);
// Install camera
- sensor = problem->installSensor("SensorOdom2d", "auxiliar sensor", (Eigen::Vector3d() << 0,0,0).finished(), std::make_shared<ParamsSensorBase>());
+ sensor = problem->installSensor("SensorOdom2d", "auxiliar sensor", wolf_root + "/test/yaml/sensor_odom_2d.yaml");
// Install processor
params = std::make_shared<ParamsProcessorTrackerLandmarkDummy>();
diff --git a/test/gtest_sensor_odom_2d.cpp b/test/gtest_sensor_odom_2d.cpp
index 8fdaf0907eff360158d905b07d09c4d021012ba1..7b87fdf69a6b5ea3410f180dbe9f51ba4ee6a3c1 100644
--- a/test/gtest_sensor_odom_2d.cpp
+++ b/test/gtest_sensor_odom_2d.cpp
@@ -279,7 +279,7 @@ TEST(SensorOdom2d, server)
WOLF_INFO("Creating sensor from name sensor_mixed");
// Yaml parser
- ParserYaml parser = ParserYaml("test/yaml/sensor_odom_2d/sensor_odom_2d.yaml", wolf_root, true);
+ ParserYaml parser = ParserYaml("test/yaml/sensor_odom_2d/sensor_odom_2d_mixed.yaml", wolf_root, true);
ParamsServer server = ParamsServer(parser.getParams(), "sensor/sensor_mixed");
auto params = std::make_shared<ParamsSensorOdom2d>("sensor_mixed", server);
@@ -299,7 +299,7 @@ TEST(SensorOdom2d, server)
WOLF_INFO("Creating sensor from name sensor_mixed_wrong");
// Yaml parser
- ParserYaml parser = ParserYaml("test/yaml/sensor_odom_2d/sensor_odom_2d_wrong.yaml", wolf_root, true);
+ ParserYaml parser = ParserYaml("test/yaml/sensor_odom_2d/sensor_odom_2d_mixed_wrong.yaml", wolf_root, true);
ParamsServer server = ParamsServer(parser.getParams(), "sensor/sensor_mixed_wrong");
ASSERT_THROW(std::make_shared<SensorOdom2d>("sensor_mixed_wrong", 2,
@@ -367,7 +367,7 @@ TEST(SensorOdom2d, factory)
WOLF_INFO("Creating sensor from name sensor_mixed");
// Yaml parser
- ParserYaml parser = ParserYaml("test/yaml/sensor_odom_2d/sensor_odom_2d.yaml", wolf_root, true);
+ ParserYaml parser = ParserYaml("test/yaml/sensor_odom_2d/sensor_odom_2d_mixed.yaml", wolf_root, true);
ParamsServer server = ParamsServer(parser.getParams(), "sensor/sensor_mixed");
auto S = FactorySensor::create("SensorOdom2d", "sensor_mixed", 2, server);
@@ -386,7 +386,7 @@ TEST(SensorOdom2d, factory)
WOLF_INFO("Creating sensor from name sensor_mixed_wrong");
// Yaml parser
- ParserYaml parser = ParserYaml("test/yaml/sensor_odom_2d/sensor_odom_2d_wrong.yaml", wolf_root, true);
+ ParserYaml parser = ParserYaml("test/yaml/sensor_odom_2d/sensor_odom_2d_mixed_wrong.yaml", wolf_root, true);
ParamsServer server = ParamsServer(parser.getParams(), "sensor/sensor_mixed_wrong");
ASSERT_THROW(FactorySensor::create("SensorOdom2d", "sensor_mixed_wrong", 2, server),
@@ -451,7 +451,7 @@ TEST(SensorOdom2d, factory_yaml)
WOLF_INFO("Creating sensor from name sensor_mixed");
auto S = FactorySensorYaml::create("SensorOdom2d", "sensor_mixed", 2,
- wolf_root + "/test/yaml/sensor_odom_2d/sensor_odom_2d.yaml");
+ wolf_root + "/test/yaml/sensor_odom_2d/sensor_odom_2d_mixed.yaml");
// check
checkSensorOdom2d(S,
@@ -467,7 +467,7 @@ TEST(SensorOdom2d, factory_yaml)
WOLF_INFO("Creating sensor from name sensor_mixed_wrong");
ASSERT_THROW(FactorySensorYaml::create("SensorOdom2d", "sensor_mixed_wrong", 2,
- wolf_root + "/test/yaml/sensor_odom_2d/sensor_odom_2d_wrong.yaml"),
+ wolf_root + "/test/yaml/sensor_odom_2d/sensor_odom_2d_mixed_wrong.yaml"),
std::runtime_error);
}
}
diff --git a/test/yaml/sensor_base/sensor_all.yaml b/test/yaml/sensor_base/sensor_all.yaml
deleted file mode 100644
index 11e3bbe82ce94ea8d48866cb72ca7fb25b78f052..0000000000000000000000000000000000000000
--- a/test/yaml/sensor_base/sensor_all.yaml
+++ /dev/null
@@ -1,537 +0,0 @@
-sensor:
-
- #############################################################################################
- ########################################## CORRECT ##########################################
- #############################################################################################
-
- sensor_PO_2D_fix:
- states:
- P:
- mode: fix
- state: [1, 2]
- dynamic: false
- O:
- mode: fix
- state: [1]
- dynamic: false
- noise_std: [0.1, 0.2]
-
- sensor_PO_3D_fix:
- states:
- P:
- mode: fix
- state: [1, 2, 3]
- dynamic: false
- O:
- mode: fix
- state: [1, 0, 0, 0]
- dynamic: false
- noise_std: [0.1, 0.2]
-
- sensor_PO_2D_initial_guess:
- states:
- P:
- mode: initial_guess
- state: [1, 2]
- dynamic: false
- O:
- mode: initial_guess
- state: [1]
- dynamic: false
- noise_std: [0.1, 0.2]
-
- sensor_PO_3D_initial_guess:
- states:
- P:
- mode: initial_guess
- state: [1, 2, 3]
- dynamic: false
- O:
- mode: initial_guess
- state: [1, 0, 0, 0]
- dynamic: false
- noise_std: [0.1, 0.2]
-
- sensor_PO_2D_factor:
- states:
- P:
- mode: factor
- state: [1, 2]
- noise_std: [0.1, 0.2]
- dynamic: false
- O:
- mode: factor
- state: [1]
- noise_std: [0.1]
- dynamic: false
- noise_std: [0.1, 0.2]
-
- sensor_PO_3D_factor:
- states:
- P:
- mode: factor
- state: [1, 2, 3]
- noise_std: [0.1, 0.2, 0.3]
- dynamic: false
- O:
- mode: factor
- state: [1, 0, 0, 0]
- noise_std: [0.1, 0.2, 0.3]
- dynamic: false
- noise_std: [0.1, 0.2]
-
- sensor_PO_2D_fix_dynamic:
- states:
- P:
- mode: fix
- state: [1, 2]
- dynamic: true
- O:
- mode: fix
- state: [1]
- dynamic: true
- noise_std: [0.1, 0.2]
-
- sensor_PO_3D_fix_dynamic:
- states:
- P:
- mode: fix
- state: [1, 2, 3]
- dynamic: true
- O:
- mode: fix
- state: [1, 0, 0, 0]
- dynamic: true
- noise_std: [0.1, 0.2]
-
- sensor_PO_2D_initial_guess_dynamic:
- states:
- P:
- mode: initial_guess
- state: [1, 2]
- dynamic: true
- O:
- mode: initial_guess
- state: [1]
- dynamic: true
- noise_std: [0.1, 0.2]
-
- sensor_PO_3D_initial_guess_dynamic:
- states:
- P:
- mode: initial_guess
- state: [1, 2, 3]
- dynamic: true
- O:
- mode: initial_guess
- state: [1, 0, 0, 0]
- dynamic: true
- noise_std: [0.1, 0.2]
-
- sensor_PO_2D_factor_dynamic:
- states:
- P:
- mode: factor
- state: [1, 2]
- noise_std: [0.1, 0.2]
- dynamic: true
- O:
- mode: factor
- state: [1]
- noise_std: [0.1]
- dynamic: true
- noise_std: [0.1, 0.2]
-
- sensor_PO_3D_factor_dynamic:
- states:
- P:
- mode: factor
- state: [1, 2, 3]
- noise_std: [0.1, 0.2, 0.3]
- dynamic: true
- O:
- mode: factor
- state: [1, 0, 0, 0]
- noise_std: [0.1, 0.2, 0.3]
- dynamic: true
- noise_std: [0.1, 0.2]
-
- sensor_PO_2D_fix_dynamic_drift:
- states:
- P:
- mode: fix
- state: [1, 2]
- dynamic: true
- drift_std: [0.1, 0.2]
- O:
- mode: fix
- state: [1]
- dynamic: true
- drift_std: [0.1]
- noise_std: [0.1, 0.2]
-
- sensor_PO_3D_fix_dynamic_drift:
- states:
- P:
- mode: fix
- state: [1, 2, 3]
- dynamic: true
- drift_std: [0.1, 0.2, 0.3]
- O:
- mode: fix
- state: [1, 0, 0, 0]
- dynamic: true
- drift_std: [0.1, 0.2, 0.3]
- noise_std: [0.1, 0.2]
-
- sensor_PO_2D_initial_guess_dynamic_drift:
- states:
- P:
- mode: initial_guess
- state: [1, 2]
- dynamic: true
- drift_std: [0.1, 0.2]
- O:
- mode: initial_guess
- state: [1]
- dynamic: true
- drift_std: [0.1]
- noise_std: [0.1, 0.2]
-
- sensor_PO_3D_initial_guess_dynamic_drift:
- states:
- P:
- mode: initial_guess
- state: [1, 2, 3]
- dynamic: true
- drift_std: [0.1, 0.2, 0.3]
- O:
- mode: initial_guess
- state: [1, 0, 0, 0]
- dynamic: true
- drift_std: [0.1, 0.2, 0.3]
- noise_std: [0.1, 0.2]
-
- sensor_PO_2D_factor_dynamic_drift:
- states:
- P:
- mode: factor
- state: [1, 2]
- noise_std: [0.1, 0.2]
- dynamic: true
- drift_std: [0.1, 0.2]
- O:
- mode: factor
- state: [1]
- noise_std: [0.1]
- dynamic: true
- drift_std: [0.1]
- noise_std: [0.1, 0.2]
-
- sensor_PO_3D_factor_dynamic_drift:
- states:
- P:
- mode: factor
- state: [1, 2, 3]
- noise_std: [0.1, 0.2, 0.3]
- dynamic: true
- drift_std: [0.1, 0.2, 0.3]
- O:
- mode: factor
- state: [1, 0, 0, 0]
- noise_std: [0.1, 0.2, 0.3]
- dynamic: true
- drift_std: [0.1, 0.2, 0.3]
- noise_std: [0.1, 0.2]
-
- sensor_POIA_3D:
- states:
- P:
- mode: factor
- state: [1, 2, 3]
- noise_std: [0.1, 0.2, 0.3]
- dynamic: true
- O:
- mode: fix
- state: [1, 0, 0, 0]
- dynamic: false
- I:
- mode: initial_guess
- state: [1, 2, 3, 4]
- dynamic: true
- drift_std: [0.1, 0.2, 0.3, 0.4]
- A:
- mode: factor
- state: [1, 0, 0, 0]
- noise_std: [0.1, 0.2, 0.3]
- dynamic: true
- drift_std: [0.1, 0.2, 0.3]
- noise_std: [0.1, 0.2]
-
- #############################################################################################
- ######################################### INCORRECT #########################################
- #############################################################################################
-
- sensor_PO_2D_fix_wrong:
- states:
- P:
- mode: fix
- state: [1, 2]
- #dynamic: false #missing
- O:
- mode: fix
- state: [1]
- dynamic: false
- noise_std: [0.1, 0.2]
-
- sensor_PO_3D_fix_wrong:
- states:
- P:
- mode: fix
- state: [1, 2, 3]
- dynamic: false
- O:
- mode: fix
- state: [1, 0, 0, 0]
- dynamic: false
- #noise_std: [0.1, 0.2] #missing
-
- sensor_PO_2D_initial_guess_wrong:
- states:
- P:
- mode: initial_guess
- state: [1, 2]
- dynamic: false
- O:
- #mode: initial_guess #missing
- state: [1]
- dynamic: false
- noise_std: [0.1, 0.2]
-
- sensor_PO_3D_initial_guess_wrong:
- states:
- P:
- mode: initial_guess
- state: [1, 2, 3]
- dynamic: false
- # O: #missing
- # mode: initial_guess
- # state: [1, 0, 0, 0]
- # dynamic: false
- noise_std: [0.1, 0.2]
-
- sensor_PO_2D_factor_wrong:
- states:
- P:
- mode: factor
- state: [1, 2]
- noise_std: [0.1, 0.2]
- dynamic: false
- O:
- mode: factor
- #state: [1] #missing
- noise_std: [0.1]
- dynamic: false
- noise_std: [0.1, 0.2]
-
- sensor_PO_3D_factor_wrong:
- states:
- P:
- mode: factor
- state: [1, 2, 3]
- #noise_std: [0.1, 0.2, 0.3] #missing
- dynamic: false
- O:
- mode: factor
- state: [1, 0, 0, 0]
- noise_std: [0.1, 0.2, 0.3]
- dynamic: false
- noise_std: [0.1, 0.2]
-
- sensor_PO_2D_fix_dynamic_wrong:
- states:
- P:
- mode: fix
- state: [1, 2, 3] # wrong size
- dynamic: true
- O:
- mode: fix
- state: [1]
- dynamic: true
- noise_std: [0.1, 0.2]
-
- sensor_PO_3D_fix_dynamic_wrong:
- states:
- P:
- mode: fix
- state: [1, 2, 3]
- dynamic: true
- O:
- mode: fix
- state: [1, 0, 0, 1] # not normalized
- dynamic: true
- noise_std: [0.1, 0.2]
-
- sensor_PO_2D_initial_guess_dynamic_wrong:
- states:
- # P: #missing
- # mode: initial_guess
- # state: [1, 2]
- # dynamic: true
- O:
- mode: initial_guess
- state: [1]
- dynamic: true
- noise_std: [0.1, 0.2]
-
- sensor_PO_3D_initial_guess_dynamic_wrong:
- states:
- P:
- mode: initial_guess
- state: [1, 2] # wrong size
- dynamic: true
- O:
- mode: initial_guess
- state: [1, 0, 0, 0]
- dynamic: true
- noise_std: [0.1, 0.2]
-
- sensor_PO_2D_factor_dynamic_wrong:
- states:
- P:
- mode: factor
- state: [1, 2]
- noise_std: [0.1, 0.2, 0.3] #wrong size
- dynamic: true
- O:
- mode: factor
- state: [1]
- noise_std: [0.1]
- dynamic: true
- noise_std: [0.1, 0.2]
-
- sensor_PO_3D_factor_dynamic_wrong:
- states:
- P:
- mode: factor
- state: [1, 2, 3]
- noise_std: [0.1, 0.2, 0.3]
- dynamic: true
- O:
- mode: factor
- state: [1, 0, 0, 0]
- noise_std: [0.1, 0.2, 0.3, 0.4] # wrong size
- dynamic: true
- noise_std: [0.1, 0.2]
-
- sensor_PO_2D_fix_dynamic_drift_wrong:
- states:
- P:
- mode: fix
- state: [1, 2]
- dynamic: true
- drift_std: [0.1, 0.2, 0.3] #wrong size
- O:
- mode: fix
- state: [1]
- dynamic: true
- drift_std: [0.1]
- noise_std: [0.1, 0.2]
-
- sensor_PO_3D_fix_dynamic_drift_wrong:
- states:
- P:
- mode: fix
- state: [1, 2, 3]
- dynamic: true
- drift_std: [0.1, 0.2, 0.3]
- O:
- mode: fix
- state: [1, 0, 0, 0]
- #dynamic: true #missing
- drift_std: [0.1, 0.2, 0.3]
- noise_std: [0.1, 0.2]
-
- sensor_PO_2D_initial_guess_dynamic_drift_wrong:
- states:
- P:
- mode: initial_guess
- state: [1] #wrong size
- dynamic: true
- drift_std: [0.1, 0.2]
- O:
- mode: initial_guess
- state: [1]
- dynamic: true
- drift_std: [0.1]
- noise_std: [0.1, 0.2]
-
- sensor_PO_3D_initial_guess_dynamic_drift_wrong:
- states:
- P:
- mode: initial_guess
- state: [1, 2, 3]
- dynamic: true
- drift_std: [0.1, 0.2, 0.3]
- O:
- #mode: initial_guess #missing
- state: [1, 0, 0, 0]
- dynamic: true
- drift_std: [0.1, 0.2, 0.3]
- noise_std: [0.1, 0.2]
-
- sensor_PO_2D_factor_dynamic_drift_wrong:
- states:
- P:
- mode: factor
- state: [1, 2]
- noise_std: [0.1, 0.2]
- dynamic: true
- drift_std: [0.1] #wrong size
- O:
- mode: factor
- state: [1]
- noise_std: [0.1]
- dynamic: true
- drift_std: [0.1]
- noise_std: [0.1, 0.2]
-
- sensor_PO_3D_factor_dynamic_drift_wrong:
- states:
- P:
- mode: factor
- state: [1, 2, 3]
- noise_std: [0.1, 0.2, 0.3]
- dynamic: true
- drift_std: [0.1, 0.2, 0.3]
- O:
- mode: factor
- state: [1, 0, 0, 0]
- noise_std: [0.1, 0.2, 0.3, 0.4] #wrong size
- dynamic: true
- drift_std: [0.1, 0.2, 0.3]
- noise_std: [0.1, 0.2]
-
- sensor_POIA_3D_wrong:
- states:
- P:
- mode: factor
- state: [1, 2, 3]
- noise_std: [0.1, 0.2, 0.3]
- dynamic: true
- O:
- mode: fix
- state: [1, 0, 0, 0]
- dynamic: false
- I:
- mode: initial_guess
- state: [1, 2, 3, 4]
- dynamic: true
- drift_std: [0.1, 0.2, 0.3, 0.4]
- # A:
- # mode: factor
- # state: [1, 0, 0, 0]
- # noise_std: [0.1, 0.2, 0.3]
- # dynamic: true
- # drift_std: [0.1, 0.2, 0.3]
- noise_std: [0.1, 0.2]
\ No newline at end of file
diff --git a/test/yaml/sensor_odom_2d.yaml b/test/yaml/sensor_odom_2d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..16adc66acf10995f74007686e2722bb5a7518f7c
--- /dev/null
+++ b/test/yaml/sensor_odom_2d.yaml
@@ -0,0 +1,12 @@
+states:
+ P:
+ mode: fix
+ state: [1, 2]
+ dynamic: false
+ O:
+ mode: fix
+ state: [3]
+ dynamic: false
+noise_std: [0.1, 0.2]
+k_disp_to_disp: 0.5
+k_rot_to_rot: 0.8
diff --git a/test/yaml/sensor_odom_2d/sensor_odom_2d.yaml b/test/yaml/sensor_odom_2d/sensor_odom_2d_mixed.yaml
similarity index 100%
rename from test/yaml/sensor_odom_2d/sensor_odom_2d.yaml
rename to test/yaml/sensor_odom_2d/sensor_odom_2d_mixed.yaml
diff --git a/test/yaml/sensor_odom_2d/sensor_odom_2d_wrong.yaml b/test/yaml/sensor_odom_2d/sensor_odom_2d_mixed_wrong.yaml
similarity index 100%
rename from test/yaml/sensor_odom_2d/sensor_odom_2d_wrong.yaml
rename to test/yaml/sensor_odom_2d/sensor_odom_2d_mixed_wrong.yaml