debugging

src/processor/processor_gnss_single_diff.cpp

@@ -105,15 +105,15 @@ bool ProcessorGnssSingleDiff::voteForKeyFrame() const
     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;
+    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 ((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;
+    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();

test/gtest_factor_gnss_single_diff_2D.cpp

@@ -70,10 +70,13 @@ class FactorGnssSingleDiff2DTest : public testing::Test
             gnss_sensor_ptr->setEnuEcef(R_ecef_enu.transpose(), R_ecef_enu.transpose()*(-t_ecef_enu));
 
             // gnss processor
-            ProcessorParamsBasePtr gnss_params_ptr = std::make_shared<ProcessorParamsBase>();
+            ProcessorParamsGnssSingleDiffPtr gnss_params_ptr = std::make_shared<ProcessorParamsGnssSingleDiff>();
             gnss_params_ptr->time_tolerance = 1.0;
             gnss_params_ptr->voting_active = true;
             gnss_params_ptr->voting_aux_active = false;
+            gnss_params_ptr->time_th = 0;
+            gnss_params_ptr->dist_traveled = 0;
+            gnss_params_ptr->enu_map_init_dist_min = 0;
             problem_ptr->installProcessor("GNSS SINGLE DIFFERENCES", "gnss single difference", gnss_sensor_ptr, gnss_params_ptr);
 
             // odom sensor & processor
@@ -166,8 +169,10 @@ TEST_F(FactorGnssSingleDiff2DTest, gnss_1_map_base_position)
  */
 TEST_F(FactorGnssSingleDiff2DTest, gnss_1_map_base_orientation)
 {
+    ASSERT_TRUE(prior_frame_ptr != nullptr);
     // Create GNSS Fix capture
     CaptureGnssSingleDiffPtr cap_gnss_ptr = std::make_shared<CaptureGnssSingleDiff>(TimeStamp(0), gnss_sensor_ptr, t_ecef_antena2-t_ecef_antena1, 1e-6*Matrix3s::Identity(), prior_frame_ptr);
+    ASSERT_TRUE(cap_gnss_ptr->getOriginFrame() != nullptr);
     gnss_sensor_ptr->process(cap_gnss_ptr);
 
     // fixing things