From b24b64a740a934e06a504a19700f5bf081191654 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Mon, 4 May 2020 20:57:25 +0200
Subject: [PATCH] Optimize code for delta preintegration

---
 src/processor/processor_motion.cpp | 164 +++++++++++++----------------
 1 file changed, 74 insertions(+), 90 deletions(-)

diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 4942a72ba..3b11415f7 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -561,47 +561,52 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
 
 void ProcessorMotion::integrateOneStep()
 {
-    // Set dt
-    double dt_ = incoming_ptr_->getTimeStamp() - getBuffer().back().ts_;
-
-    assert(dt_ >= 0 && "Time interval _dt is negative!");
 
     calib_preint_ = getLast()->getCalibrationPreint();
 
+    TimeStamp prev_ts;
+
+    if (getBuffer().empty())
+    {
+        prev_ts = origin_ptr_->getTimeStamp();
+        delta_integrated_ = deltaZero();
+    }
+    else
+    {
+        prev_ts = getBuffer().back().ts_;
+    }
+
+    // Set dt
+    double dt = incoming_ptr_->getTimeStamp() - prev_ts;
+
+    assert(dt >= 0 && "Time interval _dt is negative!");
+
     // get data and convert it to delta, and obtain also the delta covariance
     computeCurrentDelta(incoming_ptr_->getData(),
                         incoming_ptr_->getDataCovariance(),
                         calib_preint_,
-                        dt_,
+                        dt,
                         delta_,
                         delta_cov_,
                         jacobian_delta_calib_);
 
     // integrate the current delta to pre-integrated measurements, and get Jacobians
-    if (getBuffer().empty())
-    { // first time
-
-        deltaPlusDelta(deltaZero(),
-                       delta_,
-                       dt_,
-                       delta_integrated_,
-                       jacobian_delta_preint_,
-                       jacobian_delta_);
-
+    deltaPlusDelta(delta_integrated_,
+                   delta_,
+                   dt,
+                   delta_integrated_,
+                   jacobian_delta_preint_,
+                   jacobian_delta_);
+
+    if (getBuffer().empty())    // first time
+    {
+        // init integrals of jacobian and covariance
         jacobian_calib_       = jacobian_delta_ * jacobian_delta_calib_;
         delta_integrated_cov_ = jacobian_delta_.propagate(delta_cov_);
-
     }
-    else
-    { // other times
-
-        deltaPlusDelta(getBuffer().back().delta_integr_,
-                       delta_,
-                       dt_,
-                       delta_integrated_,
-                       jacobian_delta_preint_,
-                       jacobian_delta_);
-
+    else                        // other times
+    {
+        // integrate jacobian and covariance
         jacobian_calib_ = jacobian_delta_preint_ * getBuffer().back().jacobian_calib_
                         + jacobian_delta_ * jacobian_delta_calib_;
 
@@ -611,15 +616,15 @@ void ProcessorMotion::integrateOneStep()
 
     // push all into buffer
     getBuffer().emplace_back(incoming_ptr_->getTimeStamp(),
-                                   incoming_ptr_->getData(),
-                                   incoming_ptr_->getDataCovariance(),
-                                   delta_,
-                                   delta_cov_,
-                                   delta_integrated_,
-                                   delta_integrated_cov_,
-                                   jacobian_delta_,
-                                   jacobian_delta_preint_,
-                                   jacobian_calib_);
+                             incoming_ptr_->getData(),
+                             incoming_ptr_->getDataCovariance(),
+                             delta_,
+                             delta_cov_,
+                             delta_integrated_,
+                             delta_integrated_cov_,
+                             jacobian_delta_,
+                             jacobian_delta_preint_,
+                             jacobian_calib_);
 }
 
 void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr)
@@ -627,82 +632,59 @@ void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr)
     if ( _capture_ptr->getBuffer().empty() )
         return;
 
