From e325ba60ff4e8b8db61ee6c70a6cdb2462c04c01 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 10:15:00 +0200
Subject: [PATCH] small bug fixed
---
src/processor/processor_gnss_fix.cpp | 15 +++++----------
1 file changed, 5 insertions(+), 10 deletions(-)
diff --git a/src/processor/processor_gnss_fix.cpp b/src/processor/processor_gnss_fix.cpp
index 92acb3115..152937a70 100644
--- a/src/processor/processor_gnss_fix.cpp
+++ b/src/processor/processor_gnss_fix.cpp
@@ -24,15 +24,9 @@ ProcessorGnssFix::~ProcessorGnssFix()
void ProcessorGnssFix::processCapture(CaptureBasePtr _capture_ptr)
{
// TODO: keep captures in a buffer and deal with KFpacks
-
//WOLF_DEBUG("ProcessorGnssFix::process()");
last_capture_ptr_ = std::static_pointer_cast<CaptureGnssFix>(_capture_ptr);
- // DEBUGGING ENU_ECEF
- //Eigen::Vector3s p = last_capture_ptr_->getData();
- //Eigen::Vector3s p_enu = sensor_gnss_ptr_->getREnuEcef()*p + sensor_gnss_ptr_->gettEnuEcef();
- //std::cout << "WOLF: gnss fix in ENU coordinates: " << p_enu.transpose() << std::endl;
-
FrameBasePtr new_frame_ptr = nullptr;
FactorBasePtr new_fac = nullptr;
@@ -58,7 +52,7 @@ void ProcessorGnssFix::processCapture(CaptureBasePtr _capture_ptr)
_capture_ptr->link(new_frame_ptr); // Add incoming Capture to the new Frame
// EMPLACE FEATURES
- //WOLF_DEBUG( "PR ", getName()," - emplacing the feature...");
+ WOLF_DEBUG( "PR ", getName()," - emplacing the feature...");
auto ftr_ptr = FeatureBase::emplace<FeatureGnssFix>(last_capture_ptr_, last_capture_ptr_->getData(),last_capture_ptr_->getDataCovariance());
// EMPLACE FACTOR
@@ -74,11 +68,12 @@ void ProcessorGnssFix::processCapture(CaptureBasePtr _capture_ptr)
last_gnss_fix_KF_= new_frame_ptr;
}
- // INITIALIZE ENU IF:
+ // SET ECEF_ENU IF:
// 1 - not initialized
// 2 - factor established
if (!sensor_gnss_ptr_->isEnuDefined() && new_fac != nullptr)
{
+ WOLF_DEBUG("setting ecef enu");
sensor_gnss_ptr_->setEcefEnu(last_capture_ptr_->getData());
// Store the first capture that established a factor
@@ -93,10 +88,10 @@ void ProcessorGnssFix::processCapture(CaptureBasePtr _capture_ptr)
if ( sensor_gnss_ptr_->isEnuDefined() &&
!sensor_gnss_ptr_->isEnuMapInitialized() &&
new_fac != nullptr &&
- first_capture_ptr_->getFrame() != nullptr && last_capture_ptr_->getFrame() != nullptr &&
+ first_capture_ptr_ != nullptr && first_capture_ptr_->getFrame() != nullptr && last_capture_ptr_->getFrame() != nullptr &&
(first_capture_ptr_->getFrame()->getState()-last_capture_ptr_->getFrame()->getState()).norm() > params_gnss_->enu_map_init_dist_min)
{
- std::cout << "initializing enu map\n";
+ WOLF_DEBUG("initializing enu map");
sensor_gnss_ptr_->initializeEnuMap(first_capture_ptr_->getFrame()->getState(), first_capture_ptr_->getData(),
last_capture_ptr_->getFrame()->getState(), last_capture_ptr_->getData());
}
--
GitLab