From 64e2246e499a77a877fde5f9d44d3cc6e2637d8d Mon Sep 17 00:00:00 2001
From: Sagi Amangeldi <s.amangeldi.14@gmail.com>
Date: Fri, 23 Oct 2026 23:14:27 +0900
Subject: [PATCH 1/3] Fix past-the-end dereference in getInterpolatedPose

---
 shared/include/HDMapping/PoseInterpolation.h | 9 +++++----
 1 file changed, 5 insertions(+), 4 deletions(-)

diff --git a/shared/include/HDMapping/PoseInterpolation.h b/shared/include/HDMapping/PoseInterpolation.h
index 4718c150..86ee5f0d 100644
--- a/shared/include/HDMapping/PoseInterpolation.h
+++ b/shared/include/HDMapping/PoseInterpolation.h
@@ -27,6 +27,11 @@ inline Eigen::Matrix4d getInterpolatedPose(const std::map<double, Eigen::Matrix4
     {
         return ret;
     }
+    // query_time is past the last pose; end() must be caught before it_lower is dereferenced
+    if (it_lower == trajectory.end())
+    {
+        return ret;
+    }
     if (it_lower->first > query_time)
     {
         it_lower = std::prev(it_lower);
@@ -35,10 +40,6 @@ inline Eigen::Matrix4d getInterpolatedPose(const std::map<double, Eigen::Matrix4
     {
         return ret;
     }
-    if (it_lower == trajectory.end())
-    {
-        return ret;
-    }
 
     constexpr double MaxInterpolationS = 0.1;
     double t1 = it_lower->first;

From 36770027a258c108e855e586cf13996a9191d0d9 Mon Sep 17 00:00:00 2001
From: Sagi Amangeldi <s.amangeldi.14@gmail.com>
Date: Fri, 23 Oct 2026 23:14:57 +0900
Subject: [PATCH 2/3] Build shared tests in libstdc++ debug mode

---
 shared/tests/CMakeLists.txt | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/shared/tests/CMakeLists.txt b/shared/tests/CMakeLists.txt
index 82be362f..ed7d3bd4 100644
--- a/shared/tests/CMakeLists.txt
+++ b/shared/tests/CMakeLists.txt
@@ -22,5 +22,11 @@ if (MSVC)
     target_compile_definitions(hdmapping_shared_tests PRIVATE _USE_MATH_DEFINES)
 endif()
 
+# Build in libstdc++ debug mode so iterator misuse (e.g. dereferencing end())
+# fails the test run instead of passing by luck. Safe because the code under
+# test is header-only and no other C++ library is linked in; other standard
+# libraries ignore the macro.
+target_compile_definitions(hdmapping_shared_tests PRIVATE _GLIBCXX_DEBUG)
+
 include(CTest)
 add_test(NAME hdmapping_shared_tests COMMAND hdmapping_shared_tests)

From ca79510634e733457dad5b2b33f94348f50b14b0 Mon Sep 17 00:00:00 2001
From: Sagi Amangeldi <s.amangeldi.14@gmail.com>
Date: Fri, 23 Oct 2026 23:16:00 +0900
Subject: [PATCH 3/3] Test getInterpolatedPose after the last pose

---
 shared/tests/test_pose_interpolation.cpp | 13 +++++++++++++
 1 file changed, 13 insertions(+)

diff --git a/shared/tests/test_pose_interpolation.cpp b/shared/tests/test_pose_interpolation.cpp
index ac357c44..e95c0e2e 100644
--- a/shared/tests/test_pose_interpolation.cpp
+++ b/shared/tests/test_pose_interpolation.cpp
@@ -39,6 +39,19 @@ TEST_CASE("getInterpolatedPose: query before the first timestamp returns a zero
     CHECK(isZeroMatrix(getInterpolatedPose(traj, -100.0)));
 }
 
+TEST_CASE("getInterpolatedPose: query after the last timestamp returns a zero matrix")
+{
+    // lower_bound() returns end() here, which must not be dereferenced --
+    // the _GLIBCXX_DEBUG build of this test (see CMakeLists.txt) aborts if it is.
+    std::map<double, Eigen::Matrix4d> traj{
+        { 1.0, makePose(0, 0, 0) },
+        { 2.0, makePose(10, 0, 0) },
+        { 3.0, makePose(20, 0, 0) },
+    };
+    CHECK(isZeroMatrix(getInterpolatedPose(traj, 3.5)));
+    CHECK(isZeroMatrix(getInterpolatedPose(traj, 100.0)));
+}
+
 TEST_CASE("getInterpolatedPose: exact match at an interior timestamp returns that exact pose")
 {
     // Interior = not the very first key -- see the "first segment" case below