Skip to content

Fix pose interpolation past end #577

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
JanuszBedkowski merged 3 commits into from
Oct 24, 2026
Merged

Fix pose interpolation past end #577

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
64e2246
Fix past-the-end dereference in getInterpolatedPose
capCafu Oct 23, 2026
3677002
Build shared tests in libstdc++ debug mode
capCafu Oct 23, 2026
ca79510
Test getInterpolatedPose after the last pose
capCafu Oct 23, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
9 changes: 5 additions & 4 deletions shared/include/HDMapping/PoseInterpolation.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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;
Expand Down
6 changes: 6 additions & 0 deletions shared/tests/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
13 changes: 13 additions & 0 deletions shared/tests/test_pose_interpolation.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
Loading