[Tektrix]

Closes #125.

Overview

• XYArray.homogeneous_transformation() now uses rot_mat_22 instead of a hand-written rotation matrix, and adds the translation with plain broadcasting (+ np.array([[x], [y]])) instead of np.ones(shape) * [[x], [y]].
• InformedRrtStarPathPlanner.sample_informed() now uses rot_mat_22(theta) for its rotation matrix.
• BinaryOccupancyGrid.add_object() now uses rot_mat_22(yaw).
• LShapeFittingDetector._rotate_points() now uses rot_mat_22(angle_rad).T, since it rotates by -angle_rad.
• rot_mat_22 gets a docstring in the same style as hom_mat_33.

All files load matrix_lib with the same sys.path.append(...) pattern the existing code uses. The remaining -sin matches in src/components are not rotation matrices: the lateral error in cubic_spline_course.py, the line coefficients passed to Rectangle, and the dy terms in robust_least_squares_detector.py. Behavior is unchanged.

How I confirmed it works

• rot_mat_22 builds the same matrix as the code it replaces (and its transpose is the inverse rotation _rotate_points() built by hand), and broadcasting a 2x1 column gives the same result as multiplying it with np.ones first.
• The changed code is covered by the existing tests: test_xy_array.py (including test_homogeneous_transformation), test_matrix_lib.py, test_informed_rrt_star_path_planning.py, the path planning smoke tests that build a BinaryOccupancyGrid, and test_point_cloud_rectangle_fitting.py / test_point_cloud_robustLS_rectangle_fitting.py, which run in GitHub Actions on this PR.

[ShisatoYano]

Thanks a lot for picking this up, @Tektrix, and for posting the plan on the issue first! The diff is small and easy to review: XYArray.homogeneous_transformation() and sample_informed() now read much more clearly with rot_mat_22, the broadcasting change removes the unneeded np.ones(...), and the new docstring matches hom_mat_33. I checked locally that both replacements give bitwise-identical results to the old code (20k random cases for XYArray, 200k angles for the informed RRT* rotation), and the full test suite passes.

A couple of requests before merging:

1. src/components/mapping/grid/binary_occupancy_grid.py (lines 97-100, add_object()) still builds [[cos(yaw), -sin(yaw)], [sin(yaw), cos(yaw)]] by hand. It is a real rotation matrix (applied as corners @ R.T), so it is in scope for Use the shared rotation and transformation helpers #125. Could you replace it with rot_mat_22(yaw) as well?
2. (Optional) _rotate_points() in l_shape_fitting_detector.py is the inverse rotation, i.e. rot_mat_22(angle_rad).T. Feel free to keep it as is if you think it reads better. In that case, please update the PR description so it doesn't call it a projection.

Thanks again!

[Tektrix]

Thanks for the review and for checking it so thoroughly! I've replaced the rotation in BinaryOccupancyGrid.add_object() with rot_mat_22(yaw), and _rotate_points() now uses rot_mat_22(angle_rad).T. I also corrected the PR description: you're right that _rotate_points() is a rotation, not a projection. I searched again for every -…sin spelling, and the remaining matches aren't rotation matrices.

[ShisatoYano]

Thanks for the update, @Tektrix! Both points are addressed, and the comment on _rotate_points() makes the transpose easy to follow. I checked that the BinaryOccupancyGrid change is bitwise identical to the old code (200k angles), and the full suite passes locally.

One last thing: #160 was just merged and touched the same import lines in informed_rrt_star_path_planner.py, so this PR now has a conflict. Could you rebase on (or merge) the latest main? Both sides only add lines, so the resolved part should look like this:

sys.path.append(abs_dir_path + relative_path + "mapping/grid")
sys.path.append(abs_dir_path + relative_path + "common")

from save_gif import is_save_gif_enabled
from matrix_lib import rot_mat_22

After that, I'll approve the CI run and merge. Thanks!

[ShisatoYano]

Hi @Tektrix, just a friendly ping on this one. The code itself is ready to go: I re-ran the full suite on top of the latest main with the conflict resolved locally (110 passed), and all four replacements still give bitwise-identical results to the old code.

The only thing left is the conflict in src/components/plan/informed_rrt_star/informed_rrt_star_path_planner.py. main has moved on a bit since my last comment, but that is still the only conflicting file, and the resolution is the same: keep main's side and add your rot_mat_22 import after it:

sys.path.append(abs_dir_path + relative_path + "mapping/grid")
sys.path.append(abs_dir_path + relative_path + "common")

from save_gif import is_save_gif_enabled
from matrix_lib import rot_mat_22

Thanks!

🤖 Generated with Claude Code

[Tektrix]

Merged the latest main and resolved the conflict as you suggested. Thanks for the patience!

[ShisatoYano]

Thanks @Tektrix! The conflict is resolved exactly as suggested: main's common path and save_gif import are kept, followed by the rot_mat_22 import.

I checked the new head (448f4fb) locally:

• It merges cleanly into the current main, and the diff against main is still just the five intended files.
• The full test suite passes (110 passed).
• All four replacements still give bitwise-identical results to the original rotation code.

Nothing else is needed from your side. Thanks again for the cleanup!

🤖 Generated with Claude Code