hotfix processorgnssfix

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;
     }