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

[skip ci] wip

parent 99829e8b
No related branches found
No related tags found
1 merge request!448Draft: Resolve "Implementation of new nodes creation"
Pipeline #11217 skipped
Hide whitespace changes
Inline Side-by-side
test/gtest_sensor_base.cpp
+ 80
− 65
View file @ 1adfeae8
......@@ -50,8 +50,12 @@ MatrixXd p_cov_2D = p_std_2D.cwiseAbs2().asDiagonal();
MatrixXd p_cov_3D = p_std_3D.cwiseAbs2().asDiagonal();
MatrixXd o_cov_2D = o_std_2D.cwiseAbs2().asDiagonal();
MatrixXd o_cov_3D = o_std_3D.cwiseAbs2().asDiagonal();
VectorXd noise_std = Vector2d::Constant(0.1);
MatrixXd noise_cov = noise_std.cwiseAbs2().asDiagonal();
double noise_p_std = 0.1;
double noise_o_std = 0.01;
VectorXd noise_std_2D = (Vector3d() << noise_p_std, noise_p_std, noise_o_std).finished();
VectorXd noise_std_3D = (Vector3d() << noise_p_std, noise_p_std,noise_p_std, noise_o_std, noise_o_std, noise_o_std).finished();
MatrixXd noise_cov_2D = noise_std_2D.cwiseAbs2().asDiagonal();
MatrixXd noise_cov_3D = noise_std_3D.cwiseAbs2().asDiagonal();
void checkSensor(SensorBasePtr S,
char _key,
......@@ -101,16 +105,17 @@ void checkSensor(SensorBasePtr S,
// 2D Fix
TEST(SensorBase, POfix2D)
{
auto params = std::make_shared<ParamsSensorBase>();
params->noise_std = noise_std;
auto params = std::make_shared<ParamsSensorDummy>();
params->noise_p_std = noise_p_std;
params->noise_o_std = noise_o_std;
auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 2, params,
auto S = std::make_shared<SensorDummy>("sensor1", 2, params,
Priors({{'P',Prior("P", p_state_2D)}, //default "fix", not dynamic
{'O',Prior("O", o_state_2D)}}));
// noise
ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS);
ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov, Constants::EPS);
ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std_2D, Constants::EPS);
ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov_2D, Constants::EPS);
// checks
checkSensor(S, 'P', p_state_2D, true, vector0, false, vector0);
......@@ -120,16 +125,17 @@ TEST(SensorBase, POfix2D)
// 3D Fix
TEST(SensorBase, POfix3D)
{
auto params = std::make_shared<ParamsSensorBase>();
params->noise_std = noise_std;
auto params = std::make_shared<ParamsSensorDummy>();
params->noise_p_std = noise_p_std;
params->noise_o_std = noise_o_std;
auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 3, params,
auto S = std::make_shared<SensorDummy>("sensor1", 3, params,
Priors({{'P',Prior("P", p_state_3D)}, //default "fix", not dynamic
{'O',Prior("O", o_state_3D)}}));
// noise
ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS);
ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov, Constants::EPS);
ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std_3D, Constants::EPS);
ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov_3D, Constants::EPS);
// factors
ASSERT_EQ(S->getPriorFeatures().size(), 0);
......@@ -142,16 +148,17 @@ TEST(SensorBase, POfix3D)
// 2D Initial guess
TEST(SensorBase, POinitial_guess2D)
{
auto params = std::make_shared<ParamsSensorBase>();
params->noise_std = noise_std;
auto params = std::make_shared<ParamsSensorDummy>();
params->noise_p_std = noise_p_std;
params->noise_o_std = noise_o_std;
auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 2, params,
auto S = std::make_shared<SensorDummy>("sensor1", 2, params,
Priors({{'P',Prior("P", p_state_2D, "initial_guess")},
{'O',Prior("O", o_state_2D, "initial_guess")}}));
// noise
ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS);
ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov, Constants::EPS);
ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std_2D, Constants::EPS);
ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov_2D, Constants::EPS);
// factors
ASSERT_EQ(S->getPriorFeatures().size(), 0);
......@@ -164,16 +171,17 @@ TEST(SensorBase, POinitial_guess2D)
// 3D Initial guess
TEST(SensorBase, POinitial_guess3D)
{
auto params = std::make_shared<ParamsSensorBase>();
params->noise_std = noise_std;
auto params = std::make_shared<ParamsSensorDummy>();
params->noise_p_std = noise_p_std;
params->noise_o_std = noise_o_std;
auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 3, params,
auto S = std::make_shared<SensorDummy>("sensor1", 3, params,
Priors({{'P',Prior("P", p_state_3D, "initial_guess")},
{'O',Prior("O", o_state_3D, "initial_guess")}}));
// noise
ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS);
ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov, Constants::EPS);
ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std_3D, Constants::EPS);
ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov_3D, Constants::EPS);
// factors
ASSERT_EQ(S->getPriorFeatures().size(), 0);
......@@ -186,16 +194,17 @@ TEST(SensorBase, POinitial_guess3D)
// 2D Factor
TEST(SensorBase, POfactor2D)
{
auto params = std::make_shared<ParamsSensorBase>();
params->noise_std = noise_std;
auto params = std::make_shared<ParamsSensorDummy>();
params->noise_p_std = noise_p_std;
params->noise_o_std = noise_o_std;
auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 2, params,
auto S = std::make_shared<SensorDummy>("sensor1", 2, params,
Priors({{'P',Prior("P", p_state_2D, "factor", p_std_2D)},
{'O',Prior("O", o_state_2D, "factor", o_std_2D)}}));
// noise
ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS);
ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov, Constants::EPS);
ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std_2D, Constants::EPS);
ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov_2D, Constants::EPS);
// factors
ASSERT_EQ(S->getPriorFeatures().size(), 2);
......@@ -208,16 +217,17 @@ TEST(SensorBase, POfactor2D)
// 3D Factor
TEST(SensorBase, POfactor3D)
{
auto params = std::make_shared<ParamsSensorBase>();
params->noise_std = noise_std;
auto params = std::make_shared<ParamsSensorDummy>();
params->noise_p_std = noise_p_std;
params->noise_o_std = noise_o_std;
auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 3, params,
auto S = std::make_shared<SensorDummy>("sensor1", 3, params,
Priors({{'P',Prior("P", p_state_3D, "factor", p_std_3D)},
{'O',Prior("O", o_state_3D, "factor", o_std_3D)}}));
// noise
ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS);
ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov, Constants::EPS);
ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std_3D, Constants::EPS);
ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov_3D, Constants::EPS);
// factors
ASSERT_EQ(S->getPriorFeatures().size(), 2);
......@@ -230,16 +240,17 @@ TEST(SensorBase, POfactor3D)
// 2D Initial guess dynamic
TEST(SensorBase, POinitial_guess_dynamic2D)
{
auto params = std::make_shared<ParamsSensorBase>();
params->noise_std = noise_std;
auto params = std::make_shared<ParamsSensorDummy>();
params->noise_p_std = noise_p_std;
params->noise_o_std = noise_o_std;
auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 2, params,
auto S = std::make_shared<SensorDummy>("sensor1", 2, params,
Priors({{'P',Prior("P", p_state_2D, "initial_guess", vector0, true)},
{'O',Prior("O", o_state_2D, "initial_guess", vector0, true)}}));
// noise
ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS);
ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov, Constants::EPS);
ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std_2D, Constants::EPS);
ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov_2D, Constants::EPS);
// factors
ASSERT_EQ(S->getPriorFeatures().size(), 0);
......@@ -252,16 +263,17 @@ TEST(SensorBase, POinitial_guess_dynamic2D)
// 3D Initial guess dynamic
TEST(SensorBase, POinitial_guess_dynamic3D)
{
auto params = std::make_shared<ParamsSensorBase>();
params->noise_std = noise_std;
auto params = std::make_shared<ParamsSensorDummy>();
params->noise_p_std = noise_p_std;
params->noise_o_std = noise_o_std;
auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 3, params,
auto S = std::make_shared<SensorDummy>("sensor1", 3, params,
Priors({{'P',Prior("P", p_state_3D, "initial_guess", vector0, true)},
{'O',Prior("O", o_state_3D, "initial_guess", vector0, true)}}));
// noise
ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS);
ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov, Constants::EPS);
ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std_3D, Constants::EPS);
ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov_3D, Constants::EPS);
// factors
ASSERT_EQ(S->getPriorFeatures().size(), 0);
......@@ -274,16 +286,17 @@ TEST(SensorBase, POinitial_guess_dynamic3D)
// 2D Initial guess dynamic drift
TEST(SensorBase, POinitial_guess_dynamic2D_drift)
{
auto params = std::make_shared<ParamsSensorBase>();
params->noise_std = noise_std;
auto params = std::make_shared<ParamsSensorDummy>();
params->noise_p_std = noise_p_std;
params->noise_o_std = noise_o_std;
auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 2, params,
auto S = std::make_shared<SensorDummy>("sensor1", 2, params,
Priors({{'P',Prior("P", p_state_2D, "initial_guess", vector0, true, p_std_2D)},
{'O',Prior("O", o_state_2D, "initial_guess", vector0, true, o_std_2D)}}));
// noise
ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS);
ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov, Constants::EPS);
ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std_2D, Constants::EPS);
ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov_2D, Constants::EPS);
// factors
ASSERT_EQ(S->getPriorFeatures().size(), 0);
......@@ -296,16 +309,17 @@ TEST(SensorBase, POinitial_guess_dynamic2D_drift)
// 3D Initial guess dynamic drift
TEST(SensorBase, POinitial_guess_dynamic3D_drift)
{
auto params = std::make_shared<ParamsSensorBase>();
params->noise_std = noise_std;
auto params = std::make_shared<ParamsSensorDummy>();
params->noise_p_std = noise_p_std;
params->noise_o_std = noise_o_std;
auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 3, params,
auto S = std::make_shared<SensorDummy>("sensor1", 3, params,
Priors({{'P',Prior("P", p_state_3D, "initial_guess", vector0, true, p_std_3D)},
{'O',Prior("O", o_state_3D, "initial_guess", vector0, true, o_std_3D)}}));
// noise
ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS);
ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov, Constants::EPS);
ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std_3D, Constants::EPS);
ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov_3D, Constants::EPS);
// factors
ASSERT_EQ(S->getPriorFeatures().size(), 0);
......@@ -318,20 +332,21 @@ TEST(SensorBase, POinitial_guess_dynamic3D_drift)
// 3D POI mixed
TEST(SensorBase, POI_mixed)
{
auto params = std::make_shared<ParamsSensorBase>();
params->noise_std = noise_std;
auto params = std::make_shared<ParamsSensorDummy>();
params->noise_p_std = noise_p_std;
params->noise_o_std = noise_o_std;
VectorXd i_state_3D = VectorXd::Random(5);
auto S = std::make_shared<SensorBase>("SensorBase", "sensor1", 3, params,
auto S = std::make_shared<SensorDummy>("sensor1", 3, params,
Priors({{'P',Prior("P", p_state_3D, "fix", vector0, true)},
{'O',Prior("O", o_state_3D, "factor", o_std_3D, true, o_std_3D)},
{'I',Prior("StateBlock", i_state_3D, "initial_guess")},
{'A',Prior("StateQuaternion", o_state_3D, "factor", o_std_3D)}}));
// noise
ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS);
ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov, Constants::EPS);
ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std_3D, Constants::EPS);
ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov_3D, Constants::EPS);
// factors
ASSERT_EQ(S->getPriorFeatures().size(), 2);
......@@ -376,8 +391,8 @@ TEST(SensorBase, server_PO)
// CORRECT YAML
WOLF_INFO("Creating sensor from name ", name);
auto params = std::make_shared<ParamsSensorBase>(name, server);
auto S = std::make_shared<SensorBase>("SensorBase", name, dim, params, server);
auto params = std::make_shared<ParamsSensorDummy>(name, server);
auto S = std::make_shared<SensorDummy>( name, dim, params, server);
auto p_size = dim;
auto o_size = dim == 2 ? 1 : 4;
......@@ -400,8 +415,8 @@ TEST(SensorBase, server_PO)
// INCORRECT YAML
WOLF_INFO("Creating sensor from name ", name + "_wrong");
ASSERT_THROW(std::make_shared<SensorBase>("SensorBase", name + "_wrong", dim,
std::make_shared<ParamsSensorBase>(name + "_wrong", server),
ASSERT_THROW(std::make_shared<SensorDummy>( name + "_wrong", dim,
std::make_shared<ParamsSensorDummy>(name + "_wrong", server),
server),std::runtime_error);
}
}
......@@ -421,8 +436,8 @@ TEST(SensorBase, server_POIA)
// POIA - 3D - CORRECT YAML
WOLF_INFO("Creating sensor from name sensor_POIA_3D");
auto params = std::make_shared<ParamsSensorBase>("sensor_POIA_3D", server);
auto S = std::make_shared<SensorBase>("SensorBase", "sensor_POIA_3D", 3, params, server,
auto params = std::make_shared<ParamsSensorDummy>("sensor_POIA_3D", server);
auto S = std::make_shared<SensorDummy>("sensor_POIA_3D", 3, params, server,
std::unordered_map<char, std::string>({{'I',"StateBlock"},
{'A',"StateQuaternion"}}));
......@@ -449,8 +464,8 @@ TEST(SensorBase, server_POIA_wrong)
// POIA - 3D - INCORRECT YAML
WOLF_INFO("Creating sensor from name sensor_POIA_3D_wrong");
ASSERT_THROW(std::make_shared<SensorBase>("SensorBase", "sensor_POIA_3D_wrong", 3,
std::make_shared<ParamsSensorBase>("sensor_POIA_3D_wrong", server),
ASSERT_THROW(std::make_shared<SensorDummy>("sensor_POIA_3D_wrong", 3,
std::make_shared<ParamsSensorDummy>("sensor_POIA_3D_wrong", server),
server,
std::unordered_map<char, std::string>({{'I',"StateBlock"},
{'A',"StateQuaternion"}})),
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment