diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index f4cae62c31f2c8389107388bdebedbec72a68b5b..e7bac59f83bbe1e36fbb2ff4ee60a885c5d2fcac 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -65,7 +65,13 @@ SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXd& _ex
} \
-
+struct ParamsStateBlock
+{
+ Eigen::VectorXd state;
+ std::string prior_mode;
+ Eigen::VectorXd sigma;
+ bool dynamic;
+};
/** \brief base struct for intrinsic sensor parameters
*
@@ -74,8 +80,71 @@ SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXd& _ex
struct ParamsSensorBase: public ParamsBase
{
std::string prefix = "sensor/";
+
+ int dim;
+
+ std::unordered_map<char, ParamsStateBlock> state_blocks;
+
+ ParamsSensorBase() = default;
+ ParamsSensorBase(std::string _unique_name, const ParamsServer& _server) :
+ ParamsBase(_unique_name, _server)
+ {
+ dim = _server.getParam<int>("problem/dimension");
+ if (dim != 2 and dim != 3)
+ {
+ WOLF_ERROR("ParamsSensorBase: parameter 'problem/dimension' should be 2 or 3. Value provided: ", dim);
+ throw std::runtime_error("ParamsSensorBase wrong 'problem/dimension'");
+ }
+
+ // P
+ state_blocks.emplace('P',ParamsStateBlock());
+ if (dim == 2)
+ {
+ state_blocks['P'].state = _server.getParam<Eigen::Vector2d> (prefix + _unique_name + "/P/state");
+ state_blocks['P'].prior_mode = _server.getParam<std::string> (prefix + _unique_name + "/P/prior_mode");
+ state_blocks['P'].dynamic = _server.getParam<bool> (prefix + _unique_name + "/P/is_dynamic");
+ if (state_blocks['P'].prior_mode == "factor")
+ state_blocks['P'].sigma = _server.getParam<Eigen::Vector2d> (prefix + _unique_name + "/P/sigma");
+ }
+ else if (dim == 3)
+ {
+ state_blocks['P'].state = _server.getParam<Eigen::Vector3d> (prefix + _unique_name + "/P/state");
+ state_blocks['P'].prior_mode = _server.getParam<std::string> (prefix + _unique_name + "/P/prior_mode");
+ state_blocks['P'].dynamic = _server.getParam<bool> (prefix + _unique_name + "/P/is_dynamic");
+ if (state_blocks['P'].prior_mode == "factor")
+ state_blocks['P'].sigma = _server.getParam<Eigen::Vector3d> (prefix + _unique_name + "/P/sigma");
+ }
+
+ // O
+ state_blocks.emplace('O',ParamsStateBlock());
+ if (dim == 2)
+ {
+ state_blocks['O'].state = _server.getParam<Eigen::Vector1d> (prefix + _unique_name + "/O/state");
+ state_blocks['O'].prior_mode = _server.getParam<std::string> (prefix + _unique_name + "/O/prior_mode");
+ state_blocks['O'].dynamic = _server.getParam<bool> (prefix + _unique_name + "/O/is_dynamic");
+ if (state_blocks['O'].prior_mode == "factor")
+ state_blocks['O'].sigma = _server.getParam<Eigen::Vector1d> (prefix + _unique_name + "/O/sigma");
+ }
+ else if (dim == 3)
+ {
+ state_blocks['O'].state = _server.getParam<Eigen::Vector4d> (prefix + _unique_name + "/O/state");
+ state_blocks['O'].prior_mode = _server.getParam<std::string> (prefix + _unique_name + "/O/prior_mode");
+ state_blocks['O'].dynamic = _server.getParam<bool> (prefix + _unique_name + "/O/is_dynamic");
+ if (state_blocks['O'].prior_mode == "factor")
+ state_blocks['O'].sigma = _server.getParam<Eigen::Vector3d> (prefix + _unique_name + "/O/sigma");
+ }
+
+ // I
+ state_blocks.emplace('I',ParamsStateBlock());
+ state_blocks['I'].state = _server.getParam<Eigen::VectorXd> (prefix + _unique_name + "/I/state");
+ state_blocks['I'].prior_mode = _server.getParam<std::string> (prefix + _unique_name + "/I/prior_mode");
+ state_blocks['I'].dynamic = _server.getParam<bool> (prefix + _unique_name + "/I/is_dynamic");
+ if (state_blocks['I'].prior_mode == "factor")
+ state_blocks['I'].sigma = _server.getParam<Eigen::VectorXd> (prefix + _unique_name + "/I/sigma");
+ }
+
~ParamsSensorBase() override = default;
- using ParamsBase::ParamsBase;
+
std::string print() const override
{
return "";
@@ -108,6 +177,17 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
void setProblem(ProblemPtr _problem) override final;
public:
+ /** \brief Constructor with params struct
+ *
+ * Constructor with parameter struct
+ * \param _tp Type of the sensor (types defined at wolf.h)
+ * \param _params ParamsSensorBasePtr pointer to params struct
+ *
+ **/
+ SensorBase(const std::string& _type,
+ ParamsSensorBasePtr _params,
+ const int& _intrinsics_size);
+
/** \brief Constructor with noise size
*
* Constructor with parameter vector
diff --git a/include/core/sensor/sensor_diff_drive.h b/include/core/sensor/sensor_diff_drive.h
index 212a955b19c1a76aecebaa90f7cc39be73f38a7a..6e9a486227ff48ceffc63a49f8f415bd22b32c4d 100644
--- a/include/core/sensor/sensor_diff_drive.h
+++ b/include/core/sensor/sensor_diff_drive.h
@@ -26,7 +26,7 @@ struct ParamsSensorDiffDrive : public ParamsSensorBase
double ticks_cov_factor;
ParamsSensorDiffDrive() = default;
- ParamsSensorDiffDrive(std::string _unique_name, const wolf::ParamsServer& _server)
+ ParamsSensorDiffDrive(std::string _unique_name, const ParamsServer& _server)
: ParamsSensorBase(_unique_name, _server)
{
radius_left = _server.getParam<double>(prefix + _unique_name + "/radius_left");
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 0d2d54638d145f333efa0e349990beda54d1b17e..f5e091572555bb0361bff35cdcc1f22419aa4b7e 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -9,6 +9,71 @@ namespace wolf {
unsigned int SensorBase::sensor_id_count_ = 0;
+SensorBase::SensorBase(const std::string& _type,
+ ParamsSensorBasePtr _params,
+ const int& _intrinsics_size) :
+ NodeBase("SENSOR", _type),
+ HasStateBlocks(""),
+ hardware_ptr_(),
+ sensor_id_(++sensor_id_count_), // simple ID factory
+ last_capture_(nullptr)
+{
+ for (auto sb_pair : _params->state_blocks)
+ {
+ const char& key = sb_pair.first;
+ const ParamsStateBlock& sb_param = sb_pair.second;
+
+ if (key == 'I' and _intrinsics_size == 0)
+ continue;
+
+ addStateBlock(key,
+ (key != 'O' ?
+ std::make_shared<StateBlock>(sb_param.state,sb_param.prior_mode == "fix") :
+ (_params->dim == 2 ?
+ std::static_pointer_cast<StateBlock>(std::make_shared<StateAngle>(sb_param.state, sb_param.prior_mode == "fix")) :
+ std::static_pointer_cast<StateBlock>(std::make_shared<StateQuaternion>(sb_param.state,sb_param.prior_mode == "fix")) ) ),
+ sb_param.dynamic);
+
+ if (sb_param.prior_mode == "factor")
+ addPriorParameter(key,
+ sb_param.state,
+ sb_param.sigma.asDiagonal());
+ }
+
+ /*// P
+ addStateBlock('P',
+ std::make_shared<StateBlock>(_params->state_blocks.at('P').state,
+ _params->state_blocks.at('P').prior_mode == "fix"),
+ _params->state_blocks.at('P').dynamic);
+
+ if (_params->state_blocks.at('P').prior_mode == "factor")
+ addPriorP(_params->state_blocks.at('P').state, _params->state_blocks.at('P').sigma.asDiagonal());
+
+ // O
+ addStateBlock('O',
+ (_params->dim == 2 ?
+ std::make_shared<StateAngle>(_params->state_blocks.at('O').state,
+ _params->state_blocks.at('O').prior_mode == "fix") :
+ std::make_shared<StateQuaternion>(_params->state_blocks.at('O').state,
+ _params->state_blocks.at('O').prior_mode == "fix") ),
+ _params->o_dynamic);
+
+ if (_params->state_blocks.at('O').prior_mode == "factor")
+ addPriorO(_params->p_state, _params->p_cov);
+
+ // I
+ if (_intrinsics_size != 0)
+ {
+ addStateBlock('I',
+ std::make_shared<StateBlock>(_params->state_blocks.at('I').state,
+ _params->state_blocks.at('I').prior_mode == "fix"),
+ _params->state_blocks.at('I').dynamic);
+
+ if (_params->state_blocks.at('I').prior_mode == "factor")
+ addPriorI(_params->state_blocks.at('I').state, _params->state_blocks.at('I').sigma.asDiagonal());
+ }*/
+}
+
SensorBase::SensorBase(const std::string& _type,
StateBlockPtr _p_ptr,
StateBlockPtr _o_ptr,