From ea18c62a972370ae9a2bb5ff27b19ced000bb115 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Thu, 19 Dec 2019 11:45:06 +0100
Subject: [PATCH] debugging
---
src/processor/processor_gnss_single_diff.cpp | 6 +++---
test/gtest_factor_gnss_single_diff_2D.cpp | 7 ++++++-
2 files changed, 9 insertions(+), 4 deletions(-)
diff --git a/src/processor/processor_gnss_single_diff.cpp b/src/processor/processor_gnss_single_diff.cpp
index cb41fabb8..3636ccd73 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/test/gtest_factor_gnss_single_diff_2D.cpp b/test/gtest_factor_gnss_single_diff_2D.cpp
index 57dc12daf..dcabb00df 100644
--- a/test/gtest_factor_gnss_single_diff_2D.cpp
+++ b/test/gtest_factor_gnss_single_diff_2D.cpp
@@ -70,10 +70,13 @@ class FactorGnssSingleDiff2DTest : public testing::Test
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
@@ -166,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
--
GitLab