diff --git a/include/core/state_block/prior.h b/include/core/state_block/prior.h
index 24fa589c8ae108a24f35102bf16f87dd47b0483a..e1bab00d989329004bef89c074b9c95e07be7ced 100644
--- a/include/core/state_block/prior.h
+++ b/include/core/state_block/prior.h
@@ -38,7 +38,7 @@ typedef std::unordered_map<char, Prior> Priors;
class Prior
{
private:
- char key_; // State key
+ std::string type_; // State type
std::string mode_; // Prior mode. Can be 'nothing', 'initial_guess', 'fix' and 'factor'
Eigen::VectorXd state_; // state values (only filled from server if mode != 'nothing')
Eigen::VectorXd sigma_; // factor sigmas (only filled from server if mode == 'factor')
@@ -47,18 +47,18 @@ class Prior
public:
Prior() = default;
- Prior(char _key,
+ Prior(const std::string& _type,
const std::string& _mode,
const Eigen::VectorXd& _state,
const Eigen::VectorXd& _sigma = Eigen::VectorXd(0),
bool _dynamic = false,
const Eigen::VectorXd& _sigma_drift = Eigen::VectorXd(0));
- Prior(const std::string& _prefix, char _key, const ParamsServer& _server);
+ Prior(const std::string& _prefix, const std::string& _type, const ParamsServer& _server);
virtual ~Prior() = default;
- char getKey() const;
+ const std::string& getType() const;
const std::string& getMode() const;
const Eigen::VectorXd& getState() const;
const Eigen::VectorXd& getSigma() const;
@@ -73,10 +73,9 @@ class Prior
virtual std::string print() const final;
};
-
-inline char Prior::getKey() const
+inline const std::string& Prior::getType() const
{
- return key_;
+ return type_;
}
inline const std::string& Prior::getMode() const
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index fc8e5582b03b1290eaee05f95d021e23782af9fb..64860f706a2e9f709230758f26bee39eec093ffc 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -58,7 +58,7 @@ SensorBase::SensorBase(const std::string& _type,
const std::string& _unique_name,
const SizeEigen& _dim,
const ParamsServer& _server,
- std::string _keys) :
+ std::unordered_map<char, std::string> _keys_types) :
NodeBase("SENSOR", _type, _unique_name),
HasStateBlocks(""),
hardware_ptr_(),
@@ -75,8 +75,8 @@ SensorBase::SensorBase(const std::string& _type,
// create priors
Priors priors;
- for (auto key : _keys)
- priors[key] = Prior("sensor/" + _unique_name, key, _server);
+ for (auto pair : _keys_types)
+ priors[pair.first] = Prior("sensor/" + _unique_name, pair.second, _server);
// load priors
loadPriors(priors, _dim);
diff --git a/src/state_block/prior.cpp b/src/state_block/prior.cpp
index 28dd039c0c2d2fa075aa5d5e366442687a119a4b..1d7fdbbdb8b8b29dcdb752e2b2c7d07d61429209 100644
--- a/src/state_block/prior.cpp
+++ b/src/state_block/prior.cpp
@@ -26,13 +26,13 @@
namespace wolf {
-Prior::Prior(char _key,
+Prior::Prior(const std::string& _type,
const std::string& _mode,
const Eigen::VectorXd& _state,
const Eigen::VectorXd& _sigma,
bool _dynamic,
const Eigen::VectorXd& _sigma_drift) :
- key_(_key),
+ type_(_type),
mode_(_mode),
state_(_state),
sigma_(_sigma),
@@ -42,24 +42,25 @@ Prior::Prior(char _key,
check();
}
-Prior::Prior(const std::string& _prefix, char _key, const ParamsServer& _server)
+Prior::Prior(const std::string& _prefix, const ParamsServer& _server, const std::string& _type)
{
- mode_ = _server.getParam<double>(_prefix + _key + "/mode");
+ type_ = _type;
+ mode_ = _server.getParam<double>(_prefix + "/mode");
if (mode_ != "nothing")
- state_ = _server.getParam<Eigen::VectorXd>(_prefix + _key + "/state");
+ state_ = _server.getParam<Eigen::VectorXd>(_prefix + "/state");
else
state_ = Eigen::VectorXd(0);
if (mode_ == "factor")
- sigma_ = _server.getParam<Eigen::VectorXd>(_prefix + _key + "/sigma");
+ sigma_ = _server.getParam<Eigen::VectorXd>(_prefix + "/sigma");
else
sigma_ = Eigen::VectorXd(0);
- dynamic_ = _server.getParam<bool>(_prefix + _key + "/dynamic");
+ dynamic_ = _server.getParam<bool>(_prefix + "/dynamic");
if (dynamic_)
- sigma_drift_ = _server.getParam<Eigen::VectorXd>(_prefix + _key + "/sigma_drift");
+ sigma_drift_ = _server.getParam<Eigen::VectorXd>(_prefix + "/sigma_drift");
else
sigma_drift_ = Eigen::VectorXd(0);
@@ -71,35 +72,35 @@ void Prior::check() const
if (mode_ != "nothing" and mode_ != "initial_guess" and mode_ != "fix" and mode_ != "factor")
throw std::runtime_error("wrong mode value: " + mode_ + ", it should be: 'nothing', 'initial_guess', 'fix' or 'factor'");
- if ( mode_ == "nothing" and (key_ == 'P' or key_ == 'O'))
+ if ( mode_ == "nothing" and (type_ == "P" or type_ == "O"))
throw std::runtime_error("For P and O keys, mode 'nothing' is not valid");
- if (dynamic_ and (key_ == 'P' or key_ == 'O') )
+ if (dynamic_ and (type_ == "P" or type_ == "O") )
throw std::runtime_error("Dynamic state blocks not implemented for extrinsics");
// try to create a state block and check for local parameterization and dimensions consistency
- auto sb = FactoryStateBlock::create(std::string(1, key_), state_, mode_ == "fix");
+ auto sb = FactoryStateBlock::create(type_, state_, mode_ == "fix");
// check local param
if (not sb->isValid())
- throw std::runtime_error("Prior " + std::string(1, key_) + " state is not valid (local param violation)");
+ throw std::runtime_error("Prior " + type_ + " state is not valid (local param violation)");
// check state size
if (mode_ != "nothing" and state_.size() != sb->getSize())
- throw std::runtime_error("Prior " + std::string(1, key_) + " state size different of state size");
+ throw std::runtime_error("Prior " + type_ + " state size different of state size");
// check sigma size
if (mode_ == "factor" and sigma_.size() != sb->getLocalSize())
- throw std::runtime_error("Prior " + std::string(1, key_) + " sigma size different of state local size");
+ throw std::runtime_error("Prior " + type_ + " sigma size different of state local size");
// check sigma size
if (dynamic_ and sigma_drift_.size() != sb->getLocalSize())
- throw std::runtime_error("Prior " + std::string(1, key_) + " sigma_drift size different of state local size");
+ throw std::runtime_error("Prior " + type_ + " sigma_drift size different of state local size");
}
std::string Prior::print() const
{
- return "Prior " + std::string(1,key_) + "\n"
+ return "Prior " + type_ + "\n"
+ "mode: " + mode_ + "\n"
//+ "state: " + to_string(state_) + "\n"
//+ (mode_ == "factor" ? "sigma: " + to_string(sigma_) + "\n" : "")
diff --git a/test/gtest_prior.cpp b/test/gtest_prior.cpp
index c73b78d198a6b6a95e15b03e965a7ceb5a1e065f..974600d199321f38b815a3b0c0c1ca206bd8141f 100644
--- a/test/gtest_prior.cpp
+++ b/test/gtest_prior.cpp
@@ -230,6 +230,9 @@ TEST(Prior, K)
testPriors(setups_death, false);
}
+// TODO: param_server constructor!
+
+
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
diff --git a/test/gtest_sensor_base.cpp b/test/gtest_sensor_base.cpp
index ff1b4f5e3373208f034b6915ed239b98f42209f0..5f2e785f147ed275ce48c73ac6d922d028fd2f73 100644
--- a/test/gtest_sensor_base.cpp
+++ b/test/gtest_sensor_base.cpp
@@ -39,6 +39,7 @@ TEST(SensorBase, POfix)
auto params = std::make_shared<ParamsSensorBase>();
params->noise_std = Eigen::Vector2d::Constant(0.1);
+
auto S = std::make_shared<SensorBase>("SensorBase",
"sensor1",
2,