From 27f401828e3b8da101a4d0c88841ec6ecf4763c7 Mon Sep 17 00:00:00 2001
From: jixingwu <1341582239@qq.com>
Date: Tue, 25 Apr 2023 14:21:42 +0800
Subject: [PATCH] fix the bug that may go out of index

---
 .gitignore       | 2 ++
 InterpolateImu.m | 5 ++++-
 2 files changed, 6 insertions(+), 1 deletion(-)
 create mode 100644 .gitignore

diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..27e6232
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,2 @@
+
+*.asv
diff --git a/InterpolateImu.m b/InterpolateImu.m
index 6bbd73b..eb2cbc7 100644
--- a/InterpolateImu.m
+++ b/InterpolateImu.m
@@ -91,7 +91,7 @@
     if (brp_index(i) > 5) % this only not happens in kitti 00
         p=polyfit([brp_index(i)-3:brp_index(i)]',imu_unsync_data(brp_index(i)-3:brp_index(i),1),1);
         prob_index = brp_index(i);
-        while (prob_index<length(imu_unsync_data) && prob_index < brp_index(i)+1000 )
+        while (prob_index<=length(imu_unsync_data) - prob_step && prob_index < brp_index(i)+1000 )
             prob_index = prob_index + prob_step;
             dist = abs(polyval(p, prob_index) - imu_unsync_data(prob_index,1));
             if dist < 1e-3
@@ -201,6 +201,9 @@
             
             figure(imu_fig_wz_h);
             plot(ins_matrix(:,1), ins_matrix(:,21), 'b.','MarkerSize',1);hold on;
+            % for a clearer view of the interpolated position
+            plot(ins_matrix(1,1), ins_matrix(1,21), 'bo', 'MarkerSize',5); hold on;
+            plot(ins_matrix(end,1), ins_matrix(end,21), 'bo', 'MarkerSize',5); hold on;
         end
 end
 imu_time_const_bias = mean(img_unsync_timestamp(1:length(img_sync_timestamp)) - img_sync_timestamp)...