bug fixed

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