small bug fixed

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