Bootstrap accounts for multip KFs. test pass

src/processor/processor_imu.cpp

@@ -261,24 +261,47 @@ void ProcessorImu::bootstrap()
             // TODO implement static strategy
             break;
         }
-        case ParamsProcessorImu::BootstrapMethod::BOOTSTRAP_G:
-        {
-            // Implementation of G strategy. 
-            if (last_ptr_->getBuffer().size() - 1 > params_motion_Imu_->bootstrap_averaging_length) 
+        case ParamsProcessorImu::BootstrapMethod::BOOTSTRAP_G: {
+            // Implementation of G strategy.
+            CaptureBasePtr first_capture;
+            if (list_fac_inactive_bootstrap_.empty()) first_capture = origin_ptr_;
+            else
+            first_capture =
+                std::static_pointer_cast<CaptureMotion>(list_fac_inactive_bootstrap_.front()->getCapture())
+                    ->getOriginCapture();
+            TimeStamp t_current = last_ptr_->getBuffer().back().ts_;
+            if (t_current - first_capture->getTimeStamp() >= params_motion_Imu_->bootstrap_averaging_length)
             {
                 // frames:
                 //   w: world global ( where g = [0,0,-9.806] );
                 //   l: world local;
                 //   r: robot;
                 //   s: sensor (IMU)
-                Quaterniond q_l_r(this->getOrigin()->getFrame()->getStateVector("O").data());
-                Quaterniond q_r_s(this->getSensor()->getStateVector("O").data());
-
-                Vector3d    delta_v    = getMotion().delta_integr_.segment(7, 3);              //
-                double      delta_t    = getMotion().ts_ - this->getOrigin()->getTimeStamp();  //
-                Vector3d    g_l        = -((q_l_r * q_r_s) * delta_v / delta_t);               // See eq. (20)
-                const auto& g_w        = gravity();                                            //
-                Vector3d    p_w_l      = Vector3d::Zero();                       // will pivot around the local origin
+
+                // get initial IMU frame 's' expressed in local world frame 'l'
+                Quaterniond q_l_r(first_capture->getFrame()->getStateVector("O").data());
+                Quaterniond q_r_s(first_capture->getSensor()->getStateVector("O").data());
+
+                // Compute total integrated delta during bootstrap period
+                // first, integrate all deltas in previous factors
+                VectorXd delta_int = deltaZero();
+                for (const auto& fac : list_fac_inactive_bootstrap_)
+                // here, we take advantage of the list of IMU factors to recover all deltas
+                {
+                    double      dt    = fac->getCapture()->getTimeStamp() - fac->getCaptureOther()->getTimeStamp();
+                    const auto& delta = fac->getFeature()->getMeasurement();  // In FeatImu, delta = measurement
+                    delta_int         = imu::compose(delta_int, delta, dt);
+                }
+                // now compose with delta in last_ptr_
+                double dt = t_current - origin_ptr_->getTimeStamp();
+                delta_int = imu::compose(delta_int, last_ptr_->getDeltaPreint(), dt);
+
+                // compute local g and transformation to global g
+                dt                     = t_current - first_capture->getTimeStamp();  //
+                Vector3d    dv         = delta_int.segment(7, 3);                    //
+                Vector3d    g_l        = -((q_l_r * q_r_s) * dv / dt);               // See eq. (20)
+                const auto& g_w        = gravity();                                  //
+                const auto& p_w_l      = Vector3d::Zero();                       // will pivot around the local origin
                 Quaterniond q_w_l      = Quaterniond::FromTwoVectors(g_l, g_w);  //
                 transformation.at('P') = p_w_l;                                  //
                 transformation.at('O') = q_w_l.coeffs();                         //
@@ -287,8 +310,7 @@ void ProcessorImu::bootstrap()
             }
             break;
         }
-        case ParamsProcessorImu::BootstrapMethod::BOOTSTRAP_V0_G:
-        {
+        case ParamsProcessorImu::BootstrapMethod::BOOTSTRAP_V0_G: {
             // TODO implement v0-g strategy
             break;
         }

test/gtest_imu.cpp

@@ -1542,7 +1542,7 @@ TEST_F(Process_Factor_Imu_ODO, RecoverTrajectory_MotionRandom_PqV_b__pqV_b) // F
 TEST_F(Process_Factor_Imu, bootstrap)
 {
     processor_imu->setVotingActive(true);
-    processor_imu->setMaxBuffLength(20);
+    processor_imu->setMaxTimeSpan(0.1);
 
     auto KF0 = problem->emplaceFrame(0.0);
     problem->keyFrameCallback(KF0,nullptr);
@@ -1553,18 +1553,18 @@ TEST_F(Process_Factor_Imu, bootstrap)
 
     capture_imu = std::make_shared<CaptureImu>(0.0, sensor_imu, data, Matrix6d::Identity());
 
-    for (t = 0.0; t < 1.0; t += dt)
+    for (t = 0; t < 0.35; t += dt)
     {
         capture_imu->setTimeStamp(t);
         capture_imu->process();
-        problem->print(4, 0, 1, 0);
+        problem->print(4, 0, 1, 1);
     }
 }
 
 int main(int argc, char **argv)
 {
     testing::InitGoogleTest(&argc, argv);
-    //    ::testing::GTEST_FLAG(filter) = "Process_Factor_Imu.bootstrap";
+    ::testing::GTEST_FLAG(filter) = "Process_Factor_Imu.bootstrap";
     //    ::testing::GTEST_FLAG(filter) = "Process_Factor_Imu_ODO.*";
     //    ::testing::GTEST_FLAG(filter) = "Process_Factor_Imu_ODO.RecoverTrajectory_MotionRandom_PqV_b__pqV_b";