work on the test

test/gtest_imu_static_init.cpp

@@ -31,7 +31,7 @@ class ProcessorImuStaticInitTest : public testing::Test
         double dt;
         Vector6d data;
         Matrix6d data_cov;
-        FrameBasePtr KF0;
+        FrameBasePtr KF0_;
         FrameBasePtr first_frame_;
         FrameBasePtr last_frame_;
         SolverCeresPtr solver_;
@@ -51,8 +51,8 @@ class ProcessorImuStaticInitTest : public testing::Test
         // Wolf problem
         problem_ptr_ = Problem::create("POV", 3);
         Vector7d extrinsics = (Vector7d() << 0,0,0,0,0,0,1).finished();
-        sensor_ptr_ = problem_ptr_->installSensor("SensorImu", "Main Imu", extrinsics,  wolf_root + "/demos/sensor_imu.yaml");
-        ProcessorBasePtr processor_ptr = problem_ptr_->installProcessor("ProcessorImu", "Imu pre-integrator", "Main Imu", wolf_root + "/demos/imu_static_init.yaml");
+        sensor_ptr_ = problem_ptr_->installSensor("SensorImu", "Main Imu", extrinsics,  wolf_root + "/test/yaml/sensor_imu_static_init.yaml");
+        ProcessorBasePtr processor_ptr = problem_ptr_->installProcessor("ProcessorImu", "Imu pre-integrator", "Main Imu", wolf_root + "/test/yaml/imu_static_init.yaml");
         processor_motion_ = std::static_pointer_cast<ProcessorMotion>(processor_ptr);
 
         // Time and data variables
@@ -60,7 +60,7 @@ class ProcessorImuStaticInitTest : public testing::Test
         t0.set(0);
         t = t0;
         data = Vector6d::Random();
-        data_cov = Matrix6d::Identity() * 1e-3;
+        data_cov = Matrix6d::Identity();
         last_frame_ = nullptr;
         first_frame_ = nullptr;
 
@@ -70,37 +70,44 @@ class ProcessorImuStaticInitTest : public testing::Test
         // Set the origin
         VectorComposite x0c("POV", {Vector3d::Zero(), (Vector4d() << 0,0,0,1).finished(), Vector3d::Zero()});
         WOLF_INFO("x0c is: \n", x0c);
