Implement bootstrap strategy "G"

include/imu/processor/processor_imu.h

@@ -32,11 +32,29 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorImu);
 
 struct ParamsProcessorImu : public ParamsProcessorMotion
 {
+    bool bootstrap_enable;
+    enum BootstrapMethod 
+    {
+        BOOTSTRAP_STATIC,
+        BOOTSTRAP_G,
+        BOOTSTRAP_V0_G
+    } bootstrap_method;
+    int bootstrap_averaging_length;
+
     ParamsProcessorImu() = default;
     ParamsProcessorImu(std::string _unique_name, const ParamsServer& _server):
         ParamsProcessorMotion(_unique_name, _server)
     {
-        //
+        bootstrap_enable = _server.getParam<bool>(prefix + _unique_name + "/bootstrap_enable");
+        if (bootstrap_enable)
+        {
+            bootstrap_averaging_length = _server.getParam<int>(prefix + _unique_name + "/bootstrap_averaging_length");
+            string str                 = _server.getParam<string>(prefix + _unique_name + "/bootstrap_method");
+            std::transform(str.begin(), str.end(), str.begin(), ::toupper);
+            if (str == "STATIC" /**/) bootstrap_method = BOOTSTRAP_STATIC;
+            if (str == "G" /*     */) bootstrap_method = BOOTSTRAP_G;
+            if (str == "V0_G" /*  */) bootstrap_method = BOOTSTRAP_V0_G;
+        }
     }
     std::string print() const override
     {
@@ -95,11 +113,12 @@ class ProcessorImu : public ProcessorMotion{
         FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_motion) override;
         FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
                                             CaptureBasePtr _capture_origin) override;
+        virtual void bootstrap() override;
 
-    protected:
-        ParamsProcessorImuPtr params_motion_Imu_;
+      protected:
+        ParamsProcessorImuPtr    params_motion_Imu_;
+        std::list<FactorBasePtr> list_fac_inactive_bootstrap_;
 };
-
 }
 
 /////////////////////////////////////////////////////////

src/processor/processor_imu.cpp

@@ -33,7 +33,8 @@ ProcessorImu::ProcessorImu(ParamsProcessorImuPtr _params_motion_imu) :
         ProcessorMotion("ProcessorImu", "POV", 3, 10, 10, 9, 6, 6, _params_motion_imu),
         params_motion_Imu_(std::make_shared<ParamsProcessorImu>(*_params_motion_imu))
 {
-    //
+    bootstrapping_ = params_motion_Imu_->bootstrap_enable;
+    list_fac_inactive_bootstrap_.clear();
 }
 
 ProcessorImu::~ProcessorImu()
@@ -122,6 +123,12 @@ FactorBasePtr ProcessorImu::emplaceFactor(FeatureBasePtr _feature_motion, Captur
 
     auto fac_imu = FactorBase::emplace<FactorImu>(_feature_motion, ftr_imu, cap_imu, shared_from_this(), params_->apply_loss_function);
 
+    if (bootstrapping_) 
+    {
+        fac_imu->setStatus(FAC_INACTIVE);
+        list_fac_inactive_bootstrap_.push_back(fac_imu);
+    }
+
     return fac_imu;
 }
 
@@ -240,7 +247,71 @@ Eigen::VectorXd ProcessorImu::correctDelta (const Eigen::VectorXd& delta_preint,
     return imu::plus(delta_preint, delta_step);
 }
 
+void ProcessorImu::bootstrap()
+{
+    // TODO bootstrap strategies. 
+    // See Sola-22 "Imu bootstrap strategies" https://www.overleaf.com/project/629e276e7f68b0c2bfa469ac
+
+    VectorComposite transformation("PO");
+    bool            bootstrap_done = false;
+    switch (params_motion_Imu_->bootstrap_method)
+    {
+        case ParamsProcessorImu::BootstrapMethod::BOOTSTRAP_STATIC:
+        {
+            // TODO implement static strategy
+            break;
+        }
+        case ParamsProcessorImu::BootstrapMethod::BOOTSTRAP_G:
+        {
+            // Implementation of G strategy. 
+            if (last_ptr_->getBuffer().size() > 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
+                Quaterniond q_w_l      = Quaterniond::FromTwoVectors(g_l, g_w);  //
+                transformation.at('P') = p_w_l;                                  //
+                transformation.at('O') = q_w_l.coeffs();                         //
 
+                bootstrap_done = true;
+            }
+            break;
+        }
+        case ParamsProcessorImu::BootstrapMethod::BOOTSTRAP_V0_G:
+        {
+            // TODO implement v0-g strategy
+            break;
+        }
+        default:
+            break;
+    }
+
+    if (bootstrap_done)
+    {
+        // Transform problem to new reference
+        getProblem()->transform(transformation);
+
+        // Activate factors that were inactive during bootstrap
+        while (not list_fac_inactive_bootstrap_.empty())
+        {
+            list_fac_inactive_bootstrap_.front()->setStatus(FAC_ACTIVE);
+            list_fac_inactive_bootstrap_.pop_front();
+        }
+
+        // Clear bootstrapping flag. This marks the end of the bootstrapping process
+        bootstrapping_ = false;
+    }
+}
 
 } // namespace wolf