diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index e7bac59f83bbe1e36fbb2ff4ee60a885c5d2fcac..1a9ca45aa5cf2334f45b4b4d894f25692b93af03 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -114,6 +114,7 @@ struct ParamsSensorBase: public ParamsBase
if (state_blocks['P'].prior_mode == "factor")
state_blocks['P'].sigma = _server.getParam<Eigen::Vector3d> (prefix + _unique_name + "/P/sigma");
}
+ assert((state_blocks['P'].prior_mode == "initial_guess" || state_blocks['P'].prior_mode == "fix" || state_blocks['P'].prior_mode == "factor") && "wrong prior_mode value, it should be: 'initial_guess', 'fix' or 'factor'");
// O
state_blocks.emplace('O',ParamsStateBlock());
@@ -133,14 +134,23 @@ struct ParamsSensorBase: public ParamsBase
if (state_blocks['O'].prior_mode == "factor")
state_blocks['O'].sigma = _server.getParam<Eigen::Vector3d> (prefix + _unique_name + "/O/sigma");
}
+ assert((state_blocks['O'].prior_mode == "initial_guess" || state_blocks['O'].prior_mode == "fix" || state_blocks['O'].prior_mode == "factor") && "wrong prior_mode value, it should be: 'initial_guess', 'fix' or 'factor'");
// 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");
+ try
+ {
+ 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");
+ assert((state_blocks['I'].prior_mode == "initial_guess" || state_blocks['I'].prior_mode == "fix" || state_blocks['I'].prior_mode == "factor") && "wrong prior_mode value, it should be: 'initial_guess', 'fix' or 'factor'");
+ }
+ catch(...)
+ {
+ WOLF_INFO("ParamsSensorBase: Prior for the intrinsics not provided for sensor ", _unique_name);
+ }
}
~ParamsSensorBase() override = default;
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index a9f0ecf1de0ef120d896189325b5456092433a7b..1dcf9debf7c39fc5389448309362308aca6630c6 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -27,6 +27,9 @@ SensorBase::SensorBase(const std::string& _type,
if (key == 'I' and _intrinsics_size == 0)
continue;
+ assert((key != 'I' or sb_param.state.size() == _intrinsics_size) && "wrong state size for intrinsics");
+ assert((key != 'I' or sb_param.sigma.size() == _intrinsics_size) && "wrong sigma size for intrinsics");
+
addStateBlock(key,
(key != 'O' ?
std::make_shared<StateBlock>(sb_param.state,sb_param.prior_mode == "fix") :
@@ -35,10 +38,10 @@ SensorBase::SensorBase(const std::string& _type,
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());
+ if (sb_param.prior_mode == "factor")
+ addPriorParameter(key,
+ sb_param.state,
+ sb_param.sigma.asDiagonal());
}
}
diff --git a/test/gtest_sensor_base.cpp b/test/gtest_sensor_base.cpp
index 3472a400fcbd571f6c7ab1232bbe09e1319d0e13..9f01cdf41b37d614ae9ea2a8b4efe2365a9289b3 100644
--- a/test/gtest_sensor_base.cpp
+++ b/test/gtest_sensor_base.cpp
@@ -5,11 +5,14 @@
* \author: jsola
*/
-#include "core/sensor/sensor_base.h"
-
#include "core/utils/utils_gtest.h"
+#include "core/sensor/sensor_base.h"
+#include "core/yaml/parser_yaml.h"
+#include "core/utils/params_server.h"
+
using namespace wolf;
+std::string wolf_root = _WOLF_ROOT_DIR;
TEST(SensorBase, setNoiseStd)
{
@@ -24,6 +27,52 @@ TEST(SensorBase, setNoiseStd)
ASSERT_MATRIX_APPROX(noise_cov, S->getNoiseCov(), 1e-8);
}
+TEST(SensorBase, constructorParams3D)
+{
+ ParserYaml parser = ParserYaml("test/yaml/sensor_base_3d.yaml", wolf_root);
+ ParamsServer server = ParamsServer(parser.getParams());
+
+ ParamsSensorBasePtr params = std::make_shared<ParamsSensorBase>("SensorBase", server);
+
+ SensorBasePtr S = std::make_shared<SensorBase>("SensorBase", params, 2); // intrinsics size 2
+
+ ASSERT_MATRIX_APPROX(S->getP()->getState(), (Eigen::Vector3d() << 1, 2, 3).finished(), 1e-8);
+ ASSERT_MATRIX_APPROX(S->getO()->getState(), (Eigen::Vector4d() << 0, 0, 0, 1).finished(), 1e-8);
+ ASSERT_MATRIX_APPROX(S->getIntrinsic()->getState(), (Eigen::Vector2d() << 4, 5).finished(), 1e-8);
+
+ ASSERT_FALSE(S->getP()->isFixed());
+ ASSERT_TRUE(S->getO()->isFixed());
+ ASSERT_FALSE(S->getIntrinsic()->isFixed());
+
+ ASSERT_FALSE(S->isStateBlockDynamic('P'));
+ ASSERT_TRUE(S->isStateBlockDynamic('O'));
+ ASSERT_FALSE(S->isStateBlockDynamic('I'));
+
+ ASSERT_DEATH(std::make_shared<SensorBase>("SensorBase", params, 3);,"");
+}
+
+TEST(SensorBase, constructorParams2D)
+{
+ ParserYaml parser = ParserYaml("test/yaml/sensor_base_2d.yaml", wolf_root);
+ ParamsServer server = ParamsServer(parser.getParams());
+
+ ParamsSensorBasePtr params = std::make_shared<ParamsSensorBase>("SensorBase", server);
+
+ SensorBasePtr S = std::make_shared<SensorBase>("SensorBase", params, 0); // intrinsics size 2
+
+ ASSERT_MATRIX_APPROX(S->getP()->getState(), (Eigen::Vector2d() << 1, 2).finished(), 1e-8);
+ ASSERT_MATRIX_APPROX(S->getO()->getState(), (Eigen::Vector1d() << 3).finished(), 1e-8);
+
+ ASSERT_FALSE(S->getP()->isFixed());
+ ASSERT_TRUE(S->getO()->isFixed());
+ ASSERT_FALSE(S->getIntrinsic());
+
+ ASSERT_FALSE(S->isStateBlockDynamic('P'));
+ ASSERT_TRUE(S->isStateBlockDynamic('O'));
+
+ ASSERT_DEATH(std::make_shared<SensorBase>("SensorBase", params, 3);,"");
+}
+
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
diff --git a/test/yaml/sensor_base_2d.yaml b/test/yaml/sensor_base_2d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..00a9849acaaa04490de06421535d6556bdfd501e
--- /dev/null
+++ b/test/yaml/sensor_base_2d.yaml
@@ -0,0 +1,15 @@
+config:
+ problem:
+ dimension: 2
+
+ sensor:
+ SensorBase:
+ P:
+ state: [1, 2]
+ prior_mode: "initial_guess"
+ is_dynamic: false
+ O:
+ state: [3]
+ prior_mode: "fix"
+ is_dynamic: true
+
\ No newline at end of file
diff --git a/test/yaml/sensor_base_3d.yaml b/test/yaml/sensor_base_3d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..1cad11e527e7adb2785fbd350367bd6e06fdee26
--- /dev/null
+++ b/test/yaml/sensor_base_3d.yaml
@@ -0,0 +1,20 @@
+config:
+ problem:
+ dimension: 3
+
+ sensor:
+ SensorBase:
+ P:
+ state: [1, 2, 3]
+ prior_mode: "initial_guess"
+ is_dynamic: false
+ O:
+ state: [0, 0, 0, 1]
+ prior_mode: "fix"
+ is_dynamic: true
+ I:
+ state: [4, 5]
+ prior_mode: "factor"
+ is_dynamic: false
+ sigma: [6, 7]
+
\ No newline at end of file