Offset Reprojecting Depth -> Points

[micat001]

Preliminary Checks

• [X] This issue is not a duplicate. Before opening a new issue, please search existing issues.
• [X] This issue is not a question, feature request, or anything other than a bug report directly related to this project.

As a note: this is listed as a "bug" because there doesn't seem to be a "questions" label (although it would fall more into that category)

Description

I'm having issues using the returned depth image and reprojecting it to a point cloud. I'm seeing an offset in the y-direction (relative to the zed2 left_camera_frame):

There's a small offset shown here:

Steps to Reproduce

I'm reading in the depth image and using the depth/camera_info to reproject the points to (x, y, z) like this:

1. Read in depth/depth_registered:

   depth_array = np.frombuffer(depth_msg.data, dtype=np.float32).reshape(depth_msg.height, depth_msg.width)

2. Remove nan/inf values:

   depth_array = np.nan_to_num(depth_array, nan=depth_max, posinf=depth_max, neginf=depth_max)

3. Project to points where intrinsics are from the depth/camera_info

   intrinsic_array = np.array(depth_info_msg.K).reshape(3, 3)
   fx = intrinsic_array[0, 0]
   fy = intrinsic_array[0, -1]
   cx = intrinsic_array[1, 1]
   cy = intrinsic_array[1, -1]
   rows, cols = depth_array.shape
   c_grid, r_grid = np.meshgrid(np.arange(cols), np.arange(rows), sparse=True)
   x = depth_array * (c_grid - cx) / fx
   y = depth_array * (r_grid - cy) / fy
   z = depth_array
   pts = np.dstack((x, y, z))
   points = pts.reshape(-1, 3)

4. Transform from optical frame to camera frame:

   target_frame = "zed2_left_camera_frame"
   transform_time = self._buffer.get_latest_common_time(target_frame, depth_msg.header.frame_id)
   transform = self._buffer.lookup_transform(target_frame, depth_msg.header.frame_id, transform_time)
   tf = numpify(transform.transform)
   points_homogeneous = np.hstack((points, np.ones((points.shape[0], 1))))
   transformed_pts = np.dot(points_homogeneous, tf.T)

5. Publish the points:

   N = len(points)
   pc_data = np.zeros(N, dtype=[(field.name, np.float32) for field in fields])
   pc_data["x"] = transformed_pts[:, 0]
   pc_data["y"] = transformed_pts[:, 1]
   pc_data["z"] = transformed_pts[:, 2]
   pc_msg = msgify(PointCloud2, pc_data)
   pc_msg.header = depth_msg.header
   pc_msg.header.frame_id = target_frame
   self.pc_pub.publish(pc_msg)

I've always struggled to get color properly into a python pc2 message, so i just left it as (x, y, z).

Expected Result

I'd expect the points to overlap pretty much perfectly, unless there's some magic step that ZED is doing in their API to get the (x, y, z) from depth.

Actual Result

A visible offset between the zed registered pointcloud and my published pointcloud.

ZED Camera model

ZED2

Environment

OS: Linux Ubuntu 20.04
CPU: x86_64
GPU: NVIDIA GeForce RTX 4080
ZED SDK: 4/0 (for cuda 11.8)
ROS: Noetic

Anything else?

No response

[Myzhar]

Hi @micat001 can you extract the value of the offset that you get?

[Myzhar]

I suspect that the offset is generated by step 4. If you reproject the points into the camera frame and then you set pc_msg.header.frame_id = target_frame the result is a point cloud rendered with an offset of baseline/2 You can fix it in two ways:

1. (recommended) Do not reproject the point cloud
2. set pc_msg.header.frame_id to the camera frame and not the target frame which in your case is the optical frame.

[micat001]

A bit confused by this:

If you reproject the points into the camera frame and then you set pc_msg.header.frame_id = target_frame the result is a point cloud rendered with an offset of baseline/2

I don't understand this, unless I'm misunderstanding the pointcloud frame and generation. There doesn't seem like there should be an offset from the left_camera_frame -> left_camera_optical_frame (and vice versa) right? Just an axes flip. The point_cloud_registered message has a frame_id of left_camera_frame and the depth has a frame of left_camera_optical_frame. Why can I not transform between the two?

For your first comment if i manually tweak the translation/rotation tf until it seems like it overlaps, it looks something like this:

from tf.transformations import compose_matrix
tf_vector = [0.000, 0.15, -0.010, -1.521, -0.000, -1.571]
tf_matrix = compose_matrix(cam_pose[:3], angles=cam_pose[3:])

It's still not quite aligned, but closer:

As for option 1 - I'd love to not reproject the cloud (assuming I can be confident of the frame of the point cloud message) but my issue is that I am running a custom detector on the /left/image_rect_color topic. Once I have detected an object, I'd like to crop it from the point cloud for further 3D processing. To do this, I need to go from Image [u, v] -> point_cloud[x, y, z]. I am currently doing this by the process defined above (image[u, v] == depth[u, v] -> reproject(depth)-> [x, y, z]). If there's a better way to do this, I'm all ears!

Thanks for your prompt responses @Myzhar!

[micat001]

*Edit. Removed as I think about the minimum working example that I'd need to solve this.

How, using the published ROS topics, how can I take a [u, v] point in the color image (is the left_rect image the one to use?) and find the x, y, x point in the left camera frame. I've seen a few other issues that offer a solution like this where you can look-up the [u, v] point in the pointcloud data but I find this a bit hard to work with because not all depth points are valid.

[micat001]

The issue is directly related to the resolution of the ZED.

When I set the ZED to the highest resolution (resolution: 0 in the config file) the reprojection is right on. For each other resolution, the reprojection is way off. This leads me to believe that I cannot use the intrinsics from the CameraInfo message from the registered depth image to do the reprojection.

@Myzhar - Can you chime in with why this might be resolution dependent?

[github-actions[bot]]

This issue is stale because it has been open 30 days with no activity. Remove stale label or comment otherwise it will be automatically closed in 5 days

[micat001]

Commenting to keep this issue alive.

[micat001]

The issue was in Step 3, the extraction of the intrinsics:

fx = intrinsic_array[0, 0]
cx = intrinsic_array[0, -1] # was labelled "fy"
fy = intrinsic_array[1, 1] # was labelled "cx"
cy = intrinsic_array[1, -1]