Pixel to world

[yfshaikh]

Summary

• Add pixel_to_world to frame_tf_service (srv flag + client + optional pixel overlay).
• Fix world_to_pixel (correct camera-frame projection, TF handling, and float64 serialization).
• A pixel (u, v) alone is underdetermined: with camera pose it defines a ray, so we need depth.
• Depth source: project the latest LiDAR cloud into the image and associate the nearest return within a pixel radius; return that point in map.

Conventions

Camera optical frame: +X right, +Y down, +Z forward.
Intrinsics from CameraInfo.k: fx, fy, cx, cy.

Algorithm — pixel_to_world

1. Parse LiDAR XYZ; TF cloud → camera frame and → map.
2. Drop z_c <= 0 / off-image projections.
3. Project: u = fx·x_c/z_c + cx, v = fy·y_c/z_c + cy.
4. Pick nearest (u,v) within MAX_PIXEL_RADIUS (tie-break by smaller z_c).
5. Return that return’s map XYZ (float64 bytes), or tf_success=false if none.

Note: this returns a measured LiDAR point near the query pixel (nearby-ray association), not an exact intersection of the query ray with a surface.

Algorithm — world_to_pixel

1. Load CameraInfo for camera_name; use header.frame_id as the camera frame.
2. TF the map point into the camera: p_cam = R · P_map + t (lookup_transform(camera, map, …)).
3. Reject non-finite points or points behind the camera (z_c <= 0).
4. Project: u = fx·x_c/z_c + cx, v = fy·y_c/z_c + cy.
5. Return (u, v) as float64 bytes, or tf_success=false on failure.

Fixes vs the starter code: one TF lookup (no hang loop), project p_cam (not raw map XYZ), Rm.apply(...) for the rigid transform, stamp 0/0 → latest TF, and np.asarray([u,v], float64).tobytes() instead of bytes(coords).

Camera name → RViz image topic (this stack)

camera_name	Image topic
rgb_front	/cameras/camera0
rgb_right	/cameras/camera1
rgb_back	/cameras/camera2
rgb_left	/cameras/camera3

rgb_center may have 0 publishers — use rgb_front for the main camera.

---

Test plan — pixel_to_world

Requires CARLA + carla_interface / bridge + navigator (launch.carla.py), same ROS_DOMAIN_ID.

1. Build

# inside navigator_carla
cd /navigator
colcon build --symlink-install --packages-select frame_tf_serv frame_tf_service frame_tf_client
source /navigator/install/setup.bash

2. Confirm sensors

ros2 topic hz /lidar
ros2 topic hz /carla/hero/rgb_front/camera_info
ros2 topic echo /carla/hero/rgb_front/camera_info --once   # width/height/frame_id (e.g. 800x600, hero/rgb_front)

3. Start service and wait for data

# terminal A
ros2 run frame_tf_service service

Wait until logs show CameraInfo + LiDAR ready (status should move off NO). Do not call the client before that.

Sensor subscriptions use Best Effort (and Reliable fallback); without matching QoS the node stays at No CameraInfo yet.

4. Basic client checks

# terminal B — ground/center-ish pixel (800x600 → try 400,350)
ros2 run frame_tf_client client 0 0 0 1 rgb_front 400.0 350.0 0.0
# expect: (True, array([X, Y, Z]))

# sky / top of image
ros2 run frame_tf_client client 0 0 0 1 rgb_front 400.0 5.0 0.0
# expect: (False, array([]))

Client args:
cam_to_world cam_to_pixel world_to_pixel pixel_to_world camera_name x y z
For pixel_to_world, x,y = u,v.

5. Visual check (overlay + map marker)

# mark the query pixel on the camera image
ros2 run frame_tf_client overlay 400 350 rgb_front

In RViz:

• Add → Image, topic /pixel_query_overlay (red crosshair = queried pixel)
• Fixed Frame: map
• PointCloud2 on /lidar (Reliability: Best Effort if the display shows 0 msgs)

Drop the returned XYZ as a marker:

ros2 topic pub --once /pixel_to_world_debug visualization_msgs/msg/Marker "{
  header: {frame_id: 'map'},
  ns: 'p2w', id: 0, type: 2, action: 0,
  pose: {position: {x: X, y: Y, z: Z}, orientation: {w: 1.0}},
  scale: {x: 1.5, y: 1.5, z: 1.5},
  color: {r: 1.0, g: 0.2, b: 0.2, a: 1.0}
}"

Add → Marker, topic /pixel_to_world_debug.

Pass criteria: crosshair sits on a surface in the image (e.g. road); red sphere sits on that same surface in the map/LiDAR view. The sphere is 3D — it will not appear on the image.

6. Other cameras

ros2 run frame_tf_client overlay 400 350 rgb_left
ros2 run frame_tf_client client 0 0 0 1 rgb_left 400.0 350.0 0.0
# optional: new marker with id: 1

Side cameras may return false more often (fewer LiDAR hits in view).

Checklist

• Build + source succeeds
• /lidar + /carla/hero/rgb_front/camera_info publishing
• Service receives CameraInfo + cloud before client calls
• Ground pixel → tf_success: true
• Sky pixel → tf_success: false
• Overlay crosshair + map marker agree visually
• Optional: rgb_left / rgb_right / rgb_back

---

Test plan — world_to_pixel

Idea

Given a map XYZ known to be in front of the camera, project it to image pixels and confirm (u, v) land on the expected feature.

Smoke checks

# Use a map point from pixel_to_world (or any known in-view map XYZ)
ros2 run frame_tf_client client 0 0 1 0 rgb_front X Y Z
# expect: (True, array([u, v]))

Client args:
cam_to_world cam_to_pixel world_to_pixel pixel_to_world camera_name x y z
For world_to_pixel, x,y,z = map XYZ.

Behind-camera / bad TF should return tf_success: false without hanging.

Checklist

• world_to_pixel returns without hanging
• In-view map point → tf_success: true and two floats (u, v)
• Point clearly behind the camera / invalid → tf_success: false

---

Test plan — consistency (pixel_to_world ↔ world_to_pixel)

These two services are inverses in intent but not bit-exact inverses: pixel_to_world returns a nearby LiDAR return, not the exact ray–surface intersection. A few pixels of round-trip error is expected and OK.

Round-trip: pixel → world → pixel

1. Query a ground/center-ish pixel.
2. Feed the returned map XYZ into world_to_pixel.
3. Compare recovered (u', v') to the original (u, v).

# 1) pixel → world
ros2 run frame_tf_client client 0 0 0 1 rgb_front 400.0 350.0 0.0
# save X Y Z from the result, e.g. (48.97, -306.66, 0.22)

# 2) world → pixel
ros2 run frame_tf_client client 0 0 1 0 rgb_front X Y Z
# expect u'≈400, v'≈350 (a few px of error is OK)

# 3) visual: overlay the recovered pixel
ros2 run frame_tf_client overlay <u'> <v'> rgb_front

Pass criteria

• Round-trip succeeds (tf_success: true both ways).
• |u'−u| and |v'−v| are small (typically within a few pixels; association radius / depth discreteness set the floor).
• Overlay of (u', v') sits on the same scene feature as the original query (e.g. same road patch).

Optional: world → pixel → world (sanity)

If you start from a known map point that is a LiDAR return visible in the camera, world_to_pixel then pixel_to_world should land near that same 3D location (meters, not necessarily exact — NN association again).

ros2 run frame_tf_client client 0 0 1 0 rgb_front X Y Z
# → u v
ros2 run frame_tf_client client 0 0 0 1 rgb_front u v 0.0
# → X' Y' Z' near X Y Z in map

Checklist

• pixel_to_world(u,v) → map XYZ succeeds
• world_to_pixel(XYZ) → (u',v') succeeds
• (u',v') within a few px of (u,v)
• Overlay of (u',v') matches the same image feature as (u,v)
• Optional: world → pixel → world stays near the original map point