Make CSV file optional, disable by default

test/gtest_processor_motion_intrinsics_update.cpp

@@ -151,10 +151,11 @@ TEST_F(ProcessorImuTest, test1)
 
 
 
+//#define WRITE_CSV_FILE // COMMENT OUT THIS LINE TO AVOID PRODUCING CSV FILE
 TEST_F(ProcessorImuTest, getState)
 {
     Matrix6d data_cov; data_cov. setIdentity();
-    // Advances at constant speed on both wheels
+    // Advances at constant acceleration
     double AX = 0;
 
     problem_->print(4,true,true,true);
@@ -165,33 +166,33 @@ TEST_F(ProcessorImuTest, getState)
     double dt(0.1);
     int nb_kf = 1;
 
-
+#ifdef WRITE_CSV_FILE
     std::fstream file_est; 
     file_est.open("./est.csv", std::fstream::out);
 //    std::string header_est = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz,bax_est,bax_preint\n";
     std::string header_est = "t;px;vx;bax_est;bax_preint\n";
     file_est << header_est;
-
+#endif
 
     while (t < 4){
         auto C = std::make_shared<CaptureImu>(t, sensor_, data, data_cov, KF0_->getCaptureList().front()); 
         C->process();
-        VectorComposite state = problem_->getState(t);
-        VectorXd calib_estim = sensor_->getIntrinsic(t)->getState();
-        VectorXd calib_preint = processor_->getLast()->getCalibrationPreint();
-        std::cout << "calib size: " << calib_estim.size() << std::endl;
-        std::cout << "calib_preint size: " << calib_preint.size() << std::endl;
+
 
         std::cout << "t " << t << "; cap id " << sensor_->findLastCaptureBefore(t) << "; cap ts " << sensor_->findLastCaptureBefore(t)->getTimeStamp() << std::endl;
 
 
+#ifdef WRITE_CSV_FILE
         // pre-solve print to CSV
+        VectorComposite state = problem_->getState(t);
+        VectorXd calib_estim = sensor_->getIntrinsic(t)->getState();
+        VectorXd calib_preint = processor_->getLast()->getCalibrationPreint();
         file_est << std::fixed << t << ";"
             << state['P'](0) << ";"
             << state['V'](0) << ";"
             << calib_estim(0) << ";"
             << calib_preint(0) << "\n";
-
+#endif
 
 
         if (problem_->getTrajectory()->getFrameMap().size() > nb_kf){
@@ -201,23 +202,28 @@ TEST_F(ProcessorImuTest, getState)
 
             solver_->solve();
             problem_->print(4, true, true, true);
-            nb_kf++;
 
+            nb_kf = problem_->getTrajectory()->getFrameMap().size();
+
+#ifdef WRITE_CSV_FILE
             // post-solve print to CSV with time-stamp shifted by dt/2 to separate from pre-solve  result
-            VectorComposite state = problem_->getState(t);
-            VectorXd calib_estim = sensor_->getIntrinsic(t)->getState();
-            VectorXd calib_preint = processor_->getLast()->getCalibrationPreint();
+            state = problem_->getState(t);
+            calib_estim = sensor_->getIntrinsic(t)->getState();
+            calib_preint = processor_->getLast()->getCalibrationPreint();
             file_est << std::fixed << t+dt/2 << ";"
                 << state['P'](0) << ";"
                 << state['V'](0) << ";"
                 << calib_estim(0) << ";"
                 << calib_preint(0) << "\n";
-
+#endif
         }
 
         t += dt;
     }
 
+#ifdef WRITE_CSV_FILE
+    file_est.close();
+#endif
 }