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

working on tdcp factor

parent c98b88d6
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/CMakeLists.txt
+ 2
− 2
View file @ bf0a0df1
...@@ -21,6 +21,6 @@ wolf_add_gtest(gtest_factor_gnss_pseudo_range gtest_factor_gnss_pseudo_range.cpp ...@@ -21,6 +21,6 @@ wolf_add_gtest(gtest_factor_gnss_pseudo_range gtest_factor_gnss_pseudo_range.cpp
target_link_libraries(gtest_factor_gnss_pseudo_range ${PLUGIN_NAME} ${wolf_LIBRARY}) target_link_libraries(gtest_factor_gnss_pseudo_range ${PLUGIN_NAME} ${wolf_LIBRARY})
# FactorGnssTdcp test # FactorGnssTdcp test
#wolf_add_gtest(gtest_factor_gnss_tdcp_2d gtest_factor_gnss_tdcp_2d.cpp) wolf_add_gtest(gtest_factor_gnss_tdcp gtest_factor_gnss_tdcp.cpp)
#target_link_libraries(gtest_factor_gnss_tdcp_2d ${PLUGIN_NAME} ${wolf_LIBRARY}) target_link_libraries(gtest_factor_gnss_tdcp ${PLUGIN_NAME} ${wolf_LIBRARY})
test/gtest_factor_gnss_pseudo_range.cpp
+ 0
− 2
View file @ bf0a0df1
#include "gnss/factor/factor_gnss_fix_2d.h"
#include "gnss/factor/factor_gnss_pseudo_range.h" #include "gnss/factor/factor_gnss_pseudo_range.h"
#include <core/utils/utils_gtest.h> #include <core/utils/utils_gtest.h>
#include "core/problem/problem.h" #include "core/problem/problem.h"
#include "gnss/sensor/sensor_gnss.h" #include "gnss/sensor/sensor_gnss.h"
#include "gnss/processor/processor_gnss_fix.h"
#include "gnss/capture/capture_gnss.h" #include "gnss/capture/capture_gnss.h"
#include "core/ceres_wrapper/ceres_manager.h" #include "core/ceres_wrapper/ceres_manager.h"
......
test/gtest_factor_gnss_tdcp.cpp 0 → 100644
+ 311
− 0
View file @ bf0a0df1
#include "gnss/factor/factor_gnss_tdcp.h"
#include <core/utils/utils_gtest.h>
#include "core/problem/problem.h"
#include "gnss/sensor/sensor_gnss.h"
#include "gnss/capture/capture_gnss.h"
#include "core/ceres_wrapper/ceres_manager.h"
using namespace Eigen;
using namespace wolf;
using namespace GnssUtils;
// groundtruth transformations
Vector3d t_ecef_enu, t_enu_map, t_map_base_r, t_map_base_k, t_base_antena; // Antena extrinsics
Vector3d t_ecef_antena_r, t_ecef_antena_k;
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;
Vector1d clock_drift_r, clock_drift_k;
// WOLF
ProblemPtr prb = Problem::create("PO", 3);
CeresManagerPtr solver = std::make_shared<CeresManager>(prb);
SensorGnssPtr gnss_sensor = std::static_pointer_cast<SensorGnss>(prb->installSensor("SensorGnss", "gnss", t_base_antena, std::make_shared<ParamsSensorGnss>()));
FrameBasePtr frm_r, frm_k;
CaptureGnssPtr cap_r, cap_k;
FeatureGnssSatellitePtr ftr1_r, ftr2_r, ftr3_r, ftr4_r, ftr1_k, ftr2_k, ftr3_k, ftr4_k;
FactorGnssTdcpPtr fac1, fac2, fac3, fac4;
void randomGroundtruth()
{
// ECEF-ENU
t_ecef_enu = Vector3d::Random() * 1e3;
Vector3d rpy_ecef_enu = M_PI*Vector3d::Random();
R_ecef_enu = (AngleAxis<double>(rpy_ecef_enu(0), Vector3d::UnitX()) *
AngleAxis<double>(rpy_ecef_enu(1), Vector3d::UnitY()) *
AngleAxis<double>(rpy_ecef_enu(2), Vector3d::UnitZ())).toRotationMatrix();
// ENU-MAP
t_enu_map = Vector3d::Random() * 1e3;
rpy_enu_map = M_PI*Vector3d::Random();
R_enu_map = (AngleAxis<double>(rpy_enu_map(0), Vector3d::UnitX()) *
AngleAxis<double>(rpy_enu_map(1), Vector3d::UnitY()) *
AngleAxis<double>(rpy_enu_map(2), Vector3d::UnitZ())).toRotationMatrix();
// MAP-BASE
t_map_base_k = Vector3d::Random() * 1e2;
q_map_base_k = Quaterniond::UnitRandom();
t_map_base_r = Vector3d::Random() * 1e2;
q_map_base_r = Quaterniond::UnitRandom();
// BASE-ANTENA (EXTRINSICS)
t_base_antena = Vector3d::Random();
// composition
t_ecef_antena_r = R_ecef_enu * (R_enu_map * (q_map_base_r * t_base_antena + t_map_base_r) + t_enu_map ) + t_ecef_enu;
t_ecef_antena_k = R_ecef_enu * (R_enu_map * (q_map_base_k * t_base_antena + t_map_base_k) + t_enu_map ) + t_ecef_enu;
// SATELLITES
t_ecef_sat1_r = Vector3d::Random() * 1e4;
t_ecef_sat2_r = Vector3d::Random() * 1e4;
t_ecef_sat3_r = Vector3d::Random() * 1e4;
t_ecef_sat4_r = Vector3d::Random() * 1e4;
t_ecef_sat1_k = Vector3d::Random() * 1e4;
t_ecef_sat2_k = Vector3d::Random() * 1e4;
t_ecef_sat3_k = Vector3d::Random() * 1e4;
t_ecef_sat4_k = Vector3d::Random() * 1e4;
// clock drift
clock_drift_r = Vector1d::Random() * 1e2;
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);
}
void setUpProblem()
{
// remove previous setup
if (frm_r)
frm_r->remove();
if (frm_k)
frm_k->remove();
solver->getSolverOptions().max_num_iterations = 100;
// Sensor
// ENU-MAP
gnss_sensor->setEnuMapTranslationState(t_enu_map);
gnss_sensor->setEnuMapRollState(rpy_enu_map(0));
gnss_sensor->setEnuMapPitchState(rpy_enu_map(1));
gnss_sensor->setEnuMapYawState(rpy_enu_map(2));
// ECEF-ENU
gnss_sensor->setEnuEcef(R_ecef_enu.transpose(), R_ecef_enu.transpose()*(-t_ecef_enu));
// Extrinsics
gnss_sensor->getP()->setState(t_base_antena);
// Frame r
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);
// 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);
// ASSERTS
// ENU-MAP
ASSERT_FLOAT_EQ(gnss_sensor->getEnuMapRoll()->getState()(0), rpy_enu_map(0));
ASSERT_FLOAT_EQ(gnss_sensor->getEnuMapPitch()->getState()(0), rpy_enu_map(1));
ASSERT_FLOAT_EQ(gnss_sensor->getEnuMapYaw()->getState()(0), rpy_enu_map(2));
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);
}
void fixAllStates()
{
// ENU-MAP
gnss_sensor->getEnuMapRoll()->fix();
gnss_sensor->getEnuMapPitch()->fix();
gnss_sensor->getEnuMapYaw()->fix();
gnss_sensor->getEnuMapTranslation()->fix();
// Antena
gnss_sensor->getP()->fix();
// Frame
frm->fix();
// clock drift
cap->fix();
}
////////////////////////////////////////////////////////
TEST(FactorGnssPreusoRangeTest, observe_clock_drift)
{
for (auto i = 0; i < 100; i++)
{
// setup random problem
randomGroundtruth();
setUpProblem();
// fix/unfix
fixAllStates();
cap->getStateBlock("T")->unfix();
// perturb
cap->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, cap->getStateBlock("T")->getState(), 1e-4);
}
}
TEST(FactorGnssPreusoRangeTest, observe_frame_p)
{
for (auto i = 0; i < 100; i++)
{
// setup random problem
randomGroundtruth();
setUpProblem();
// fix/unfix
fixAllStates();
frm->getP()->unfix();
// perturb
frm->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->getP()->getState(), t_map_base, 1e-4);
}
}
TEST(FactorGnssPreusoRangeTest, observe_frame_p_clock)
{
for (auto i = 0; i < 100; i++)
{
// setup random problem
randomGroundtruth();
setUpProblem();
// fix/unfix
fixAllStates();
frm->getP()->unfix();
cap->getStateBlock("T")->unfix();
// perturb
frm->getP()->perturb(1);
cap->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->getP()->getState(), t_map_base, 1e-4);
ASSERT_MATRIX_APPROX(clock_drift, cap->getStateBlock("T")->getState(), 1e-4);
}
}
TEST(FactorGnssPreusoRangeTest, observe_enumap_p)
{
for (auto i = 0; i < 100; i++)
{
// setup random problem
randomGroundtruth();
setUpProblem();
// fix/unfix
fixAllStates();
gnss_sensor->getEnuMapTranslation()->unfix();
// perturb
gnss_sensor->getEnuMapTranslation()->perturb(1e2);
// Only 3 factors
fac4->setStatus(FAC_INACTIVE);
// solve
std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
//std::cout << report << std::endl;
ASSERT_MATRIX_APPROX(gnss_sensor->getEnuMapTranslation()->getState(), t_enu_map, 1e-4);
}
}
TEST(FactorGnssPreusoRangeTest, observe_enumap_o)
{
for (auto i = 0; i < 100; i++)
{
// setup random problem
randomGroundtruth();
setUpProblem();
// fix/unfix
fixAllStates();
gnss_sensor->getEnuMapRoll()->unfix();
gnss_sensor->getEnuMapPitch()->unfix();
gnss_sensor->getEnuMapYaw()->unfix();
// perturb
gnss_sensor->getEnuMapRoll()->perturb(1);
gnss_sensor->getEnuMapPitch()->perturb(1);
gnss_sensor->getEnuMapYaw()->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(gnss_sensor->getEnuMapTranslation()->getState(), t_enu_map, 1e-4);
}
}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
test/gtest_factor_gnss_tdcp_2d.cpp deleted 100644 → 0
+ 0
− 266
View file @ c98b88d6
/**
* \file gtest_factor_gnss_Tdcp_2d.cpp
*
* Created on: Aug 28, 2018
* \author: jvallve
*/
#include <core/utils/utils_gtest.h>
#include "core/capture/capture_motion.h"
#include "core/problem/problem.h"
#include "gnss/sensor/sensor_gnss.h"
#include "core/sensor/sensor_odom_2d.h"
#include "core/processor/processor_odom_2d.h"
#include "core/ceres_wrapper/ceres_manager.h"
#include "../include/gnss/factor/factor_gnss_tdcp_2d.h"
#include "../include/gnss/processor/processor_gnss_tdcp.h"
using namespace Eigen;
using namespace wolf;
using std::cout;
using std::endl;
class FactorGnssTdcp2dTest : public testing::Test
{
public:
// groundtruth transformations
Vector3d t_ecef_enu;
Matrix3d R_ecef_enu, R_enu_map, R_map_base1, R_map_base2;
Vector3d t_base_antena, t_ecef_antena1, t_ecef_antena2;
Vector1d o_enu_map;
Vector3d t_map_base1, t_map_base2;
Vector1d o_map_base1, o_map_base2;
// WOLF
ProblemPtr problem_ptr;
CeresManagerPtr ceres_mgr_ptr;
SensorGnssPtr gnss_sensor_ptr;
SensorOdom2dPtr odom_sensor_ptr;
FrameBasePtr prior_frame_ptr;
FactorGnssTdcp2dTest()
{
o_enu_map << 2.6;
t_map_base1 << 32, -34, 0; // z=0
o_map_base1 << -0.4;
t_map_base2 << -76, 63, 0; // z=0
o_map_base2 << 0.7;
t_base_antena << 3,-2,8;// Antena extrinsics
t_ecef_enu << 100,30,50;
R_ecef_enu = (AngleAxis<double>(1.3, Vector3d::UnitX())
*AngleAxis<double>(-2.2, Vector3d::UnitY())
*AngleAxis<double>(-1.8, Vector3d::UnitZ())).toRotationMatrix();
// ----------------------------------------------------
// Problem and solver
problem_ptr = Problem::create("PO", 2);
ceres_mgr_ptr = std::make_shared<CeresManager>(problem_ptr);
ceres_mgr_ptr->getSolverOptions().max_num_iterations = 10;
// gnss sensor
gnss_sensor_ptr = std::static_pointer_cast<SensorGnss>(problem_ptr->installSensor("SensorGnss", "gnss", t_base_antena, std::make_shared<ParamsSensorBase>()));
gnss_sensor_ptr->getEnuMapRoll()->fix();
gnss_sensor_ptr->getEnuMapPitch()->fix();
gnss_sensor_ptr->setEnuEcef(R_ecef_enu.transpose(), R_ecef_enu.transpose()*(-t_ecef_enu));
// gnss processor
ParamsProcessorGnssTdcpPtr gnss_params_ptr = std::make_shared<ParamsProcessorGnssTdcp>();
gnss_params_ptr->time_tolerance = 1.0;
gnss_params_ptr->voting_active = true;
gnss_params_ptr->voting_aux_active = false;
gnss_params_ptr->max_time_span = 0;
gnss_params_ptr->dist_traveled = 0;
gnss_params_ptr->enu_map_init_dist_min = 0;
problem_ptr->installProcessor("ProcessorGnssTdcp", "gnss single difference", gnss_sensor_ptr, gnss_params_ptr);
// odom sensor & processor
ParamsSensorOdom2dPtr odom_sensor_params_ptr = std::make_shared<ParamsSensorOdom2d>();
odom_sensor_params_ptr->k_disp_to_disp = 0.1;
odom_sensor_params_ptr->k_rot_to_rot = 0.1;
odom_sensor_ptr = std::static_pointer_cast<SensorOdom2d>(problem_ptr->installSensor("SensorOdom2d", "odometer", VectorXd::Zero(3), odom_sensor_params_ptr));
ParamsProcessorOdom2dPtr odom_params_ptr = std::make_shared<ParamsProcessorOdom2d>();
odom_params_ptr->voting_active = true;
odom_params_ptr->dist_traveled = 1;
odom_params_ptr->angle_turned = 2.0;
odom_params_ptr->max_time_span = 1.0;
odom_params_ptr->time_tolerance = 1.0;
problem_ptr->installProcessor("ProcessorOdom2d", "main odometry", odom_sensor_ptr, odom_params_ptr);
// set prior (FIXED)
Vector3d frame1state = t_map_base1;
frame1state(2) = o_map_base1(0);
prior_frame_ptr = problem_ptr->setPrior(frame1state, Matrix3d::Identity(), TimeStamp(0), double(0.1));
prior_frame_ptr->fix();
};
virtual void SetUp()
{
// reset grountruth parameters
R_enu_map = AngleAxis<double>(o_enu_map(0), Vector3d::UnitZ()).toRotationMatrix();
R_map_base1 = Matrix3d::Identity();
R_map_base1.topLeftCorner(2,2) = Rotation2D<double>(o_map_base1(0)).matrix();
R_map_base2 = Matrix3d::Identity();
R_map_base2.topLeftCorner(2,2) = Rotation2D<double>(o_map_base2(0)).matrix();
t_ecef_antena1 = R_ecef_enu * R_enu_map * (R_map_base1 * t_base_antena + t_map_base1) + t_ecef_enu;
t_ecef_antena2 = R_ecef_enu * R_enu_map * (R_map_base2 * t_base_antena + t_map_base2) + t_ecef_enu;
// Reset antena extrinsics
gnss_sensor_ptr->getP()->setState(t_base_antena, false); //false -> do not notify the solver
// Reset ENU_ECEF
gnss_sensor_ptr->setEnuEcef(R_ecef_enu.transpose(), R_ecef_enu.transpose()*(-t_ecef_enu));
}
};
////////////////////////////////////////////////////////
TEST_F(FactorGnssTdcp2dTest, check_tree)
{
ASSERT_TRUE(problem_ptr->check(0));
ASSERT_TRUE(prior_frame_ptr != nullptr);
}
/*
* Test with one GNSS TDCP capture.
*
* Estimating: MAP_BASE2 position.
* Fixed: MAP_BASE1, ENU_MAP fixed, MAP_BASE2 orientation, BASE_ANTENA.
*/
TEST_F(FactorGnssTdcp2dTest, gnss_1_map_base_position)
{
// Create GNSS Fix capture
CaptureGnssTdcpPtr cap_gnss_ptr = std::make_shared<CaptureGnssTdcp>(TimeStamp(0), gnss_sensor_ptr, t_ecef_antena2-t_ecef_antena1, 1e-6*Matrix3d::Identity(), prior_frame_ptr);
gnss_sensor_ptr->process(cap_gnss_ptr);
// fixing things
gnss_sensor_ptr->setEnuMapYawState(o_enu_map(0)); // enu-map yaw orientation
gnss_sensor_ptr->getEnuMapYaw()->fix();
cap_gnss_ptr->getFrame()->getO()->setState(o_map_base2); // frame orientation
cap_gnss_ptr->getFrame()->getO()->fix();
std::cout << "frame1: " << prior_frame_ptr->getState().transpose() << std::endl;
// distort frm position
Vector3d frm_dist = cap_gnss_ptr->getFrame()->getState();
frm_dist(0) += 18;
frm_dist(1) += -58;
cap_gnss_ptr->getFrame()->setState(frm_dist);
// solve for frm
std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::BRIEF);
ASSERT_TRUE(cap_gnss_ptr->getFrame()->isKey());
ASSERT_MATRIX_APPROX(cap_gnss_ptr->getFrame()->getState().head(2), t_map_base2.head(2), 1e-6);
}
/*
* Test with one GNSS TDCP capture.
*
* Estimating: MAP_BASE2 orientation.
* Fixed: MAP_BASE1, ENU_MAP fixed, MAP_BASE2 position, BASE_ANTENA.
*/
TEST_F(FactorGnssTdcp2dTest, gnss_1_map_base_orientation)
{
ASSERT_TRUE(prior_frame_ptr != nullptr);
// Create GNSS Fix capture
CaptureGnssTdcpPtr cap_gnss_ptr = std::make_shared<CaptureGnssTdcp>(TimeStamp(0), gnss_sensor_ptr, t_ecef_antena2-t_ecef_antena1, 1e-6*Matrix3d::Identity(), prior_frame_ptr);
ASSERT_TRUE(cap_gnss_ptr->getOriginFrame() != nullptr);
gnss_sensor_ptr->process(cap_gnss_ptr);
// fixing things
gnss_sensor_ptr->setEnuMapYawState(o_enu_map(0)); // enu-map yaw orientation
gnss_sensor_ptr->getEnuMapYaw()->fix();
cap_gnss_ptr->getFrame()->getP()->setState(t_map_base2.head(2)); // frame position
cap_gnss_ptr->getFrame()->getP()->fix();
// distort frm position
Vector1d frm_dist = cap_gnss_ptr->getFrame()->getO()->getState();
frm_dist(0) += 1.8;
cap_gnss_ptr->getFrame()->getO()->setState(frm_dist);
// solve for frm
std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::QUIET);
ASSERT_TRUE(cap_gnss_ptr->getFrame()->isKey());
ASSERT_MATRIX_APPROX(cap_gnss_ptr->getFrame()->getO()->getState(), o_map_base2, 1e-6);
}
/*
* Test with one GNSS TDCP capture.
*
* Estimating: ENU_MAP yaw.
* Fixed: MAP_BASE1, MAP_BASE2 and BASE_ANTENA.
*/
TEST_F(FactorGnssTdcp2dTest, gnss_1_enu_map_yaw)
{
// Create GNSS Fix capture
CaptureGnssTdcpPtr cap_gnss_ptr = std::make_shared<CaptureGnssTdcp>(TimeStamp(0), gnss_sensor_ptr, t_ecef_antena2-t_ecef_antena1, 1e-6*Matrix3d::Identity(), prior_frame_ptr);
gnss_sensor_ptr->process(cap_gnss_ptr);
// unfixing things
gnss_sensor_ptr->getEnuMapYaw()->unfix();
// fixing things
cap_gnss_ptr->getFrame()->getP()->setState(t_map_base2.head(2)); // frame position
cap_gnss_ptr->getFrame()->getP()->fix();
cap_gnss_ptr->getFrame()->getO()->setState(o_map_base2); // frame orientation
cap_gnss_ptr->getFrame()->getO()->fix();
// distort yaw enu_map
Vector1d o_enu_map_dist = gnss_sensor_ptr->getEnuMapYaw()->getState();
o_enu_map_dist(0) += 1.8;
gnss_sensor_ptr->setEnuMapYawState(o_enu_map_dist(0));
// solve for frm
std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::QUIET);
ASSERT_TRUE(cap_gnss_ptr->getFrame()->isKey());
ASSERT_MATRIX_APPROX(gnss_sensor_ptr->getEnuMapYaw()->getState(), o_enu_map, 1e-6);
}
/*
* Test with one GNSS TDCP capture.
*
* Estimating: BASE_ANTENA (2 first components obsevable).
* Fixed: MAP_BASE1, ENU_MAP, MAP_BASE2.
*/
TEST_F(FactorGnssTdcp2dTest, gnss_1_base_antena)
{
// Create GNSS Fix capture
CaptureGnssTdcpPtr cap_gnss_ptr = std::make_shared<CaptureGnssTdcp>(TimeStamp(0), gnss_sensor_ptr, t_ecef_antena2-t_ecef_antena1, 1e-6*Matrix3d::Identity(), prior_frame_ptr);
gnss_sensor_ptr->process(cap_gnss_ptr);
// unfixing things
gnss_sensor_ptr->getP()->unfix();
// fixing things
gnss_sensor_ptr->setEnuMapYawState(o_enu_map(0)); // enu-map yaw orientation
gnss_sensor_ptr->getEnuMapYaw()->fix();
cap_gnss_ptr->getFrame()->getP()->setState(t_map_base2.head(2)); // frame position
cap_gnss_ptr->getFrame()->getP()->fix();
cap_gnss_ptr->getFrame()->getO()->setState(o_map_base2); // frame orientation
cap_gnss_ptr->getFrame()->getO()->fix();
// distort base_antena
Vector3d base_antena_dist = gnss_sensor_ptr->getP()->getState();
base_antena_dist(0) += 16.8;
base_antena_dist(0) += -40.8;
gnss_sensor_ptr->getP()->setState(base_antena_dist);
// solve for frm
std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::QUIET);
ASSERT_TRUE(cap_gnss_ptr->getFrame()->isKey());
ASSERT_MATRIX_APPROX(gnss_sensor_ptr->getP()->getState().head(2), t_base_antena.head(2), 1e-6);
}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
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