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
...@@ -50,8 +50,12 @@ MatrixXd p_cov_2D = p_std_2D.cwiseAbs2().asDiagonal(); ...@@ -50,8 +50,12 @@ MatrixXd p_cov_2D = p_std_2D.cwiseAbs2().asDiagonal();
MatrixXd p_cov_3D = p_std_3D.cwiseAbs2().asDiagonal(); MatrixXd p_cov_3D = p_std_3D.cwiseAbs2().asDiagonal();
MatrixXd o_cov_2D = o_std_2D.cwiseAbs2().asDiagonal(); MatrixXd o_cov_2D = o_std_2D.cwiseAbs2().asDiagonal();
MatrixXd o_cov_3D = o_std_3D.cwiseAbs2().asDiagonal(); MatrixXd o_cov_3D = o_std_3D.cwiseAbs2().asDiagonal();
VectorXd noise_std = Vector2d::Constant(0.1); double noise_p_std = 0.1;
MatrixXd noise_cov = noise_std.cwiseAbs2().asDiagonal(); 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, void checkSensor(SensorBasePtr S,
char _key, char _key,
...@@ -101,16 +105,17 @@ void checkSensor(SensorBasePtr S, ...@@ -101,16 +105,17 @@ void checkSensor(SensorBasePtr S,
// 2D Fix // 2D Fix
TEST(SensorBase, POfix2D) TEST(SensorBase, POfix2D)
{ {
auto params = std::make_shared<ParamsSensorBase>(); auto params = std::make_shared<ParamsSensorDummy>();
params->noise_std = noise_std; 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 Priors({{'P',Prior("P", p_state_2D)}, //default "fix", not dynamic
{'O',Prior("O", o_state_2D)}})); {'O',Prior("O", o_state_2D)}}));
// noise // noise
ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS); ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std_2D, Constants::EPS);
ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov, Constants::EPS); ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov_2D, Constants::EPS);
// checks // checks
checkSensor(S, 'P', p_state_2D, true, vector0, false, vector0); checkSensor(S, 'P', p_state_2D, true, vector0, false, vector0);
...@@ -120,16 +125,17 @@ TEST(SensorBase, POfix2D) ...@@ -120,16 +125,17 @@ TEST(SensorBase, POfix2D)
// 3D Fix // 3D Fix
TEST(SensorBase, POfix3D) TEST(SensorBase, POfix3D)
{ {
auto params = std::make_shared<ParamsSensorBase>(); auto params = std::make_shared<ParamsSensorDummy>();
params->noise_std = noise_std; 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 Priors({{'P',Prior("P", p_state_3D)}, //default "fix", not dynamic
{'O',Prior("O", o_state_3D)}})); {'O',Prior("O", o_state_3D)}}));
// noise // noise
ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS); ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std_3D, Constants::EPS);
ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov, Constants::EPS); ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov_3D, Constants::EPS);
// factors // factors
ASSERT_EQ(S->getPriorFeatures().size(), 0); ASSERT_EQ(S->getPriorFeatures().size(), 0);
...@@ -142,16 +148,17 @@ TEST(SensorBase, POfix3D) ...@@ -142,16 +148,17 @@ TEST(SensorBase, POfix3D)
// 2D Initial guess // 2D Initial guess
TEST(SensorBase, POinitial_guess2D) TEST(SensorBase, POinitial_guess2D)
{ {
auto params = std::make_shared<ParamsSensorBase>(); auto params = std::make_shared<ParamsSensorDummy>();
params->noise_std = noise_std; 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")}, Priors({{'P',Prior("P", p_state_2D, "initial_guess")},
{'O',Prior("O", o_state_2D, "initial_guess")}})); {'O',Prior("O", o_state_2D, "initial_guess")}}));
// noise // noise
ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS); ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std_2D, Constants::EPS);
ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov, Constants::EPS); ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov_2D, Constants::EPS);
// factors // factors
ASSERT_EQ(S->getPriorFeatures().size(), 0); ASSERT_EQ(S->getPriorFeatures().size(), 0);
...@@ -164,16 +171,17 @@ TEST(SensorBase, POinitial_guess2D) ...@@ -164,16 +171,17 @@ TEST(SensorBase, POinitial_guess2D)
// 3D Initial guess // 3D Initial guess
TEST(SensorBase, POinitial_guess3D) TEST(SensorBase, POinitial_guess3D)
{ {
auto params = std::make_shared<ParamsSensorBase>(); auto params = std::make_shared<ParamsSensorDummy>();
params->noise_std = noise_std; 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")}, Priors({{'P',Prior("P", p_state_3D, "initial_guess")},
{'O',Prior("O", o_state_3D, "initial_guess")}})); {'O',Prior("O", o_state_3D, "initial_guess")}}));
// noise // noise
ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS); ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std_3D, Constants::EPS);
ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov, Constants::EPS); ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov_3D, Constants::EPS);
// factors // factors
ASSERT_EQ(S->getPriorFeatures().size(), 0); ASSERT_EQ(S->getPriorFeatures().size(), 0);
...@@ -186,16 +194,17 @@ TEST(SensorBase, POinitial_guess3D) ...@@ -186,16 +194,17 @@ TEST(SensorBase, POinitial_guess3D)
// 2D Factor // 2D Factor
TEST(SensorBase, POfactor2D) TEST(SensorBase, POfactor2D)
{ {
auto params = std::make_shared<ParamsSensorBase>(); auto params = std::make_shared<ParamsSensorDummy>();
params->noise_std = noise_std; 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)}, Priors({{'P',Prior("P", p_state_2D, "factor", p_std_2D)},
{'O',Prior("O", o_state_2D, "factor", o_std_2D)}})); {'O',Prior("O", o_state_2D, "factor", o_std_2D)}}));
// noise // noise
ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS); ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std_2D, Constants::EPS);
ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov, Constants::EPS); ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov_2D, Constants::EPS);
// factors // factors
ASSERT_EQ(S->getPriorFeatures().size(), 2); ASSERT_EQ(S->getPriorFeatures().size(), 2);
...@@ -208,16 +217,17 @@ TEST(SensorBase, POfactor2D) ...@@ -208,16 +217,17 @@ TEST(SensorBase, POfactor2D)
// 3D Factor // 3D Factor
TEST(SensorBase, POfactor3D) TEST(SensorBase, POfactor3D)
{ {
auto params = std::make_shared<ParamsSensorBase>(); auto params = std::make_shared<ParamsSensorDummy>();
params->noise_std = noise_std; 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)}, Priors({{'P',Prior("P", p_state_3D, "factor", p_std_3D)},
{'O',Prior("O", o_state_3D, "factor", o_std_3D)}})); {'O',Prior("O", o_state_3D, "factor", o_std_3D)}}));
// noise // noise
ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS); ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std_3D, Constants::EPS);
ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov, Constants::EPS); ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov_3D, Constants::EPS);
// factors // factors
ASSERT_EQ(S->getPriorFeatures().size(), 2); ASSERT_EQ(S->getPriorFeatures().size(), 2);
...@@ -230,16 +240,17 @@ TEST(SensorBase, POfactor3D) ...@@ -230,16 +240,17 @@ TEST(SensorBase, POfactor3D)
// 2D Initial guess dynamic // 2D Initial guess dynamic
TEST(SensorBase, POinitial_guess_dynamic2D) TEST(SensorBase, POinitial_guess_dynamic2D)
{ {
auto params = std::make_shared<ParamsSensorBase>(); auto params = std::make_shared<ParamsSensorDummy>();
params->noise_std = noise_std; 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)}, Priors({{'P',Prior("P", p_state_2D, "initial_guess", vector0, true)},
{'O',Prior("O", o_state_2D, "initial_guess", vector0, true)}})); {'O',Prior("O", o_state_2D, "initial_guess", vector0, true)}}));
// noise // noise
ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS); ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std_2D, Constants::EPS);
ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov, Constants::EPS); ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov_2D, Constants::EPS);
// factors // factors
ASSERT_EQ(S->getPriorFeatures().size(), 0); ASSERT_EQ(S->getPriorFeatures().size(), 0);
...@@ -252,16 +263,17 @@ TEST(SensorBase, POinitial_guess_dynamic2D) ...@@ -252,16 +263,17 @@ TEST(SensorBase, POinitial_guess_dynamic2D)
// 3D Initial guess dynamic // 3D Initial guess dynamic
TEST(SensorBase, POinitial_guess_dynamic3D) TEST(SensorBase, POinitial_guess_dynamic3D)
{ {
auto params = std::make_shared<ParamsSensorBase>(); auto params = std::make_shared<ParamsSensorDummy>();
params->noise_std = noise_std; 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)}, Priors({{'P',Prior("P", p_state_3D, "initial_guess", vector0, true)},
{'O',Prior("O", o_state_3D, "initial_guess", vector0, true)}})); {'O',Prior("O", o_state_3D, "initial_guess", vector0, true)}}));
// noise // noise
ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS); ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std_3D, Constants::EPS);
ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov, Constants::EPS); ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov_3D, Constants::EPS);
// factors // factors
ASSERT_EQ(S->getPriorFeatures().size(), 0); ASSERT_EQ(S->getPriorFeatures().size(), 0);
...@@ -274,16 +286,17 @@ TEST(SensorBase, POinitial_guess_dynamic3D) ...@@ -274,16 +286,17 @@ TEST(SensorBase, POinitial_guess_dynamic3D)
// 2D Initial guess dynamic drift // 2D Initial guess dynamic drift
TEST(SensorBase, POinitial_guess_dynamic2D_drift) TEST(SensorBase, POinitial_guess_dynamic2D_drift)
{ {
auto params = std::make_shared<ParamsSensorBase>(); auto params = std::make_shared<ParamsSensorDummy>();
params->noise_std = noise_std; 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)}, 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)}})); {'O',Prior("O", o_state_2D, "initial_guess", vector0, true, o_std_2D)}}));
// noise // noise
ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS); ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std_2D, Constants::EPS);
ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov, Constants::EPS); ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov_2D, Constants::EPS);
// factors // factors
ASSERT_EQ(S->getPriorFeatures().size(), 0); ASSERT_EQ(S->getPriorFeatures().size(), 0);
...@@ -296,16 +309,17 @@ TEST(SensorBase, POinitial_guess_dynamic2D_drift) ...@@ -296,16 +309,17 @@ TEST(SensorBase, POinitial_guess_dynamic2D_drift)
// 3D Initial guess dynamic drift // 3D Initial guess dynamic drift
TEST(SensorBase, POinitial_guess_dynamic3D_drift) TEST(SensorBase, POinitial_guess_dynamic3D_drift)
{ {
auto params = std::make_shared<ParamsSensorBase>(); auto params = std::make_shared<ParamsSensorDummy>();
params->noise_std = noise_std; 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)}, 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)}})); {'O',Prior("O", o_state_3D, "initial_guess", vector0, true, o_std_3D)}}));
// noise // noise
ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS); ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std_3D, Constants::EPS);
ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov, Constants::EPS); ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov_3D, Constants::EPS);
// factors // factors
ASSERT_EQ(S->getPriorFeatures().size(), 0); ASSERT_EQ(S->getPriorFeatures().size(), 0);
...@@ -318,20 +332,21 @@ TEST(SensorBase, POinitial_guess_dynamic3D_drift) ...@@ -318,20 +332,21 @@ TEST(SensorBase, POinitial_guess_dynamic3D_drift)
// 3D POI mixed // 3D POI mixed
TEST(SensorBase, POI_mixed) TEST(SensorBase, POI_mixed)
{ {
auto params = std::make_shared<ParamsSensorBase>(); auto params = std::make_shared<ParamsSensorDummy>();
params->noise_std = noise_std; params->noise_p_std = noise_p_std;
params->noise_o_std = noise_o_std;
VectorXd i_state_3D = VectorXd::Random(5); 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)}, Priors({{'P',Prior("P", p_state_3D, "fix", vector0, true)},
{'O',Prior("O", o_state_3D, "factor", o_std_3D, true, o_std_3D)}, {'O',Prior("O", o_state_3D, "factor", o_std_3D, true, o_std_3D)},
{'I',Prior("StateBlock", i_state_3D, "initial_guess")}, {'I',Prior("StateBlock", i_state_3D, "initial_guess")},
{'A',Prior("StateQuaternion", o_state_3D, "factor", o_std_3D)}})); {'A',Prior("StateQuaternion", o_state_3D, "factor", o_std_3D)}}));
// noise // noise
ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std, Constants::EPS); ASSERT_MATRIX_APPROX(S->getNoiseStd(), noise_std_3D, Constants::EPS);
ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov, Constants::EPS); ASSERT_MATRIX_APPROX(S->getNoiseCov(), noise_cov_3D, Constants::EPS);
// factors // factors
ASSERT_EQ(S->getPriorFeatures().size(), 2); ASSERT_EQ(S->getPriorFeatures().size(), 2);
...@@ -376,8 +391,8 @@ TEST(SensorBase, server_PO) ...@@ -376,8 +391,8 @@ TEST(SensorBase, server_PO)
// CORRECT YAML // CORRECT YAML
WOLF_INFO("Creating sensor from name ", name); WOLF_INFO("Creating sensor from name ", name);
auto params = std::make_shared<ParamsSensorBase>(name, server); auto params = std::make_shared<ParamsSensorDummy>(name, server);
auto S = std::make_shared<SensorBase>("SensorBase", name, dim, params, server); auto S = std::make_shared<SensorDummy>( name, dim, params, server);
auto p_size = dim; auto p_size = dim;
auto o_size = dim == 2 ? 1 : 4; auto o_size = dim == 2 ? 1 : 4;
...@@ -400,8 +415,8 @@ TEST(SensorBase, server_PO) ...@@ -400,8 +415,8 @@ TEST(SensorBase, server_PO)
// INCORRECT YAML // INCORRECT YAML
WOLF_INFO("Creating sensor from name ", name + "_wrong"); WOLF_INFO("Creating sensor from name ", name + "_wrong");
ASSERT_THROW(std::make_shared<SensorBase>("SensorBase", name + "_wrong", dim, ASSERT_THROW(std::make_shared<SensorDummy>( name + "_wrong", dim,
std::make_shared<ParamsSensorBase>(name + "_wrong", server), std::make_shared<ParamsSensorDummy>(name + "_wrong", server),
server),std::runtime_error); server),std::runtime_error);
} }
} }
...@@ -421,8 +436,8 @@ TEST(SensorBase, server_POIA) ...@@ -421,8 +436,8 @@ TEST(SensorBase, server_POIA)
// POIA - 3D - CORRECT YAML // POIA - 3D - CORRECT YAML
WOLF_INFO("Creating sensor from name sensor_POIA_3D"); WOLF_INFO("Creating sensor from name sensor_POIA_3D");
auto params = std::make_shared<ParamsSensorBase>("sensor_POIA_3D", server); auto params = std::make_shared<ParamsSensorDummy>("sensor_POIA_3D", server);
auto S = std::make_shared<SensorBase>("SensorBase", "sensor_POIA_3D", 3, params, server, auto S = std::make_shared<SensorDummy>("sensor_POIA_3D", 3, params, server,
std::unordered_map<char, std::string>({{'I',"StateBlock"}, std::unordered_map<char, std::string>({{'I',"StateBlock"},
{'A',"StateQuaternion"}})); {'A',"StateQuaternion"}}));
...@@ -449,8 +464,8 @@ TEST(SensorBase, server_POIA_wrong) ...@@ -449,8 +464,8 @@ TEST(SensorBase, server_POIA_wrong)
// POIA - 3D - INCORRECT YAML // POIA - 3D - INCORRECT YAML
WOLF_INFO("Creating sensor from name sensor_POIA_3D_wrong"); WOLF_INFO("Creating sensor from name sensor_POIA_3D_wrong");
ASSERT_THROW(std::make_shared<SensorBase>("SensorBase", "sensor_POIA_3D_wrong", 3, ASSERT_THROW(std::make_shared<SensorDummy>("sensor_POIA_3D_wrong", 3,
std::make_shared<ParamsSensorBase>("sensor_POIA_3D_wrong", server), std::make_shared<ParamsSensorDummy>("sensor_POIA_3D_wrong", server),
server, server,
std::unordered_map<char, std::string>({{'I',"StateBlock"}, std::unordered_map<char, std::string>({{'I',"StateBlock"},
{'A',"StateQuaternion"}})), {'A',"StateQuaternion"}})),
......
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