From 88806bdf984717b1ba92b87be3cff1b352c794c3 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Sat, 18 Jun 2022 14:04:50 +0200
Subject: [PATCH] Extract local methods

---
 include/imu/processor/processor_imu.h | 81 ++++++++++++++-------------
 src/processor/processor_imu.cpp       | 54 +++++++++++-------
 2 files changed, 75 insertions(+), 60 deletions(-)

diff --git a/include/imu/processor/processor_imu.h b/include/imu/processor/processor_imu.h
index e7b6b9549..ff144eda4 100644
--- a/include/imu/processor/processor_imu.h
+++ b/include/imu/processor/processor_imu.h
@@ -86,46 +86,47 @@ class ProcessorImu : public ProcessorMotion{
         WOLF_PROCESSOR_CREATE(ProcessorImu, ParamsProcessorImu);
         void preProcess() override;
 
-    protected:
-        void computeCurrentDelta(const Eigen::VectorXd& _data,
-                                         const Eigen::MatrixXd& _data_cov,
-                                         const Eigen::VectorXd& _calib,
-                                         const double _dt,
-                                         Eigen::VectorXd& _delta,
-                                         Eigen::MatrixXd& _delta_cov,
-                                         Eigen::MatrixXd& _jacobian_calib) const override;
-        void deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
-                                    const Eigen::VectorXd& _delta,
-                                    const double _dt,
-                                    Eigen::VectorXd& _delta_preint_plus_delta) const override;
-        void deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
-                                    const Eigen::VectorXd& _delta,
-                                    const double _dt,
-                                    Eigen::VectorXd& _delta_preint_plus_delta,
-                                    Eigen::MatrixXd& _jacobian_delta_preint,
-                                    Eigen::MatrixXd& _jacobian_delta) const override;
-        void statePlusDelta(const VectorComposite& _x,
-                                    const Eigen::VectorXd& _delta,
-                                    const double _Dt,
-                                    VectorComposite& _x_plus_delta) const override;
-        Eigen::VectorXd deltaZero() const override;
-        Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
-                                             const Eigen::VectorXd& delta_step) const override;
-        VectorXd getCalibration (const CaptureBaseConstPtr _capture = nullptr) const override;
-        void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
-        bool voteForKeyFrame() const override;
-        CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
-                                                const SensorBasePtr& _sensor,
-                                                const TimeStamp& _ts,
-                                                const VectorXd& _data,
-                                                const MatrixXd& _data_cov,
-                                                const VectorXd& _calib,
-                                                const VectorXd& _calib_preint,
-                                                const CaptureBasePtr& _capture_origin) override;
-        FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_motion) override;
-        FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
-                                            CaptureBasePtr _capture_origin) override;
-        virtual void bootstrap() override;
+      protected:
+        void             computeCurrentDelta(const Eigen::VectorXd& _data,
+                                             const Eigen::MatrixXd& _data_cov,
+                                             const Eigen::VectorXd& _calib,
+                                             const double           _dt,
+                                             Eigen::VectorXd&       _delta,
+                                             Eigen::MatrixXd&       _delta_cov,
+                                             Eigen::MatrixXd&       _jacobian_calib) const override;
+        void             deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
+                                        const Eigen::VectorXd& _delta,
+                                        const double           _dt,
+                                        Eigen::VectorXd&       _delta_preint_plus_delta) const override;
+        void             deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
+                                        const Eigen::VectorXd& _delta,
+                                        const double           _dt,
+                                        Eigen::VectorXd&       _delta_preint_plus_delta,
+                                        Eigen::MatrixXd&       _jacobian_delta_preint,
+                                        Eigen::MatrixXd&       _jacobian_delta) const override;
+        void             statePlusDelta(const VectorComposite& _x,
+                                        const Eigen::VectorXd& _delta,
+                                        const double           _Dt,
+                                        VectorComposite&       _x_plus_delta) const override;
+        Eigen::VectorXd  deltaZero() const override;
+        Eigen::VectorXd  correctDelta(const Eigen::VectorXd& delta_preint,
+                                      const Eigen::VectorXd& delta_step) const override;
+        VectorXd         getCalibration(const CaptureBaseConstPtr _capture = nullptr) const override;
+        void             setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
+        bool             voteForKeyFrame() const override;
+        CaptureMotionPtr emplaceCapture(const FrameBasePtr&   _frame_own,
+                                        const SensorBasePtr&  _sensor,
+                                        const TimeStamp&      _ts,
+                                        const VectorXd&       _data,
+                                        const MatrixXd&       _data_cov,
+                                        const VectorXd&       _calib,
+                                        const VectorXd&       _calib_preint,
+                                        const CaptureBasePtr& _capture_origin) override;
+        FeatureBasePtr   emplaceFeature(CaptureMotionPtr _capture_motion) override;
+        FactorBasePtr    emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin) override;
+        virtual void     bootstrap() override;
+        CaptureBasePtr   bootstrapOrigin() const;
+        VectorXd         bootstrapDelta() const;
 
       protected:
         ParamsProcessorImuPtr    params_motion_Imu_;
diff --git a/src/processor/processor_imu.cpp b/src/processor/processor_imu.cpp
index 8f3433ea0..87582d974 100644
--- a/src/processor/processor_imu.cpp
+++ b/src/processor/processor_imu.cpp
@@ -263,13 +263,8 @@ void ProcessorImu::bootstrap()
         }
         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_;
+            CaptureBasePtr first_capture = bootstrapOrigin();
+            TimeStamp      t_current     = last_ptr_->getBuffer().back().ts_;
             if (t_current - first_capture->getTimeStamp() >= params_motion_Imu_->bootstrap_averaging_length)
             {
                 // frames:
@@ -283,21 +278,10 @@ void ProcessorImu::bootstrap()
                 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);
+                VectorXd delta_int = bootstrapDelta();
 
                 // compute local g and transformation to global g
-                dt                     = t_current - first_capture->getTimeStamp();  //
+                double      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();                                  //
@@ -335,6 +319,36 @@ void ProcessorImu::bootstrap()
     }
 }
 
+CaptureBasePtr ProcessorImu::bootstrapOrigin() const
+{
+    if (list_fac_inactive_bootstrap_.empty())
+        return origin_ptr_;
+    else
+        return std::static_pointer_cast<CaptureMotion>(list_fac_inactive_bootstrap_.front()->getCapture())
+            ->getOriginCapture();
+}
+
+VectorXd ProcessorImu::bootstrapDelta() const
+{
+    // Compute total integrated delta during bootstrap period
+    // first, integrate all deltas in previous factors
+    VectorXd delta_int = deltaZero();
+    double   dt;
+    for (const auto& fac : list_fac_inactive_bootstrap_)
+    // here, we take advantage of the list of IMU factors to recover all deltas
+    {
+        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_
+    dt                = last_ptr_->getBuffer().back().ts_ - origin_ptr_->getTimeStamp();
+    const auto& delta = last_ptr_->getDeltaPreint();
+    delta_int         = imu::compose(delta_int, delta, dt);
+
+    return delta_int;
+}
+
 } // namespace wolf
 
 // Register in the FactoryProcessor
-- 
GitLab