more changes in sd

src/processor/processor_gnss_single_diff.cpp

@@ -22,8 +22,7 @@ ProcessorGnssSingleDiff::~ProcessorGnssSingleDiff()
 void ProcessorGnssSingleDiff::processCapture(CaptureBasePtr _capture)
 {
     // TODO: keep captures in a buffer and deal with KFpacks
-
-    WOLF_INFO("ProcessorGnssSingleDiff::process()");
+    WOLF_DEBUG("ProcessorGnssSingleDiff::process()");
     incoming_capture_ = std::static_pointer_cast<CaptureGnssSingleDiff>(_capture);
 
     // discard capture with null or non-key origin frame
@@ -33,7 +32,7 @@ void ProcessorGnssSingleDiff::processCapture(CaptureBasePtr _capture)
         return;
     }
 
-    if (last_KF_ == nullptr)
+    if (last_KF_ == nullptr && incoming_capture_->getOriginFrame() != nullptr && incoming_capture_->getOriginFrame()->isKey())
         last_KF_ = incoming_capture_->getOriginFrame();
 
     // NEW KF? ------------------------------------------------
@@ -81,7 +80,7 @@ void ProcessorGnssSingleDiff::processCapture(CaptureBasePtr _capture)
          incoming_capture_->getFrame() != nullptr && incoming_capture_->getFrame()->isKey() &&
          incoming_capture_->getData().norm() > params_gnss_->enu_map_init_dist_min)
     {
-        WOLF_DEBUG("initializing enu map");
+        WOLF_INFO("initializing enu map");
         sensor_gnss_->initializeEnuMapYaw(incoming_capture_->getOriginFrame()->getState(),
                                           incoming_capture_->getFrame()->getState(),
                                           incoming_capture_->getData());
@@ -111,17 +110,23 @@ bool ProcessorGnssSingleDiff::voteForKeyFrame()
 
     // Elapsed time criterion: From the last KF with gnssfix capture
     if ((incoming_capture_->getTimeStamp() - last_KF_->getTimeStamp()) > params_gnss_->time_th)
-        return true;
+    {
+    	WOLF_DEBUG("voting for KF: elapsed time criterion");
+    	return true;
+    }
 
     // Distance criterion: From the last KF with gnssfix capture
     Eigen::Vector2s v_origin_current = (sensor_gnss_->getREnuMap().transpose() * sensor_gnss_->getREnuEcef() * incoming_capture_->getData()).head<2>();
     Eigen::Vector2s v_lastKF_origin = incoming_capture_->getOriginFrame()->getP()->getState() - last_KF_->getP()->getState();
-    //std::cout << "params_gnss_->dist_traveled" << params_gnss_->dist_traveled << std::endl;
-    //std::cout << "v_origin_current: " << v_origin_current.transpose() << std::endl;
-    //std::cout << "v_lastKF_origin: " << v_lastKF_origin.transpose() << std::endl;
-    //std::cout << "v_lastKF_origin + v_origin_current: " << (v_lastKF_origin + v_origin_current).transpose() << std::endl;
+	//std::cout << "params_gnss_->dist_traveled" << params_gnss_->dist_traveled << std::endl;
+	//std::cout << "v_origin_current: " << v_origin_current.transpose() << std::endl;
+	//std::cout << "v_lastKF_origin: " << v_lastKF_origin.transpose() << std::endl;
+	//std::cout << "v_lastKF_origin + v_origin_current: " << (v_lastKF_origin + v_origin_current).transpose() << std::endl;
     if ((v_lastKF_origin + v_origin_current).norm() > params_gnss_->dist_traveled)
-        return true;
+	{
+    	WOLF_DEBUG("voting for KF: distance criterion");
+    	return true;
+	}
 
     // TODO: more alternatives?