diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index ec85b16148970e54351b3d56137636101f900072..faf359f727df4058543204a9a2e29d661e4ca96d 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -128,7 +128,7 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
CaptureBasePtr last_capture_; // last capture of the sensor (in the WOLF tree)
protected:
- Eigen::VectorXd noise_std_; // std of sensor noise
+ //Eigen::VectorXd noise_std_; // std of sensor noise
Eigen::MatrixXd noise_cov_; // cov matrix of noise
void setProblem(ProblemPtr _problem) override final;
@@ -146,6 +146,7 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
*
**/
SensorBase(const std::string& _type,
+ const std::string& _unique_name,
const SizeEigen& _dim,
const Priors& _priors,
ParamsSensorBasePtr _params);
@@ -159,8 +160,8 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
*
**/
SensorBase(const std::string& _type,
- const SizeEigen& _dim,
const std::string& _unique_name,
+ const SizeEigen& _dim,
const ParamsServer& _server,
std::string _keys);
diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h
index ac82297f5e36b5130151bd1b0464a110264b5fe7..7483ef45e2d3cd122ff094d072c9a359a34d8f8c 100644
--- a/include/core/state_block/has_state_blocks.h
+++ b/include/core/state_block/has_state_blocks.h
@@ -76,13 +76,8 @@ class HasStateBlocks
std::unordered_map<char, StateBlockConstPtr>::const_iterator find(const StateBlockConstPtr& _sb) const;
std::unordered_map<char, StateBlockPtr>::const_iterator find(const StateBlockPtr& _sb);
- // Emplace derived state blocks (angle, quaternion, etc).
- template<typename SB, typename ... Args>
- std::shared_ptr<SB> emplaceStateBlock(const char& _sb_type, ProblemPtr _problem, Args&&... _args_of_derived_state_block_constructor);
-
// Emplace base state blocks.
- template<typename ... Args>
- StateBlockPtr emplaceStateBlock(const char& _sb_type, ProblemPtr _problem, Args&&... _args_of_base_state_block_constructor);
+ StateBlockPtr emplaceStateBlock(const char& _sb_type, ProblemPtr _problem, const Eigen::VectorXd& _state, bool _fixed);
// Register/remove state blocks to/from wolf::Problem
virtual void registerNewStateBlocks(ProblemPtr _problem);
@@ -171,28 +166,6 @@ inline unsigned int HasStateBlocks::removeStateBlock(const char& _sb_type)
return state_block_const_map_.erase(_sb_type);
}
-template<typename SB, typename ... Args>
-inline std::shared_ptr<SB> HasStateBlocks::emplaceStateBlock(const char& _sb_type, ProblemPtr _problem, Args&&... _args_of_derived_state_block_constructor)
-{
- assert(state_block_map_.count(_sb_type) == 0 && state_block_const_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
- std::shared_ptr<SB> sb = std::make_shared<SB>(std::forward<Args>(_args_of_derived_state_block_constructor)...);
-
- addStateBlock(_sb_type, sb, _problem);
-
- return sb;
-}
-
-template<typename ... Args>
-inline StateBlockPtr HasStateBlocks::emplaceStateBlock(const char& _sb_type, ProblemPtr _problem, Args&&... _args_of_base_state_block_constructor)
-{
- assert(state_block_map_.count(_sb_type) == 0 && state_block_const_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
- auto sb = std::make_shared<StateBlock>(std::forward<Args>(_args_of_base_state_block_constructor)...);
-
- addStateBlock(_sb_type, sb, _problem);
-
- return sb;
-}
-
inline bool HasStateBlocks::setStateBlock(const char _sb_type, const StateBlockPtr& _sb)
{
assert (structure_.find(_sb_type) > 0 && "Cannot set a state block out of the state structure! Use addStateBlock instead.");
diff --git a/include/core/state_block/prior.h b/include/core/state_block/prior.h
index 3ee8835f0c8309dd9ce56c7136ffad06412b7ac1..b246ed47a1f99187ee27615269bbc7f681cadb97 100644
--- a/include/core/state_block/prior.h
+++ b/include/core/state_block/prior.h
@@ -42,36 +42,14 @@ class Prior
public:
Prior() = default;
- Prior(const std::string& _prefix, char _key, const ParamsServer& _server)
- {
- 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'");
-
- if ( mode == "nothing" and (key == 'P' or key == 'O'))
- throw std::runtime_error("For P and O keys, mode 'nothing' is not valid");
+ Prior(char _key,
+ const std::string& _mode,
+ const Eigen::VectorXd& _state,
+ const Eigen::VectorXd& _sigma,
+ bool _dynamic,
+ const Eigen::VectorXd& _sigma_drift);
- if (mode != "nothing")
- state = _server.getParam<Eigen::VectorXd>(_prefix + _key + "/state");
- else
- state = Eigen::VectorXd(0);
-
- if (mode == "factor")
- 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");
-
- if (dynamic)
- sigma_drift = _server.getParam<Eigen::VectorXd>(_prefix + _key + "/sigma_drift");
- else
- sigma_drift = Eigen::VectorXd(0);
- }
+ Prior(const std::string& _prefix, char _key, const ParamsServer& _server);
virtual ~Prior() = default;
@@ -120,9 +98,21 @@ class Prior
return sigma_drift;
}
+ void check() const
+ {
+ 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'");
+
+ if ( mode == "nothing" and (key == 'P' or key == 'O'))
+ throw std::runtime_error("For P and O keys, mode 'nothing' is not valid");
+
+ if (dynamic and (key == 'P' or key == 'O') )
+ throw std::runtime_error("Dynamic state blocks not implemented for extrinsics");
+ }
+
virtual std::string print() const final
{
- return "Prior " + _key + "\n"
+ return "Prior " + key + "\n"
+ "mode: " + std::to_string(mode) + "\n"
+ "state: " + std::to_string(state) + "\n"
+ (mode == "factor" ? "sigma: " + std::to_string(sigma) + "\n" : "")
@@ -131,5 +121,45 @@ class Prior
}
};
+inline Prior::Prior(char _key,
+ const std::string& _mode,
+ const Eigen::VectorXd& _state,
+ const Eigen::VectorXd& _sigma,
+ bool _dynamic,
+ const Eigen::VectorXd& _sigma_drift) :
+ key(_key),
+ mode(_mode),
+ state(_state),
+ sigma(_sigma),
+ dynamic(_dynamic),
+ sigma_drift(_sigma_drift)
+{
+ check();
+}
+
+inline Prior::Prior(const std::string& _prefix, char _key, const ParamsServer& _server)
+{
+ mode = _server.getParam<double>(_prefix + _key + "/mode");
+
+ if (mode != "nothing")
+ state = _server.getParam<Eigen::VectorXd>(_prefix + _key + "/state");
+ else
+ state = Eigen::VectorXd(0);
+
+ if (mode == "factor")
+ sigma = _server.getParam<Eigen::VectorXd>(_prefix + _key + "/sigma");
+ else
+ sigma = Eigen::VectorXd(0);
+
+ dynamic = _server.getParam<bool>(_prefix + _key + "/dynamic");
+
+ if (dynamic)
+ sigma_drift = _server.getParam<Eigen::VectorXd>(_prefix + _key + "/sigma_drift");
+ else
+ sigma_drift = Eigen::VectorXd(0);
+
+ check();
+}
+
}
#endif
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index c195c3fcab2eaadde379655355ea4a59382ac0ee..954af1f0de9042172c4e46802004adb7ef6bca8e 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -22,6 +22,7 @@
#include "core/sensor/sensor_base.h"
#include "core/utils/params_server.h"
#include "core/state_block/prior.h"
+#include "core/state_block/factory_state_block.h"
#include "core/state_block/state_block.h"
#include "core/state_block/state_quaternion.h"
#include "core/factor/factor_block_absolute.h"
@@ -32,10 +33,11 @@ namespace wolf {
unsigned int SensorBase::sensor_id_count_ = 0;
SensorBase::SensorBase(const std::string& _type,
+ const std::string& _unique_name,
const SizeEigen& _dim,
const Priors& _priors,
ParamsSensorBasePtr _params) :
- NodeBase("SENSOR", _type),
+ NodeBase("SENSOR", _type, _unique_name),
HasStateBlocks(""),
hardware_ptr_(),
sensor_id_(++sensor_id_count_), // simple ID factory
@@ -53,11 +55,11 @@ SensorBase::SensorBase(const std::string& _type,
}
SensorBase::SensorBase(const std::string& _type,
- const SizeEigen& _dim,
const std::string& _unique_name,
+ const SizeEigen& _dim,
const ParamsServer& _server,
std::string _keys) :
- NodeBase("SENSOR", _type),
+ NodeBase("SENSOR", _type, _unique_name),
HasStateBlocks(""),
hardware_ptr_(),
sensor_id_(++sensor_id_count_), // simple ID factory
@@ -93,6 +95,8 @@ void SensorBase::loadParams(ParamsSensorBasePtr _params)
void SensorBase::loadPriors(Priors _priors, SizeEigen _dim)
{
+ assert(_dim == 2 or _dim == 3);
+
for (auto&& prior_pair : _priors)
{
const Prior& prior = prior_pair.second;
@@ -104,11 +108,7 @@ void SensorBase::loadPriors(Priors _priors, SizeEigen _dim)
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());
+ auto sb = FactoryStateBlock::create(std::string(1, prior.getKey()), prior.getState(), prior.isFixed());
// Add state block
addStateBlock(prior.getKey(), sb, prior.isDynamic());
@@ -116,7 +116,6 @@ void SensorBase::loadPriors(Priors _priors, SizeEigen _dim)
// Factor
if (prior.isFactor())
addPriorParameter(prior.getKey(), prior.getState(), prior.getSigma());
-
}
}
diff --git a/src/state_block/has_state_blocks.cpp b/src/state_block/has_state_blocks.cpp
index 047469dc899a8973b7bdf07df318b2079c155ef9..181188658a4776bf657ba7acb6c86f88775c92e4 100644
--- a/src/state_block/has_state_blocks.cpp
+++ b/src/state_block/has_state_blocks.cpp
@@ -21,10 +21,25 @@
//--------LICENSE_END--------
#include "core/state_block/has_state_blocks.h"
+#include "core/state_block/factory_state_block.h"
namespace wolf
{
+StateBlockPtr HasStateBlocks::emplaceStateBlock(const char& _sb_type,
+ ProblemPtr _problem,
+ const Eigen::VectorXd& _state,
+ bool _fixed)
+{
+ assert(state_block_map_.count(_sb_type) == 0 && state_block_const_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
+
+ auto sb = FactoryStateBlock::create(std::string(1,_sb_type), _state, _fixed);
+
+ addStateBlock(_sb_type, sb, _problem);
+
+ return sb;
+}
+
StateBlockPtr HasStateBlocks::addStateBlock(const char& _sb_type, const StateBlockPtr& _sb, ProblemPtr _problem)
{
assert(state_block_map_.count(_sb_type) == 0 && state_block_const_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
diff --git a/src/state_block/state_block.cpp b/src/state_block/state_block.cpp
index 513d6d4627627a029e3ebe61bab550497d369634..f3ade24eb4156d07ae445895dfe8c5af90419e6e 100644
--- a/src/state_block/state_block.cpp
+++ b/src/state_block/state_block.cpp
@@ -87,7 +87,7 @@ StateBlockPtr create_orientation(const Eigen::VectorXd& _state, bool _fixed)
if (_state.size() == 4)
return StateQuaternion::create(_state, _fixed);
- throw std::length_error("Wrong vector size for orientation. Must be 4 for a quaternion in 3D, or 1 for an angle in 2D.");
+ throw std::runtime_error("Wrong vector size for orientation. Must be 4 for a quaternion in 3D, or 1 for an angle in 2D.");
return nullptr;
}