From 451916344dc8b85b3c9b37d20986bba8e75f2ecd Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Fri, 12 Jul 2019 18:11:11 +0200
Subject: [PATCH] debugging couts
---
src/processor/processor_gnss_single_diff.cpp | 9 ++++++++-
1 file changed, 8 insertions(+), 1 deletion(-)
diff --git a/src/processor/processor_gnss_single_diff.cpp b/src/processor/processor_gnss_single_diff.cpp
index c8684af15..2dd0a9179 100644
--- a/src/processor/processor_gnss_single_diff.cpp
+++ b/src/processor/processor_gnss_single_diff.cpp
@@ -106,13 +106,20 @@ bool ProcessorGnssSingleDiff::voteForKeyFrame()
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;
+
// Depending on time since the last KF with gnssfix capture
- if ((last_KF_->getTimeStamp() - incoming_capture_->getTimeStamp()) > params_gnss_->time_th)
+ 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;
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();
+ std::cout << "v_origin_last_KF: " << v_origin_last_KF.transpose() << std::endl;
+ std::cout << "v_current_origin + v_origin_last_KF: " << (v_current_origin + v_origin_last_KF).transpose() << std::endl;
if ((v_current_origin + v_origin_last_KF).norm() > params_gnss_->dist_traveled)
return true;
--
GitLab