Initialize with IMU orientation

Author: AHuguetUPC

include/Headers/Accumulator.hpp

@@ -94,16 +94,16 @@ class Accumulator {
         template <typename ContentType>
         ContentType get_prev(Buffer<ContentType>& source, double t) {
             int k_t = before_t(source, t) + 1;
+            if (k_t >= source.content.size()) k_t = source.content.size() - 1;
 
-            // Get leftest State right to t (sorted new to old)
-            for (int k = k_t; k < source.content.size(); ++k) {
+            // Get leftest (newest) content right (previous) to t (sorted new to old)
+            for (int k = k_t; k >= 0; --k) {
                 ContentType cnt = source.content[k];
                 if (t > cnt.time) return cnt;
             }
 
-            // If not a state found, push an empty one at t
-            this->add(ContentType(), t);
-            return source.front();
+            // If not a content found, push an empty one at t
+            return ContentType();
         }
 
         // Process LiDAR pointcloud message

src/Modules/Accumulator.cpp

@@ -72,6 +72,13 @@ extern struct Params Config;
         /////////////////////////////////
 
         State Accumulator::get_prev_state(double t) {
+            if (this->BUFFER_X.empty()) {
+                State X = Localizator::getInstance().latest_state();
+                Accumulator::getInstance().add(X, t);
+                X.time = t;
+                return X;
+            }
+            
             return this->get_prev(this->BUFFER_X, t);
         }
 

src/Modules/Localizator.cpp

@@ -64,8 +64,7 @@ extern struct Params Config;
             // Initialize
             if (not this->initialized) {
                 if (imus.empty()) return;
-                IMU initial_IMU = IMU (); // imus.back();
-                // IMU initial_IMU = imus.back();
+                IMU initial_IMU = imus.back();
                 this->initialize(initial_IMU);
             }
 
@@ -85,7 +84,10 @@ extern struct Params Config;
         State Localizator::latest_state() {
             // If no integrated, return empty state
             if (this->last_time_integrated < 0)
-                return State (Accumulator::getInstance().initial_time);
+                return State (
+                    this->get_x(),
+                    Accumulator::getInstance().initial_time
+                );
 
             // If no updates, return integrated state
             if (this->last_time_updated < 0)