Skip to content
Snippets Groups Projects
Commit c85900ae authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

working aded new constructor to gtest

parent 482dc360
No related branches found
No related tags found
1 merge request!411Draft: Resolve "SensorBase: extrinsic and intrinsic initialization and prior"
Pipeline #6376 failed
......@@ -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;
......
......@@ -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());
}
}
......
......@@ -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);
......
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
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
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment