diff --git a/test/gtest_sensor_base.cpp b/test/gtest_sensor_base.cpp
index 06e1430b2f0f58a0c9132014c246abdd6b048307..d86c98b8c8d84ab0d06d25f427a1dcba3d173676 100644
--- a/test/gtest_sensor_base.cpp
+++ b/test/gtest_sensor_base.cpp
@@ -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"}})),