-        KF0 = problem_ptr_->setPriorFix(x0c, t0, dt/2);
-        processor_motion_->setOrigin(KF0);
+        //KF0_ = problem_ptr_->setPriorFix(x0c, t0, dt/2);
+
+        Vector4d q_init; q_init << 0,0,0,1;
+        VectorComposite x_origin("POV", {Vector3d::Zero(), q_init, Vector3d::Zero()});
+        VectorComposite std_origin("POV", {0.001*Vector3d::Ones(), 0.001*Vector3d::Ones(), 0.001*Vector3d::Ones()});
+        KF0_ = problem_ptr_->setPriorFactor(x_origin, std_origin, 0, 0.01);
+
+        processor_motion_->setOrigin(KF0_);
     }
 
     void TestStatic(const Vector6d& body_magnitudes, const Vector6d& bias_groundtruth, const Vector6d& bias_initial_guess, const string& test_name, int testing_var)
     {
       //Data
-      data = body_magnitudes + bias_groundtruth;
-      data.head(3) -= Quaterniond(Vector4d(KF0->getO()->getState())).conjugate() * wolf::gravity();
+      data = body_magnitudes;
+      data.head(3) -= Quaterniond(Vector4d(KF0_->getO()->getState())).conjugate() * wolf::gravity();
+      data += bias_groundtruth;
 
       //Set bias initial guess
       sensor_ptr_->getIntrinsic(t0)->setState(bias_initial_guess);
 
 #if WRITE_CSV_FILE
-    std::fstream file_est; 
-    file_est.open("./est-"+test_name+".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;
-    if(testing_var == 0) header_est = "t;px;vx;ox;bax_est;bgx_est;bax_preint;bgx_preint\n";
-    if(testing_var == 1) header_est = "t;py;vy;oy;bay_est;bgy_est;bay_preint;bgy_preint\n";
-    if(testing_var == 2) header_est = "t;pz;vz;oz:baz_est;bgz_est;baz_preint;bgz_preint\n";
-    file_est << header_est;
+      std::fstream file_est; 
+      file_est.open("./est-"+test_name+".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;
+      if(testing_var == 0) header_est = "t;px;vx;ox;bax_est;bgx_est;bax_preint;bgx_preint\n";
+      if(testing_var == 1) header_est = "t;py;vy;oy;bay_est;bgy_est;bay_preint;bgy_preint\n";
+      if(testing_var == 2) header_est = "t;pz;vz;oz:baz_est;bgz_est;baz_preint;bgz_preint\n";
+      file_est << header_est;
 #endif
 
 
       int n_frames = 0;
-      for(t = t0+dt; t <= t0 + 3 + dt/2; t+=dt){
+      for(t = t0; t <= t0 + 3.9 + dt/2; t+=dt){
         WOLF_INFO("\n------------------------------------------------------------------------");
 
         //Create and process capture  
-        auto C = std::make_shared<CaptureImu>(t, sensor_ptr_, data, data_cov);
+        auto C = std::make_shared<CaptureImu>(t, sensor_ptr_, data, data_cov, KF0_->getCaptureList().front());
         C->process();
 
         auto state = problem_ptr_->getState();
@@ -114,8 +121,8 @@ class ProcessorImuStaticInitTest : public testing::Test
             << state['V'](testing_var) << ";"
             << state['O'](testing_var) << ";"
             << bias_est(testing_var) << ";"
-            << bias_preint(testing_var) << ";"
             << bias_est(testing_var+3) << ";"
+            << bias_preint(testing_var) << ";"
             << bias_preint(testing_var+3) << "\n";
 #endif
 
@@ -126,32 +133,35 @@ class ProcessorImuStaticInitTest : public testing::Test
           last_frame_ = processor_motion_->getOrigin()->getFrame();
 
           // impose static
-          last_frame_->getP()->setState(KF0->getP()->getState());
-          last_frame_->getO()->setState(KF0->getO()->getState());
+          last_frame_->getP()->setState(KF0_->getP()->getState());
+          last_frame_->getO()->setState(KF0_->getO()->getState());
           last_frame_->getV()->setZero();
 
           //Fix frame
           last_frame_->fix();
 
           //Solve
-          std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL);
-          //WOLF_INFO("Solver Report: ", report);
+          if(n_frames > 0)
+          {
+            std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL);
+            //WOLF_INFO("Solver Report: ", report);
 
-          state = problem_ptr_->getState();
-          bias_est = sensor_ptr_->getIntrinsic()->getState();
-          bias_preint = processor_motion_->getLast()->getCalibrationPreint();
+            state = problem_ptr_->getState();
+            bias_est = sensor_ptr_->getIntrinsic()->getState();
+            bias_preint = processor_motion_->getLast()->getCalibrationPreint();
 
 #if WRITE_CSV_FILE
-          // post-solve print to CSV with time-stamp shifted by dt/2 to separate from pre-solve  result
-          file_est << std::fixed << t+dt/2 << ";"
-              << state['P'](testing_var) << ";"
-              << state['V'](testing_var) << ";"
-              << state['O'](testing_var) << ";"
-              << bias_est(testing_var) << ";"
-              << bias_preint(testing_var) << ";"
-              << bias_est(testing_var+3) << ";"
-              << bias_preint(testing_var+3) << "\n";
+            // post-solve print to CSV with time-stamp shifted by dt/2 to separate from pre-solve  result
+            file_est << std::fixed << t+dt/2 << ";"
+                << state['P'](testing_var) << ";"
+                << state['V'](testing_var) << ";"
+                << state['O'](testing_var) << ";"
+                << bias_est(testing_var) << ";"
+                << bias_est(testing_var+3) << ";"
+                << bias_preint(testing_var) << ";"
+                << bias_preint(testing_var+3) << "\n";
 #endif
+          }
 
         }
 
@@ -165,14 +175,14 @@ class ProcessorImuStaticInitTest : public testing::Test
         WOLF_INFO("The preintegrated bias is: ", bias_preint.transpose());
         if(n_frames == 2)
         {
-          //ASSERT_MATRIX_APPROX(state.vector("POV"), KF0->getState().vector("POV"), 1e-4);
+          //ASSERT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-4);
           //ASSERT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6);
         }
         else if (n_frames > 2)
         {
-          ASSERT_MATRIX_APPROX(state.vector("POV"), KF0->getState().vector("POV"), 1e-6);
-          ASSERT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6);
-          ASSERT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-6);
+          EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6);
+          EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6);
+          EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-6);
         }
       }
 
