debugging couts

src/processor/processor_gnss_single_diff.cpp

@@ -106,13 +106,20 @@ bool ProcessorGnssSingleDiff::voteForKeyFrame()
     if (last_KF_==nullptr)
         return true;
 
+    std::cout << "params_gnss_->time_th" << params_gnss_->time_th << std::endl;
+    std::cout << "(last_KF_->getTimeStamp() - incoming_capture_->getTimeStamp())" << (last_KF_->getTimeStamp() - incoming_capture_->getTimeStamp()) << std::endl;
+
     // Depending on time since the last KF with gnssfix capture
-    if ((last_KF_->getTimeStamp() - incoming_capture_->getTimeStamp()) > params_gnss_->time_th)
+    if ((incoming_capture_->getTimeStamp() - last_KF_->getTimeStamp()) > params_gnss_->time_th)
         return true;
 
     // Distance criterion
+    std::cout << "params_gnss_->dist_traveled" << params_gnss_->dist_traveled << std::endl;
     Eigen::Vector2s v_current_origin = (sensor_gnss_->getREnuMap().transpose() * sensor_gnss_->getREnuEcef() * incoming_capture_->getData()).head<2>();
+    std::cout << "v_current_origin: " << v_current_origin.transpose() << std::endl;
     Eigen::Vector2s v_origin_last_KF = last_KF_->getP()->getState() - incoming_capture_->getOriginFrame()->getP()->getState();
+    std::cout << "v_origin_last_KF: " << v_origin_last_KF.transpose() << std::endl;
+    std::cout << "v_current_origin + v_origin_last_KF: " << (v_current_origin + v_origin_last_KF).transpose() << std::endl;
     if ((v_current_origin + v_origin_last_KF).norm() > params_gnss_->dist_traveled)
         return true;