diff --git a/include/gnss/factor/factor_gnss_fix_2D.h b/include/gnss/factor/factor_gnss_fix_2D.h
index 9adb73314ac0cc8776579d0359855395fa22dc27..773a84d8bdc26cc02c62f3268b71503a9f0aa5f5 100644
--- a/include/gnss/factor/factor_gnss_fix_2D.h
+++ b/include/gnss/factor/factor_gnss_fix_2D.h
@@ -20,7 +20,7 @@ class FactorGnssFix2D : public FactorAutodiff<FactorGnssFix2D, 3, 2, 1, 3, 3, 1,
public:
FactorGnssFix2D(FeatureBasePtr& _ftr_ptr, const SensorGnssPtr& _sensor_gnss_ptr, const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
- FactorAutodiff<FactorGnssFix2D, 3, 2, 1, 3, 3, 1, 1, 1>("GNSS FIX 2D",
+ FactorAutodiff<FactorGnssFix2D, 3, 2, 1, 3, 3, 1, 1, 1>("FactorGnssFix2D",
nullptr,
nullptr,
nullptr,
diff --git a/include/gnss/factor/factor_gnss_fix_3D.h b/include/gnss/factor/factor_gnss_fix_3D.h
index 4f47dad9fe30e61dda3c60ccfb70ccee12ac8155..c620ee58a664234dfe4f42de64ab7bab09dc94d5 100644
--- a/include/gnss/factor/factor_gnss_fix_3D.h
+++ b/include/gnss/factor/factor_gnss_fix_3D.h
@@ -19,7 +19,7 @@ class FactorGnssFix3D : public FactorAutodiff<FactorGnssFix3D, 3, 3, 4, 3, 3, 1,
public:
FactorGnssFix3D(FeatureBasePtr& _ftr_ptr, const SensorGnssPtr& _sensor_gnss_ptr, const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
- FactorAutodiff<FactorGnssFix3D, 3, 3, 4, 3, 3, 1, 1, 1>("GNSS FIX 3D",
+ FactorAutodiff<FactorGnssFix3D, 3, 3, 4, 3, 3, 1, 1, 1>("FactorGnssFix3D",
nullptr,
nullptr,
nullptr,
diff --git a/include/gnss/feature/feature_gnss_fix.h b/include/gnss/feature/feature_gnss_fix.h
index 0184490f96fcda1fcadbb97ffc12ef872a1a65ff..f33fb85571ab94f448eb9a6a5eb705911735c4ca 100644
--- a/include/gnss/feature/feature_gnss_fix.h
+++ b/include/gnss/feature/feature_gnss_fix.h
@@ -22,7 +22,7 @@ class FeatureGnssFix : public FeatureBase
*
*/
FeatureGnssFix(const Eigen::Vector3s& _measurement, const Eigen::Matrix3s& _meas_covariance) :
- FeatureBase("GNSS FIX", _measurement, _meas_covariance)
+ FeatureBase("FeatureGnssFix", _measurement, _meas_covariance)
{};
virtual ~FeatureGnssFix(){};
diff --git a/src/capture/capture_gnss_fix.cpp b/src/capture/capture_gnss_fix.cpp
index 34f76594d5cff7256726e9eb663bf6ca0b1f0c1d..d7f78a9a1b4915e9a096d0e1e9aad03d96caaacb 100644
--- a/src/capture/capture_gnss_fix.cpp
+++ b/src/capture/capture_gnss_fix.cpp
@@ -4,7 +4,7 @@
namespace wolf {
CaptureGnssFix::CaptureGnssFix(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance) :
- CaptureBase("GNSS FIX", _ts, _sensor_ptr),
+ CaptureBase("CaptureGnssFix", _ts, _sensor_ptr),
data_(_data),
data_covariance_(_data_covariance)
{
diff --git a/src/processor/processor_gnss_fix.cpp b/src/processor/processor_gnss_fix.cpp
index a7ce606d8f6841a4e59cfe74d16137af91d1b01e..26b58a4e623762586fd17ce678e9b84ee6ac46f4 100644
--- a/src/processor/processor_gnss_fix.cpp
+++ b/src/processor/processor_gnss_fix.cpp
@@ -8,7 +8,7 @@ namespace wolf
{
ProcessorGnssFix::ProcessorGnssFix(ProcessorParamsGnssFixPtr _params_gnss) :
- ProcessorBase("GNSS FIX", _params_gnss),
+ ProcessorBase("ProcessorGnssFix", _params_gnss),
params_gnss_(_params_gnss),
first_capture_(nullptr)
@@ -182,5 +182,5 @@ ProcessorBasePtr ProcessorGnssFix::create(const std::string& _unique_name, const
// Register in the SensorFactory
#include "core/processor/processor_factory.h"
namespace wolf {
-WOLF_REGISTER_PROCESSOR("GNSS FIX",ProcessorGnssFix)
+WOLF_REGISTER_PROCESSOR("ProcessorGnssFix",ProcessorGnssFix)
} // namespace wolf
diff --git a/src/processor/processor_gnss_single_diff.cpp b/src/processor/processor_gnss_single_diff.cpp
index cb41fabb8f177605f32f8a9cdd53a2af32b8d2d3..3636ccd732e6bd405fb19540afd3b7ca2b29cb45 100644
--- a/src/processor/processor_gnss_single_diff.cpp
+++ b/src/processor/processor_gnss_single_diff.cpp
@@ -105,15 +105,15 @@ bool ProcessorGnssSingleDiff::voteForKeyFrame() const
if (last_KF_==nullptr)
return true;
- std::cout << "params_gnss_->time_th" << params_gnss_->time_th << std::endl;
- std::cout << "(last_KF_->getTimeStamp() - incoming_capture_->getTimeStamp())" << (last_KF_->getTimeStamp() - incoming_capture_->getTimeStamp()) << std::endl;
+ std::cout << "params_gnss_->time_th = " << params_gnss_->time_th << std::endl;
+ std::cout << "(last_KF_->getTimeStamp() - incoming_capture_->getTimeStamp()) = " << (last_KF_->getTimeStamp() - incoming_capture_->getTimeStamp()) << std::endl;
// Depending on time since the last KF with gnssfix capture
if ((incoming_capture_->getTimeStamp() - last_KF_->getTimeStamp()) > params_gnss_->time_th)
return true;
// Distance criterion
- std::cout << "params_gnss_->dist_traveled" << params_gnss_->dist_traveled << std::endl;
+ std::cout << "params_gnss_->dist_traveled = " << params_gnss_->dist_traveled << std::endl;
Eigen::Vector2s v_current_origin = (sensor_gnss_->getREnuMap().transpose() * sensor_gnss_->getREnuEcef() * incoming_capture_->getData()).head<2>();
std::cout << "v_current_origin: " << v_current_origin.transpose() << std::endl;
Eigen::Vector2s v_origin_last_KF = last_KF_->getP()->getState() - incoming_capture_->getOriginFrame()->getP()->getState();
diff --git a/src/sensor/sensor_gnss.cpp b/src/sensor/sensor_gnss.cpp
index d422362c3300eed41696dd290a164dc9a2b01505..5beaf5097f9af77fece9da0a2de8abeb21266b19 100644
--- a/src/sensor/sensor_gnss.cpp
+++ b/src/sensor/sensor_gnss.cpp
@@ -15,7 +15,7 @@ SensorGnss::SensorGnss(StateBlockPtr _p_ptr, //GNSS antena 3D positi
StateBlockPtr _bias_ptr, //GNSS sensor time bias
IntrinsicsGnssPtr params) //sensor params
:
- SensorBase("GNSS", _p_ptr, nullptr, _bias_ptr, 0), // antena orientation has no sense in GNSS
+ SensorBase("SensorGnss", _p_ptr, nullptr, _bias_ptr, 0), // antena orientation has no sense in GNSS
params_(params),
ENU_defined_(false),
ENU_MAP_initialized_(false)
@@ -38,7 +38,7 @@ SensorGnss::SensorGnss(StateBlockPtr _p_ptr, //GNSS sensor position
StateBlockPtr _pitch_ENU_MAP_ptr, //ENU_MAP Pitch
StateBlockPtr _yaw_ENU_MAP_ptr) //ENU_MAP Yaw
:
- SensorBase("GNSS", _p_ptr, nullptr, _bias_ptr, 0),// antena orientation has no sense in GNSS
+ SensorBase("SensorGnss", _p_ptr, nullptr, _bias_ptr, 0),// antena orientation has no sense in GNSS
params_(params),
ENU_defined_(false),
ENU_MAP_initialized_(false)
@@ -273,5 +273,5 @@ SensorBasePtr SensorGnss::create(const std::string& _unique_name, const Eigen::V
// Register in the SensorFactory
#include "core/sensor/sensor_factory.h"
namespace wolf {
-WOLF_REGISTER_SENSOR("GNSS",SensorGnss)
+WOLF_REGISTER_SENSOR("SensorGnss",SensorGnss)
} // namespace wolf
diff --git a/test/gtest_factor_gnss_fix_2D.cpp b/test/gtest_factor_gnss_fix_2D.cpp
index 61a1780a8626d9618c3551f8a712c7adf4ff2e7c..762e61fb4855437b330f2b7f90e0a2349bc9a007 100644
--- a/test/gtest_factor_gnss_fix_2D.cpp
+++ b/test/gtest_factor_gnss_fix_2D.cpp
@@ -90,7 +90,7 @@ Vector3s t_ecef_antena = R_ecef_enu * (R_enu_map * (R_map_base * t_base_antena +
// WOLF
ProblemPtr problem_ptr = Problem::create("PO", 2);
CeresManagerPtr ceres_mgr_ptr = std::make_shared<CeresManager>(problem_ptr);
-SensorGnssPtr gnss_sensor_ptr = std::static_pointer_cast<SensorGnss>(problem_ptr->installSensor("GNSS", "gnss", t_base_antena, std::make_shared<IntrinsicsGnss>()));
+SensorGnssPtr gnss_sensor_ptr = std::static_pointer_cast<SensorGnss>(problem_ptr->installSensor("SensorGnss", "gnss", t_base_antena, std::make_shared<IntrinsicsGnss>()));
FrameBasePtr frame_ptr;
////////////////////////////////////////////////////////
@@ -109,7 +109,7 @@ TEST(FactorGnssFix2DTest, configure_tree)
ProcessorParamsGnssFixPtr gnss_params_ptr = std::make_shared<ProcessorParamsGnssFix>();
gnss_params_ptr->time_tolerance = 1.0;
gnss_params_ptr->voting_active = true;
- problem_ptr->installProcessor("GNSS FIX", "gnss fix", gnss_sensor_ptr, gnss_params_ptr);
+ problem_ptr->installProcessor("ProcessorGnssFix", "gnss fix", gnss_sensor_ptr, gnss_params_ptr);
// Emplace a frame (FIXED)
Vector3s frame_pose = (Vector3s() << t_map_base(0), t_map_base(1), o_map_base(0)).finished();
diff --git a/test/gtest_factor_gnss_single_diff_2D.cpp b/test/gtest_factor_gnss_single_diff_2D.cpp
index 3ce5c0156b6a6416e6690d81bfaf9ca76e8aafb5..dcabb00dfaa991e6e54f759ce36852bf5531a158 100644
--- a/test/gtest_factor_gnss_single_diff_2D.cpp
+++ b/test/gtest_factor_gnss_single_diff_2D.cpp
@@ -64,30 +64,33 @@ class FactorGnssSingleDiff2DTest : public testing::Test
ceres_mgr_ptr->getSolverOptions().max_num_iterations = 10;
// gnss sensor
- gnss_sensor_ptr = std::static_pointer_cast<SensorGnss>(problem_ptr->installSensor("GNSS", "gnss", t_base_antena, std::make_shared<IntrinsicsBase>()));
+ gnss_sensor_ptr = std::static_pointer_cast<SensorGnss>(problem_ptr->installSensor("SensorGnss", "gnss", t_base_antena, std::make_shared<IntrinsicsBase>()));
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
- ProcessorParamsBasePtr gnss_params_ptr = std::make_shared<ProcessorParamsBase>();
+ ProcessorParamsGnssSingleDiffPtr gnss_params_ptr = std::make_shared<ProcessorParamsGnssSingleDiff>();
gnss_params_ptr->time_tolerance = 1.0;
gnss_params_ptr->voting_active = true;
gnss_params_ptr->voting_aux_active = false;
+ gnss_params_ptr->time_th = 0;
+ gnss_params_ptr->dist_traveled = 0;
+ gnss_params_ptr->enu_map_init_dist_min = 0;
problem_ptr->installProcessor("GNSS SINGLE DIFFERENCES", "gnss single difference", gnss_sensor_ptr, gnss_params_ptr);
// odom sensor & processor
IntrinsicsOdom2DPtr odom_intrinsics_ptr = std::make_shared<IntrinsicsOdom2D>();
odom_intrinsics_ptr->k_disp_to_disp = 0.1;
odom_intrinsics_ptr->k_rot_to_rot = 0.1;
- odom_sensor_ptr = std::static_pointer_cast<SensorOdom2D>(problem_ptr->installSensor("ODOM 2D", "odometer", VectorXs::Zero(3), odom_intrinsics_ptr));
+ odom_sensor_ptr = std::static_pointer_cast<SensorOdom2D>(problem_ptr->installSensor("SensorOdom2D", "odometer", VectorXs::Zero(3), odom_intrinsics_ptr));
ProcessorParamsOdom2DPtr odom_params_ptr = std::make_shared<ProcessorParamsOdom2D>();
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("ODOM 2D", "main odometry", odom_sensor_ptr, odom_params_ptr);
+ problem_ptr->installProcessor("ProcessorOdom2D", "main odometry", odom_sensor_ptr, odom_params_ptr);
//problem_ptr->setProcessorMotion("main odometry");
// set prior (FIXED)
@@ -122,6 +125,7 @@ class FactorGnssSingleDiff2DTest : public testing::Test
TEST_F(FactorGnssSingleDiff2DTest, check_tree)
{
ASSERT_TRUE(problem_ptr->check(0));
+ ASSERT_TRUE(prior_frame_ptr != nullptr);
}
/*
@@ -165,8 +169,10 @@ TEST_F(FactorGnssSingleDiff2DTest, gnss_1_map_base_position)
*/
TEST_F(FactorGnssSingleDiff2DTest, gnss_1_map_base_orientation)
{
+ ASSERT_TRUE(prior_frame_ptr != nullptr);
// Create GNSS Fix capture
CaptureGnssSingleDiffPtr cap_gnss_ptr = std::make_shared<CaptureGnssSingleDiff>(TimeStamp(0), gnss_sensor_ptr, t_ecef_antena2-t_ecef_antena1, 1e-6*Matrix3s::Identity(), prior_frame_ptr);
+ ASSERT_TRUE(cap_gnss_ptr->getOriginFrame() != nullptr);
gnss_sensor_ptr->process(cap_gnss_ptr);
// fixing things
@@ -174,7 +180,6 @@ TEST_F(FactorGnssSingleDiff2DTest, gnss_1_map_base_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
Vector1s frm_dist = cap_gnss_ptr->getFrame()->getO()->getState();
frm_dist(0) += 1.8;