From ce1b8f566cfde941408dc5b226e93e9be9a8eaed 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:14:47 +0200
Subject: [PATCH] bug fixed
---
src/processor/processor_gnss_single_diff.cpp | 14 +++++---------
1 file changed, 5 insertions(+), 9 deletions(-)
diff --git a/src/processor/processor_gnss_single_diff.cpp b/src/processor/processor_gnss_single_diff.cpp
index 231807dde..18004cc67 100644
--- a/src/processor/processor_gnss_single_diff.cpp
+++ b/src/processor/processor_gnss_single_diff.cpp
@@ -27,7 +27,7 @@ void ProcessorGnssSingleDiff::processCapture(CaptureBasePtr _capture_ptr)
incoming_capture_ptr_ = std::static_pointer_cast<CaptureGnssSingleDiff>(_capture_ptr);
// discard capture with null or non-key origin frame
- if (incoming_capture_ptr_->getOriginFrame() == nullptr || incoming_capture_ptr_->getOriginFrame()->isKey())
+ if (incoming_capture_ptr_->getOriginFrame() == nullptr || !incoming_capture_ptr_->getOriginFrame()->isKey())
return;
FrameBasePtr new_frame_ptr = nullptr;
@@ -54,14 +54,10 @@ void ProcessorGnssSingleDiff::processCapture(CaptureBasePtr _capture_ptr)
_capture_ptr->link(new_frame_ptr); // Add incoming Capture to the new Frame
// EMPLACE FEATURE
- //WOLF_DEBUG("emplacing feature...");
auto ftr_ptr = FeatureBase::emplace<FeatureGnssSingleDiff>(incoming_capture_ptr_, incoming_capture_ptr_->getData(),incoming_capture_ptr_->getDataCovariance());
// ADD CONSTRAINT
- //WOLF_DEBUG("emplacing factor...");
emplaceFactor(ftr_ptr);
-
- //WOLF_DEBUG("Factor established");
}
// INITIALIZE ENU_MAP IF 4 NECESSARY CONDITIONS:
@@ -75,10 +71,10 @@ void ProcessorGnssSingleDiff::processCapture(CaptureBasePtr _capture_ptr)
incoming_capture_ptr_->getFrame() != nullptr && incoming_capture_ptr_->getFrame()->isKey() &&
incoming_capture_ptr_->getData().norm() > params_gnss_->enu_map_init_dist_min)
{
- std::cout << "initializing enu map\n";
- sensor_gnss_ptr_->initializeEnuMapYaw(last_capture_ptr_->getOriginFrame()->getState(),
- last_capture_ptr_->getFrame()->getState(),
- last_capture_ptr_->getData());
+ WOLF_DEBUG("initializing enu map");
+ sensor_gnss_ptr_->initializeEnuMapYaw(incoming_capture_ptr_->getOriginFrame()->getState(),
+ incoming_capture_ptr_->getFrame()->getState(),
+ incoming_capture_ptr_->getData());
}
last_capture_ptr_ = incoming_capture_ptr_;
--
GitLab