diff --git a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/utils.cpp b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/utils.cpp
index 2df7a69de90bc..d6f3d5e26d4e3 100644
--- a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/utils.cpp
+++ b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/utils.cpp
@@ -120,9 +120,8 @@ TrajectoryPoints resampleTrajectory(const Trajectory & trajectory, const double
 
     if (boost::geometry::distance(prev_point.to_2d(), next_point.to_2d()) >= interval) {
       resampled.push_back(traj_point);
+      prev_point = next_point;
     }
-
-    prev_point = next_point;
   }
   resampled.push_back(trajectory.points.back());
 
diff --git a/control/autoware_lane_departure_checker/test/test_resample_trajectory.cpp b/control/autoware_lane_departure_checker/test/test_resample_trajectory.cpp
index 214a165660225..10550051a1df6 100644
--- a/control/autoware_lane_departure_checker/test/test_resample_trajectory.cpp
+++ b/control/autoware_lane_departure_checker/test/test_resample_trajectory.cpp
@@ -92,5 +92,10 @@ INSTANTIATE_TEST_SUITE_P(
       "IntervalIsGreaterThanDistance",
       {{1.0, 0.0, 0.0}, {2.0, 0.0, 0.0}, {3.0, 0.0, 0.0}},
       1.5,
-      {{1.0, 0.0, 0.0}, {3.0, 0.0, 0.0}}}),
+      {{1.0, 0.0, 0.0}, {3.0, 0.0, 0.0}}},
+    ResampleTrajectoryTestParam{
+      "ConsecutiveSmallDistancesFollowedByLargeDistance",
+      {{0.0, 0.0, 0.0}, {0.7, 0.0, 0.0}, {1.3, 0.0, 0.0}, {3.0, 0.0, 0.0}},
+      1.0,
+      {{0.0, 0.0, 0.0}, {1.3, 0.0, 0.0}, {3.0, 0.0, 0.0}}}),
   ::testing::PrintToStringParamName());