-    VectorType calib              = _capture_ptr->getCalibrationPreint();
-
-    // Iterate all the buffer
-    auto motion_it      = _capture_ptr->getBuffer().begin();
-
-    {   // first time
-
-        const auto& capture_origin = _capture_ptr->getOriginCapture();
-
-        TimeStamp time_origin = capture_origin->getTimeStamp(); // TODO use a time_origin stored in the same buffer
+    calib_preint_           = _capture_ptr->getCalibrationPreint();
 
-        // get dt
-        const double dt = motion_it->ts_ - time_origin;
-
-        // re-convert data to delta with the new calibration parameters
-        computeCurrentDelta(motion_it->data_,
-                            motion_it->data_cov_,
-                            calib,
-                            dt,
-                            delta_,
-                            delta_cov_,
-                            jacobian_delta_calib_);
+    TimeStamp   prev_ts     = _capture_ptr->getOriginCapture()->getTimeStamp();
 
-        deltaPlusDelta(deltaZero(),
-                       delta_,
-                       dt,
-                       delta_integrated_,
-                       jacobian_delta_preint_,
-                       jacobian_delta_);
+    delta_integrated_       = deltaZero();
 
-        jacobian_calib_       = jacobian_delta_ * jacobian_delta_calib_;
-        delta_integrated_cov_ = jacobian_delta_.propagate(delta_cov_);
+    bool        first_time  = true;
 
-        motion_it->delta_                   = delta_;
-        motion_it->delta_cov_               = delta_cov_;
-        motion_it->delta_integr_            = delta_integrated_;
-        motion_it->delta_integr_cov_        = delta_integrated_cov_;
-        motion_it->jacobian_delta_          = jacobian_delta_;
-        motion_it->jacobian_delta_integr_   = jacobian_delta_preint_;
-        motion_it->jacobian_calib_          = jacobian_calib_;
-
-    }
-
-    // other times
-    auto prev_motion_it = motion_it;
-    motion_it++;
+    // Iterate all the buffer
+    auto motion_it    = _capture_ptr->getBuffer().begin();
     while (motion_it != _capture_ptr->getBuffer().end())
     {
         // get dt
-        const double dt = motion_it->ts_ - prev_motion_it->ts_;
+        const double dt = motion_it->ts_ - prev_ts;
 
         // re-convert data to delta with the new calibration parameters
         computeCurrentDelta(motion_it->data_,
                             motion_it->data_cov_,
-                            calib,
+                            calib_preint_,
                             dt,
                             delta_,
                             delta_cov_,
                             jacobian_delta_calib_);
 
-        // integrate delta into delta_integr, and rewrite the buffer
-        deltaPlusDelta(prev_motion_it->delta_integr_,
+        deltaPlusDelta(delta_integrated_,
                        delta_,
                        dt,
                        delta_integrated_,
                        jacobian_delta_preint_,
                        jacobian_delta_);
 
-        // integrate Jacobian wrt calib
-        jacobian_calib_ = jacobian_delta_preint_ * jacobian_calib_
-                        + jacobian_delta_        * jacobian_delta_calib_;
+        if (first_time == true)
+        {
+            first_time              = false;
+
+            // initialize Jacobian wrt calib
+            jacobian_calib_         = jacobian_delta_ * jacobian_delta_calib_;
+
+            // initialize covariance
+            delta_integrated_cov_   = jacobian_delta_.propagate(delta_cov_);
+        }
+        else
+        {
+            // integrate Jacobian wrt calib
+            jacobian_calib_ = jacobian_delta_preint_ * jacobian_calib_
+                            + jacobian_delta_        * jacobian_delta_calib_;
 
-        // Integrate covariance
-        delta_integrated_cov_ = jacobian_delta_preint_.propagate(delta_integrated_cov_)
-                                     + jacobian_delta_.propagate(delta_cov_);
+            // Integrate covariance
+            delta_integrated_cov_ = jacobian_delta_preint_.propagate(delta_integrated_cov_)
+                                         + jacobian_delta_.propagate(delta_cov_);
+        }
 
+        // store in buffer
         motion_it->delta_                   = delta_;
         motion_it->delta_cov_               = delta_cov_;
         motion_it->delta_integr_            = delta_integrated_;
@@ -711,9 +693,11 @@ void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr)
         motion_it->jacobian_delta_integr_   = jacobian_delta_preint_;
         motion_it->jacobian_calib_          = jacobian_calib_;
 
-        // advance in buffer
-        motion_it++;
-        prev_motion_it++;
+        // store carry variables
+        prev_ts         = motion_it->ts_;
+
+        // advance buffer iterator
+        motion_it ++;
     }
 }
 
-- 
GitLab