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

gtest for tdcp working

parent bf0a0df1
No related branches found
No related tags found
2 merge requests!28release after RAL,!27After 2nd RAL submission
Hide whitespace changes
Inline Side-by-side
test/gtest_factor_gnss_tdcp.cpp
+ 169
− 57
View file @ dface027
......@@ -18,7 +18,7 @@ Vector3d rpy_enu_map;
Matrix3d R_ecef_enu, R_enu_map;
Quaterniond q_map_base_r, q_map_base_k;
Vector3d t_ecef_sat1_r, t_ecef_sat2_r, t_ecef_sat3_r, t_ecef_sat4_r, t_ecef_sat1_k, t_ecef_sat2_k, t_ecef_sat3_k, t_ecef_sat4_k;
double prange1_r, prange_2_r, prange_3_r, prange_4_r, prange1_k, prange_2_k, prange_3_k, prange_4_k;
double prange1_r, prange2_r, prange3_r, prange4_r, prange1_k, prange2_k, prange3_k, prange4_k;
Vector1d clock_drift_r, clock_drift_k;
// WOLF
......@@ -74,14 +74,14 @@ void randomGroundtruth()
clock_drift_k = Vector1d::Random() * 1e2;
// pseudo ranges
prange_1_r = (t_ecef_sat1_r-t_ecef_antena_r).norm() + clock_drift_r(0);
prange_2_r = (t_ecef_sat2_r-t_ecef_antena_r).norm() + clock_drift_r(0);
prange_3_r = (t_ecef_sat3_r-t_ecef_antena_r).norm() + clock_drift_r(0);
prange_4_r = (t_ecef_sat4_r-t_ecef_antena_r).norm() + clock_drift_r(0);
prange_1_k = (t_ecef_sat1_k-t_ecef_antena_k).norm() + clock_drift_k(0);
prange_2_k = (t_ecef_sat2_k-t_ecef_antena_k).norm() + clock_drift_k(0);
prange_3_k = (t_ecef_sat3_k-t_ecef_antena_k).norm() + clock_drift_k(0);
prange_4_k = (t_ecef_sat4_k-t_ecef_antena_k).norm() + clock_drift_k(0);
prange1_r = (t_ecef_sat1_r-t_ecef_antena_r).norm() + clock_drift_r(0);
prange2_r = (t_ecef_sat2_r-t_ecef_antena_r).norm() + clock_drift_r(0);
prange3_r = (t_ecef_sat3_r-t_ecef_antena_r).norm() + clock_drift_r(0);
prange4_r = (t_ecef_sat4_r-t_ecef_antena_r).norm() + clock_drift_r(0);
prange1_k = (t_ecef_sat1_k-t_ecef_antena_k).norm() + clock_drift_k(0);
prange2_k = (t_ecef_sat2_k-t_ecef_antena_k).norm() + clock_drift_k(0);
prange3_k = (t_ecef_sat3_k-t_ecef_antena_k).norm() + clock_drift_k(0);
prange4_k = (t_ecef_sat4_k-t_ecef_antena_k).norm() + clock_drift_k(0);
}
void setUpProblem()
......@@ -109,31 +109,55 @@ void setUpProblem()
Vector7d frm_r_state;
frm_r_state.head<3>() = t_map_base_r;
frm_r_state.tail<4>() = q_map_base_r.coeffs();
frm_r = prb->emplaceFrame(KEY, frm_state, TimeStamp(0));
// capture
cap = CaptureBase::emplace<CaptureGnss>(frm, TimeStamp(0), gnss_sensor, nullptr);
cap->addStateBlock("T", std::make_shared<StateBlock>(clock_drift, false), prb);
// features
obsd_t obs1{0};
obs1.P[0] = prange_1;
ftr1 = FeatureBase::emplace<FeatureGnssSatellite>(cap, obs1, t_ecef_sat1);
obsd_t obs2{0};
obs2.P[0] = prange_2;
ftr2 = FeatureBase::emplace<FeatureGnssSatellite>(cap, obs2, t_ecef_sat2);
obsd_t obs3{0};
obs3.P[0] = prange_3;
ftr3 = FeatureBase::emplace<FeatureGnssSatellite>(cap, obs3, t_ecef_sat3);
obsd_t obs4{0};
obs4.P[0] = prange_4;
ftr4 = FeatureBase::emplace<FeatureGnssSatellite>(cap, obs4, t_ecef_sat4);
frm_r = prb->emplaceFrame(KEY, frm_r_state, TimeStamp(0));
// Frame k
Vector7d frm_k_state;
frm_k_state.head<3>() = t_map_base_k;
frm_k_state.tail<4>() = q_map_base_k.coeffs();
frm_k = prb->emplaceFrame(KEY, frm_k_state, TimeStamp(1));
// capture r
cap_r = CaptureBase::emplace<CaptureGnss>(frm_r, TimeStamp(0), gnss_sensor, nullptr);
cap_r->addStateBlock("T", std::make_shared<StateBlock>(clock_drift_r, false), prb);
// capture k
cap_k = CaptureBase::emplace<CaptureGnss>(frm_k, TimeStamp(1), gnss_sensor, nullptr);
cap_k->addStateBlock("T", std::make_shared<StateBlock>(clock_drift_k, false), prb);
// features r
obsd_t obs1_r{0};
obs1_r.L[0] = prange1_r;
ftr1_r = FeatureBase::emplace<FeatureGnssSatellite>(cap_r, obs1_r, t_ecef_sat1_r);
obsd_t obs2_r{0};
obs2_r.L[0] = prange2_r;
ftr2_r = FeatureBase::emplace<FeatureGnssSatellite>(cap_r, obs2_r, t_ecef_sat2_r);
obsd_t obs3_r{0};
obs3_r.L[0] = prange3_r;
ftr3_r = FeatureBase::emplace<FeatureGnssSatellite>(cap_r, obs3_r, t_ecef_sat3_r);
obsd_t obs4_r{0};
obs4_r.L[0] = prange4_r;
ftr4_r = FeatureBase::emplace<FeatureGnssSatellite>(cap_r, obs4_r, t_ecef_sat4_r);
// features k
obsd_t obs1_k{0};
obs1_k.L[0] = prange1_k;
ftr1_k = FeatureBase::emplace<FeatureGnssSatellite>(cap_k, obs1_k, t_ecef_sat1_k);
obsd_t obs2_k{0};
obs2_k.L[0] = prange2_k;
ftr2_k = FeatureBase::emplace<FeatureGnssSatellite>(cap_k, obs2_k, t_ecef_sat2_k);
obsd_t obs3_k{0};
obs3_k.L[0] = prange3_k;
ftr3_k = FeatureBase::emplace<FeatureGnssSatellite>(cap_k, obs3_k, t_ecef_sat3_k);
obsd_t obs4_k{0};
obs4_k.L[0] = prange4_k;
ftr4_k = FeatureBase::emplace<FeatureGnssSatellite>(cap_k, obs4_k, t_ecef_sat4_k);
// factors
fac1 = FactorBase::emplace<FactorGnssPseudoRange>(ftr1, Combination::CODE_L1, 0.1, ftr1, gnss_sensor, nullptr, false);
fac2 = FactorBase::emplace<FactorGnssPseudoRange>(ftr2, Combination::CODE_L1, 0.1, ftr2, gnss_sensor, nullptr, false);
fac3 = FactorBase::emplace<FactorGnssPseudoRange>(ftr3, Combination::CODE_L1, 0.1, ftr3, gnss_sensor, nullptr, false);
fac4 = FactorBase::emplace<FactorGnssPseudoRange>(ftr4, Combination::CODE_L1, 0.1, ftr4, gnss_sensor, nullptr, false);
fac1 = FactorBase::emplace<FactorGnssTdcp>(ftr1_r, Combination::CARRIER_L1, 0.1, ftr1_r, ftr1_k, gnss_sensor, nullptr, false);
fac2 = FactorBase::emplace<FactorGnssTdcp>(ftr2_r, Combination::CARRIER_L1, 0.1, ftr2_r, ftr2_k, gnss_sensor, nullptr, false);
fac3 = FactorBase::emplace<FactorGnssTdcp>(ftr3_r, Combination::CARRIER_L1, 0.1, ftr3_r, ftr3_k, gnss_sensor, nullptr, false);
fac4 = FactorBase::emplace<FactorGnssTdcp>(ftr4_r, Combination::CARRIER_L1, 0.1, ftr4_r, ftr4_k, gnss_sensor, nullptr, false);
// ASSERTS
// ENU-MAP
......@@ -143,11 +167,15 @@ void setUpProblem()
ASSERT_MATRIX_APPROX(gnss_sensor->getEnuMapTranslation()->getState(), t_enu_map, 1e-4);
// Antena
ASSERT_MATRIX_APPROX(gnss_sensor->getP()->getState(), t_base_antena, 1e-4);
// Frame
ASSERT_MATRIX_APPROX(frm->getP()->getState(), t_map_base, 1e-4);
ASSERT_MATRIX_APPROX(frm->getO()->getState(), q_map_base.coeffs(), 1e-4);
// clock drift
ASSERT_MATRIX_APPROX(clock_drift, cap->getStateBlock("T")->getState(), 1e-4);
// Frame r
ASSERT_MATRIX_APPROX(frm_r->getP()->getState(), t_map_base_r, 1e-4);
ASSERT_MATRIX_APPROX(frm_r->getO()->getState(), q_map_base_r.coeffs(), 1e-4);
// Frame k
ASSERT_MATRIX_APPROX(frm_k->getP()->getState(), t_map_base_k, 1e-4);
ASSERT_MATRIX_APPROX(frm_k->getO()->getState(), q_map_base_k.coeffs(), 1e-4);
// clock drift
ASSERT_MATRIX_APPROX(clock_drift_r, cap_r->getStateBlock("T")->getState(), 1e-4);
ASSERT_MATRIX_APPROX(clock_drift_k, cap_k->getStateBlock("T")->getState(), 1e-4);
}
void fixAllStates()
......@@ -159,14 +187,16 @@ void fixAllStates()
gnss_sensor->getEnuMapTranslation()->fix();
// Antena
gnss_sensor->getP()->fix();
// Frame
frm->fix();
// Frames
frm_r->fix();
frm_k->fix();
// clock drift
cap->fix();
cap_r->fix();
cap_k->fix();
}
////////////////////////////////////////////////////////
TEST(FactorGnssPreusoRangeTest, observe_clock_drift)
TEST(FactorGnssTdcpTest, observe_clock_drift_r)
{
for (auto i = 0; i < 100; i++)
{
......@@ -176,10 +206,10 @@ TEST(FactorGnssPreusoRangeTest, observe_clock_drift)
// fix/unfix
fixAllStates();
cap->getStateBlock("T")->unfix();
cap_r->getStateBlock("T")->unfix();
// perturb
cap->getStateBlock("T")->perturb(1e2);
cap_r->getStateBlock("T")->perturb(1e2);
// Only 1 factor
fac2->setStatus(FAC_INACTIVE);
......@@ -190,11 +220,11 @@ TEST(FactorGnssPreusoRangeTest, observe_clock_drift)
std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
//std::cout << report << std::endl;
ASSERT_MATRIX_APPROX(clock_drift, cap->getStateBlock("T")->getState(), 1e-4);
ASSERT_MATRIX_APPROX(clock_drift_r, cap_r->getStateBlock("T")->getState(), 1e-4);
}
}
TEST(FactorGnssPreusoRangeTest, observe_frame_p)
TEST(FactorGnssTdcpTest, observe_clock_drift_k)
{
for (auto i = 0; i < 100; i++)
{
......@@ -204,10 +234,38 @@ TEST(FactorGnssPreusoRangeTest, observe_frame_p)
// fix/unfix
fixAllStates();
frm->getP()->unfix();
cap_k->getStateBlock("T")->unfix();
// perturb
frm->getP()->perturb(1);
cap_k->getStateBlock("T")->perturb(1e2);
// Only 1 factor
fac2->setStatus(FAC_INACTIVE);
fac3->setStatus(FAC_INACTIVE);
fac4->setStatus(FAC_INACTIVE);
// solve
std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
//std::cout << report << std::endl;
ASSERT_MATRIX_APPROX(clock_drift_k, cap_k->getStateBlock("T")->getState(), 1e-4);
}
}
TEST(FactorGnssTdcpTest, observe_frame_p_r)
{
for (auto i = 0; i < 100; i++)
{
// setup random problem
randomGroundtruth();
setUpProblem();
// fix/unfix
fixAllStates();
frm_r->getP()->unfix();
// perturb
frm_r->getP()->perturb(1);
// Only 3 factors
fac4->setStatus(FAC_INACTIVE);
......@@ -216,11 +274,65 @@ TEST(FactorGnssPreusoRangeTest, observe_frame_p)
std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
//std::cout << report << std::endl;
ASSERT_MATRIX_APPROX(frm->getP()->getState(), t_map_base, 1e-4);
ASSERT_MATRIX_APPROX(frm_r->getP()->getState(), t_map_base_r, 1e-4);
}
}
TEST(FactorGnssTdcpTest, observe_frame_p_k)
{
for (auto i = 0; i < 100; i++)
{
// setup random problem
randomGroundtruth();
setUpProblem();
// fix/unfix
fixAllStates();
frm_k->getP()->unfix();
// perturb
frm_k->getP()->perturb(1);
// Only 3 factors
fac4->setStatus(FAC_INACTIVE);
// solve
std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
//std::cout << report << std::endl;
ASSERT_MATRIX_APPROX(frm_k->getP()->getState(), t_map_base_k, 1e-4);
}
}
TEST(FactorGnssTdcpTest, observe_frame_p_clock_k)
{
for (auto i = 0; i < 100; i++)
{
// setup random problem
randomGroundtruth();
setUpProblem();
// fix/unfix
fixAllStates();
frm_k->getP()->unfix();
cap_k->getStateBlock("T")->unfix();
// perturb
frm_k->getP()->perturb(1);
cap_k->getStateBlock("T")->perturb(1e2);
// all 4 factors
// solve
std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
//std::cout << report << std::endl;
ASSERT_MATRIX_APPROX(frm_k->getP()->getState(), t_map_base_k, 1e-4);
ASSERT_MATRIX_APPROX(clock_drift_k, cap_k->getStateBlock("T")->getState(), 1e-4);
}
}
TEST(FactorGnssPreusoRangeTest, observe_frame_p_clock)
TEST(FactorGnssTdcpTest, observe_frame_p_clock_r)
{
for (auto i = 0; i < 100; i++)
{
......@@ -230,12 +342,12 @@ TEST(FactorGnssPreusoRangeTest, observe_frame_p_clock)
// fix/unfix
fixAllStates();
frm->getP()->unfix();
cap->getStateBlock("T")->unfix();
frm_r->getP()->unfix();
cap_r->getStateBlock("T")->unfix();
// perturb
frm->getP()->perturb(1);
cap->getStateBlock("T")->perturb(1e2);
frm_r->getP()->perturb(1);
cap_r->getStateBlock("T")->perturb(1e2);
// all 4 factors
......@@ -243,12 +355,12 @@ TEST(FactorGnssPreusoRangeTest, observe_frame_p_clock)
std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
//std::cout << report << std::endl;
ASSERT_MATRIX_APPROX(frm->getP()->getState(), t_map_base, 1e-4);
ASSERT_MATRIX_APPROX(clock_drift, cap->getStateBlock("T")->getState(), 1e-4);
ASSERT_MATRIX_APPROX(frm_r->getP()->getState(), t_map_base_r, 1e-4);
ASSERT_MATRIX_APPROX(clock_drift_r, cap_r->getStateBlock("T")->getState(), 1e-4);
}
}
TEST(FactorGnssPreusoRangeTest, observe_enumap_p)
TEST(FactorGnssTdcpTest, observe_enumap_p)
{
for (auto i = 0; i < 100; i++)
{
......@@ -274,7 +386,7 @@ TEST(FactorGnssPreusoRangeTest, observe_enumap_p)
}
}
TEST(FactorGnssPreusoRangeTest, observe_enumap_o)
TEST(FactorGnssTdcpTest, observe_enumap_o)
{
for (auto i = 0; i < 100; i++)
{
......
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