fix: last_data_ptr_ not updated in Predictor

Author: cggos

include/fusion/predictor.hpp

@@ -38,12 +38,14 @@ class Predictor {
   void process(Data::ConstPtr data_ptr, std::function<void(Data::ConstPtr, Data::ConstPtr)> func_predict = nullptr) {
     if (!data_ptr) return;
 
-    if (!push_data(data_ptr)) return;
-
-    if (func_predict != nullptr)
-      func_predict(last_data_ptr_, data_ptr);
-    else
-      predict(last_data_ptr_, data_ptr);
+    if (!inited_)
+      push_data(data_ptr);
+    else {
+      if (func_predict != nullptr)
+        func_predict(last_data_ptr_, data_ptr);
+      else
+        predict(last_data_ptr_, data_ptr);
+    }
 
     last_data_ptr_ = data_ptr;
   }

include/sensor/imu.hpp

@@ -60,11 +60,8 @@ class IMU : public Predictor {
       last_am = imu_ptr->acc;
     }
 
-    if (!inited_) {
-      imu_buf_.push_back(imu_ptr);
-      if (imu_buf_.size() > kImuBufSize) imu_buf_.pop_front();
-      return false;
-    }
+    imu_buf_.push_back(imu_ptr);
+    if (imu_buf_.size() > kImuBufSize) imu_buf_.pop_front();
 
     return true;
   }