From 784bdd655ff70c6be2d7f813d8b637e377b7ac6b Mon Sep 17 00:00:00 2001
From: jvallve <jvallve@iri.upc.edu>
Date: Mon, 14 Nov 2022 18:52:42 +0100
Subject: [PATCH] hotfix processorgnssfix
---
src/processor/processor_gnss_fix.cpp | 16 ++++++++--------
1 file changed, 8 insertions(+), 8 deletions(-)
diff --git a/src/processor/processor_gnss_fix.cpp b/src/processor/processor_gnss_fix.cpp
index de9eed770..6fd4e30a6 100644
--- a/src/processor/processor_gnss_fix.cpp
+++ b/src/processor/processor_gnss_fix.cpp
@@ -245,15 +245,16 @@ void ProcessorGnssFix::handleEnuMap(FeatureBasePtr feature)
// Store the first capture that established a factor (for later initialization ENU-MAP)
if (first_frame_state_.empty() and
- not sensor_gnss_->isEnuMapFixed())
+ not sensor_gnss_->isEnuMapFixed() and
+ feature->getCapture() and feature->getCapture()->getFrame())
{
- first_frame_state_ = feature->getCapture()->getFrame()->getState();
+ first_frame_state_ = feature->getCapture()->getFrame()->getState("PO");
first_pos_ = feature->getMeasurement().head<3>();
}
// Initialize ENU-MAP if: ENU defined and ENU-MAP not initialized (and not fixed) and far enough
if (params_gnss_->init_enu_map and
- not first_frame_state_.empty() and
+ first_frame_state_.includesStructure("PO") and
sensor_gnss_->isEnuDefined() and
not sensor_gnss_->isEnuMapInitialized() and
not sensor_gnss_->isEnuMapFixed() and
@@ -319,18 +320,17 @@ bool ProcessorGnssFix::voteForKeyFrame() const
sensor_gnss_->isEnuDefined() and
not sensor_gnss_->isEnuMapInitialized() and
not sensor_gnss_->isEnuMapFixed() and
- (first_pos_-incoming_feature_->getMeasurement()).norm() > params_gnss_->enu_map_init_dist_min and
- (first_pos_-incoming_feature_->getMeasurement()).norm() < params_gnss_->enu_map_init_dist_max)
+ (first_pos_-incoming_feature_->getMeasurement().head<3>()).norm() > params_gnss_->enu_map_init_dist_min and
+ (first_pos_-incoming_feature_->getMeasurement().head<3>()).norm() < params_gnss_->enu_map_init_dist_max)
{
WOLF_DEBUG("KF because of enu map not initialized");
return true;
}
// Distance criterion (ENU defined and ENU-MAP initialized)
- if (last_KF_capture_ != nullptr and
- (incoming_feature_->getMeasurement() - last_KF_feature_->getMeasurement()).norm() > params_gnss_->dist_traveled)
+ if ((incoming_feature_->getMeasurement().head<3>() - last_KF_feature_->getMeasurement().head<3>()).norm() > params_gnss_->dist_traveled)
{
- WOLF_DEBUG("KF because of distance criterion: ", (incoming_feature_->getMeasurement() - last_KF_feature_->getMeasurement()).norm());
+ WOLF_DEBUG("KF because of distance criterion: ", (incoming_feature_->getMeasurement().head<3>() - last_KF_feature_->getMeasurement().head<3>()).norm());
return true;
}
--
GitLab