diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 701c5877277f64f24aee2c7641264a7ee148fdb7..ec85b16148970e54351b3d56137636101f900072 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -27,10 +27,12 @@ namespace wolf{
class HardwareBase;
class ProcessorBase;
class StateBlock;
+class ParamsServer;
}
//Wolf includes
#include "core/common/wolf.h"
+#include "core/state_block/prior.h"
#include "core/common/node_base.h"
#include "core/common/time_stamp.h"
#include "core/common/params_base.h"
@@ -91,12 +93,18 @@ SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXd& _ex
*/
struct ParamsSensorBase: public ParamsBase
{
- std::string prefix = "sensor/";
+ Eigen::VectorXd noise_std;
+
+ ParamsSensorBase() = default;
+ ParamsSensorBase(std::string prefix, const wolf::ParamsServer& _server)
+ {
+ noise_std = _server.getParam<Eigen::VectorXd>(prefix + "/noise_std");
+ }
~ParamsSensorBase() override = default;
- using ParamsBase::ParamsBase;
+
std::string print() const override
{
- return "";
+ return "noise_std: " + std::to_string(noise_std) + "\n";
}
};
@@ -124,6 +132,8 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
Eigen::MatrixXd noise_cov_; // cov matrix of noise
void setProblem(ProblemPtr _problem) override final;
+ virtual void loadParams(ParamsSensorBasePtr _params);
+ virtual void loadPriors(Priors _priors, SizeEigen _dim);
public:
@@ -131,57 +141,28 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
*
* Constructor with parameter vector
* \param _tp Type of the sensor (types defined at wolf.h)
- * \param _priors list of the priors of the sensor states
+ * \param _priors priors of the sensor states
* \param _params params struct
*
**/
SensorBase(const std::string& _type,
- std::list<Prior> _priors,
+ const SizeEigen& _dim,
+ const Priors& _priors,
ParamsSensorBasePtr _params);
- /** \brief Constructor with noise size
- *
- * Constructor with parameter vector
- * \param _tp Type of the sensor (types defined at wolf.h)
- * \param _p_ptr StateBlock pointer to the sensor position
- * \param _o_ptr StateBlock pointer to the sensor orientation
- * \param _intr_ptr StateBlock pointer to the sensor intrinsic params that might be estimated (if unfixed).
- * \param _noise_size dimension of the noise term
- * \param _p_dyn Flag indicating if position is dynamic (moving) or static (not moving)
- * \param _o_dyn Flag indicating if orientation is dynamic (moving) or static (not moving)
- * \param _intr_dyn Flag indicating if intrinsics is dynamic (moving) or static (not moving)
- *
- **/
- SensorBase(const std::string& _type,
- StateBlockPtr _p_ptr,
- StateBlockPtr _o_ptr,
- StateBlockPtr _intr_ptr,
- const unsigned int _noise_size,
- const bool _p_dyn = false,
- const bool _o_dyn = false,
- const bool _intr_dyn = false);
-
- /** \brief Constructor with noise std vector
+ /** \brief Constructor with ParamServer and keys
*
* Constructor with parameter vector
* \param _tp Type of the sensor (types defined at wolf.h)
- * \param _p_ptr StateBlock pointer to the sensor position
- * \param _o_ptr StateBlock pointer to the sensor orientation
- * \param _intr_ptr StateBlock pointer to the sensor intrinsic params that might be estimated (if unfixed).
- * \param _noise_std standard deviations of the noise term
- * \param _p_dyn Flag indicating if position is dynamic (moving) or static (not moving)
- * \param _o_dyn Flag indicating if orientation is dynamic (moving) or static (not moving)
- * \param _intr_dyn Flag indicating if intrinsics is dynamic (moving) or static (not moving)
+ * \param _server parameter server
+ * \param _keys keys of the states of the derived sensor
*
**/
SensorBase(const std::string& _type,
- StateBlockPtr _p_ptr,
- StateBlockPtr _o_ptr,
- StateBlockPtr _intr_ptr,
- const Eigen::VectorXd & _noise_std,
- const bool _p_dyn = false,
- const bool _o_dyn = false,
- const bool _intr_dyn = false);
+ const SizeEigen& _dim,
+ const std::string& _unique_name,
+ const ParamsServer& _server,
+ std::string _keys);
~SensorBase() override;
diff --git a/include/core/state_block/prior.h b/include/core/state_block/prior.h
index c70bf54837a903448d2c599f42b53c073fff7428..3ee8835f0c8309dd9ce56c7136ffad06412b7ac1 100644
--- a/include/core/state_block/prior.h
+++ b/include/core/state_block/prior.h
@@ -27,6 +27,9 @@
namespace wolf
{
+class Prior;
+typedef std::unordered_map<char, Prior> Priors;
+
class Prior
{
private:
@@ -41,7 +44,7 @@ class Prior
Prior() = default;
Prior(const std::string& _prefix, char _key, const ParamsServer& _server)
{
- mode = _server.getParam<double>(prefix + _key + "/mode");
+ mode = _server.getParam<double>(_prefix + _key + "/mode");
if (mode != "nothing" and mode != "initial_guess" and mode != "fix" and mode == "factor")
throw std::runtime_error("wrong mode value, it should be: 'nothing', 'initial_guess', 'fix' or 'factor'");
@@ -50,22 +53,22 @@ class Prior
throw std::runtime_error("For P and O keys, mode 'nothing' is not valid");
if (mode != "nothing")
- state = _server.getParam<Eigen::VectorXd>(prefix + _key + "/state");
+ state = _server.getParam<Eigen::VectorXd>(_prefix + _key + "/state");
else
state = Eigen::VectorXd(0);
if (mode == "factor")
- sigma = _server.getParam<Eigen::VectorXd>(prefix + _key + "/sigma");
+ sigma = _server.getParam<Eigen::VectorXd>(_prefix + _key + "/sigma");
else
sigma = Eigen::VectorXd(0);
if ( key == 'P' or key == 'O')
dynamic = false;
else
- dynamic = _server.getParam<bool>(prefix + _key + "/dynamic");
+ dynamic = _server.getParam<bool>(_prefix + _key + "/dynamic");
if (dynamic)
- sigma_drift = _server.getParam<Eigen::VectorXd>(prefix + _key + "/sigma_drift");
+ sigma_drift = _server.getParam<Eigen::VectorXd>(_prefix + _key + "/sigma_drift");
else
sigma_drift = Eigen::VectorXd(0);
}
@@ -97,6 +100,21 @@ class Prior
return dynamic;
}
+ bool isFixed() const
+ {
+ return mode == "fix";
+ }
+
+ bool isInitialGuess() const
+ {
+ return mode == "initial_guess";
+ }
+
+ bool isFactor() const
+ {
+ return mode == "factor";
+ }
+
const Eigen::VectorXd& getSigma() const
{
return sigma_drift;
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 996d48263c113555ba055422153bf10459bfb564..c195c3fcab2eaadde379655355ea4a59382ac0ee 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -20,80 +20,64 @@
//
//--------LICENSE_END--------
#include "core/sensor/sensor_base.h"
+#include "core/utils/params_server.h"
+#include "core/state_block/prior.h"
#include "core/state_block/state_block.h"
#include "core/state_block/state_quaternion.h"
#include "core/factor/factor_block_absolute.h"
#include "core/factor/factor_quaternion_absolute.h"
-
namespace wolf {
unsigned int SensorBase::sensor_id_count_ = 0;
SensorBase::SensorBase(const std::string& _type,
- StateBlockPtr _p_ptr,
- StateBlockPtr _o_ptr,
- StateBlockPtr _intr_ptr,
- const unsigned int _noise_size,
- const bool _p_dyn,
- const bool _o_dyn,
- const bool _intr_dyn) :
+ const SizeEigen& _dim,
+ const Priors& _priors,
+ ParamsSensorBasePtr _params) :
NodeBase("SENSOR", _type),
HasStateBlocks(""),
hardware_ptr_(),
sensor_id_(++sensor_id_count_), // simple ID factory
state_block_dynamic_(),
params_prior_map_(),
- last_capture_(nullptr),
- noise_std_(_noise_size),
- noise_cov_(_noise_size, _noise_size)
+ last_capture_(nullptr)
{
- assert((_p_ptr or not _p_dyn) and "Trying to set dynamic position state block without providing a position state block. It is required anyway.");
- assert((_o_ptr or not _o_dyn) and "Trying to set dynamic orientation state block without providing an orientation state block. It is required anyway.");
- assert((_intr_ptr or not _intr_dyn) and "Trying to set dynamic intrinsics state block without providing an intrinsics state block. It is required anyway.");
-
- noise_std_.setZero();
- noise_cov_.setZero();
-
- if (_p_ptr)
- addStateBlock('P', _p_ptr, _p_dyn);
+ assert(_dim == 2 or _dim == 3);
- if (_o_ptr)
- addStateBlock('O', _o_ptr, _o_dyn);
-
- if (_intr_ptr)
- addStateBlock('I', _intr_ptr, _intr_dyn);
+ // load params
+ loadParams(_params);
+ // load priors
+ loadPriors(_priors, _dim);
}
SensorBase::SensorBase(const std::string& _type,
- StateBlockPtr _p_ptr,
- StateBlockPtr _o_ptr,
- StateBlockPtr _intr_ptr,
- const Eigen::VectorXd & _noise_std,
- const bool _p_dyn,
- const bool _o_dyn,
- const bool _intr_dyn) :
+ const SizeEigen& _dim,
+ const std::string& _unique_name,
+ const ParamsServer& _server,
+ std::string _keys) :
NodeBase("SENSOR", _type),
HasStateBlocks(""),
hardware_ptr_(),
sensor_id_(++sensor_id_count_), // simple ID factory
state_block_dynamic_(),
params_prior_map_(),
- last_capture_(nullptr),
- noise_std_(_noise_std),
- noise_cov_(_noise_std.size(), _noise_std.size())
+ last_capture_(nullptr)
{
- setNoiseStd(_noise_std);
+ // create params
+ auto params = std::make_shared<ParamsSensorBase>("sensor/" + _unique_name, _server);
- if (_p_ptr)
- addStateBlock('P', _p_ptr, _p_dyn);
+ // load params
+ loadParams(params);
- if (_o_ptr)
- addStateBlock('O', _o_ptr, _o_dyn);
+ // create priors
+ Priors priors;
+ for (auto key : _keys)
+ priors[key] = Prior("sensor/" + _unique_name, key, _server);
- if (_intr_ptr)
- addStateBlock('I', _intr_ptr, _intr_dyn);
+ // load priors
+ loadPriors(priors, _dim);
}
SensorBase::~SensorBase()
@@ -102,6 +86,40 @@ SensorBase::~SensorBase()
removeStateBlocks(getProblem());
}
+void SensorBase::loadParams(ParamsSensorBasePtr _params)
+{
+ setNoiseStd(_params->noise_std);
+}
+
+void SensorBase::loadPriors(Priors _priors, SizeEigen _dim)
+{
+ for (auto&& prior_pair : _priors)
+ {
+ const Prior& prior = prior_pair.second;
+
+ // Checks
+ if (prior.getKey() == 'P' and prior.getState().size() != _dim)
+ throw std::runtime_error("Prior state for P has wrong size");
+ if (prior.getKey() == '0' and prior.getState().size() != (_dim == 2 ? 1 : 4))
+ throw std::runtime_error("Prior state for O has wrong size");
+
+ // create state block
+ StateBlockPtr sb;
+ if (prior.getKey() == 'O' and prior.getState().size() == 4)
+ sb = std::make_shared<StateQuaternion>(prior.getState(), prior.isFixed());
+ else
+ sb = std::make_shared<StateBlock>(prior.getState(), prior.isFixed());
+
+ // Add state block
+ addStateBlock(prior.getKey(), sb, prior.isDynamic());
+
+ // Factor
+ if (prior.isFactor())
+ addPriorParameter(prior.getKey(), prior.getState(), prior.getSigma());
+
+ }
+}
+
void SensorBase::fixExtrinsics()
{
for (auto key : std::string("PO"))