@@ -188,7 +198,7 @@ class ProcessorImuStaticInitTest : public testing::Test
 TEST_F(ProcessorImuStaticInitTest, static_bias_aX_initial_guess_zero)
 {
    Vector6d body_magnitudes = Vector6d::Zero();
-   Vector6d bias_groundtruth = (Vector6d() << 0.1, 0, 0, 0, 0, 0).finished(); 
+   Vector6d bias_groundtruth = (Vector6d() << 0.42, 0, 0, 0, 0, 0).finished(); 
    Vector6d bias_initial_guess = Vector6d::Zero();
 
    TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_aX_initial_guess_zero", 0); 
@@ -198,7 +208,7 @@ TEST_F(ProcessorImuStaticInitTest, static_bias_zero_initial_guess_aX)
 {
    Vector6d body_magnitudes = Vector6d::Zero();
    Vector6d bias_groundtruth = Vector6d::Zero();
-   Vector6d bias_initial_guess = (Vector6d() << 0.1, 0, 0, 0, 0, 0).finished(); 
+   Vector6d bias_initial_guess = (Vector6d() << 0.42, 0, 0, 0, 0, 0).finished(); 
 
    TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_aX", 0); 
 }

demos/imu_static_init.yaml → test/yaml/imu_static_init.yaml

 type: "ProcessorImu"              # This must match the KEY used in the SensorFactory. Otherwise it is an error.
 
 time_tolerance: 0.0025         # Time tolerance for joining KFs
-unmeasured_perturbation_std: 0.00001
+unmeasured_perturbation_std: 0.0001
 
 keyframe_vote:
     voting_active:      true
     voting_aux_active:  false
-    max_time_span:      1.0   # seconds
-    max_buff_length:  20000   # motion deltas
-    dist_traveled:      999   # meters
-    angle_turned:       999   # radians (1 rad approx 57 deg, approx 60 deg)
+    max_time_span:      0.9999   # seconds
+    max_buff_length:    1000000000   # motion deltas
+    dist_traveled:      100000000000   # meters
+    angle_turned:       10000000000   # radians (1 rad approx 57 deg, approx 60 deg)

test/yaml/sensor_imu.yaml

+type: "SensorImu"             # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+
+motion_variances: 
+    a_noise:                0.053     # standard deviation of Acceleration noise (same for all the axis) in m/s2
+    w_noise:                0.0011    # standard deviation of Gyroscope noise (same for all the axis) in rad/sec
+    ab_initial_stdev:       0.800     # m/s2    - initial bias 
+    wb_initial_stdev:       0.350     # rad/sec - initial bias 
+    ab_rate_stdev:          0.1       # m/s2/sqrt(s)           
+    wb_rate_stdev:          0.0400    # rad/s/sqrt(s)

test/yaml/sensor_imu_static_init.yaml

+type: "SensorImu"             # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+
+motion_variances: 
+    a_noise:                0.001     # standard deviation of Acceleration noise (same for all the axis) in m/s2
+    w_noise:                0.001    # standard deviation of Gyroscope noise (same for all the axis) in rad/sec
+    ab_initial_stdev:       0.001     # m/s2    - initial bias 
+    wb_initial_stdev:       0.001     # rad/sec - initial bias 
+    ab_rate_stdev:          0.001       # m/s2/sqrt(s)           
+    wb_rate_stdev:          0.001    # rad/s/sqrt(s)