Skip to content

Lay out organized point clouds as (height, width) - #355

Open
Rahul-cs50 wants to merge 1 commit into
ros2:rollingfrom
Rahul-cs50:fix/236-organized-cloud-shape
Open

Rahul-cs50 wants to merge 1 commit into
ros2:rollingfrom
Rahul-cs50:fix/236-organized-cloud-shape

Conversation

@Rahul-cs50

Copy link
Copy Markdown

Fixes #236

Description

An organized PointCloud2 stores its points row by row, width points per row. With reshape_organized_cloud=True, read_points() reshapes them to (width, height). The shape looks transposed, but the problem is worse than that. The data is never transposed, only re-cut, so element [r, c] is generally not the point in row r and column c. For a cloud with height 2 and width 3 holding x = row, y = column:

read_points(cloud, reshape_organized_cloud=True)[['x', 'y']].tolist()
# before: [[(0, 0), (0, 1)], [(0, 2), (1, 0)], [(1, 1), (1, 2)]]   shape (3, 2)
# after:  [[(0, 0), (0, 1), (0, 2)], [(1, 0), (1, 1), (1, 2)]]     shape (2, 3)

create_cloud() had the matching mistake. For a 2D array it set width = shape[0] and height = shape[1], so the message also got a wrong row_step, and any other consumer (C++ iterators, RViz, PCL) read the rows wrongly. The two errors cancelled out in a Python round trip, which is why the existing tests passed.

The fix reshapes to (height, width) and reads an organized array as height, width = points.shape, the same [row, column] convention that images and numpy use. This changes the shape returned for organized clouds. As @mjcarroll and @jdlangs said in the issue, the old layout was wrong, not just a different convention.

Tests

  • The organized fixture pcd3 had the same transposed layout (width=shape[0]), and it is corrected.
  • New test test_read_points_organized_row_major builds a non-square cloud with create_cloud_xyz32. It checks height, width, row_step and that each point is at [row, column].

In a ros:rolling-ros-base container, test_point_cloud2.py has 19 passed. On the current code, 3 fail: the two organized tests and the new one.

PointCloud2 stores an organized cloud row by row, with `width` points
per row. read_points(reshape_organized_cloud=True) reshaped the points
to (width, height), so element [r, c] was not the point in row r and
column c. create_cloud() made the matching mistake in the other
direction and took the array's first axis as the width, which also
gave a wrong row_step.

Reshape to (height, width) and read height and width from an organized
array in that order. The organized test fixture had the same transposed
layout and is corrected; a new test checks the point position on a
non-square cloud.

Fixes ros2#236

Signed-off-by: Rahul-cs50 <rahulseervi.dev@gmail.com>
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

sensor_msgs_py.point_cloud2 functions treat numpy arrays as having shape (width, height)

1 participant