FixTheRobot Explore Robotics / Systems Engineering / Engineering Guides / Module 06

Engineering Guides · Module 06

Projecting LiDAR points into the image

Transform points into the camera optical frame, divide by depth, apply K, and drop what is behind or outside.

  • Intermediate
  • Module 06: Camera–LiDAR Calibration & Coordinate Transformations
  • Written for ROS 2 Jazzy where ROS 2 is used
  • Sources checked 2026-11-06

Assumptions: Numbers in the examples are illustrative and the code is executed by the site’s tests. Facts taken from a cited source are marked; other statements are general engineering knowledge and must be confirmed on the datasheet of the part you use.

1. Engineering problem

Projecting LiDAR points onto the image is the basic test that a calibration is right, and the first step of camera–LiDAR fusion. It needs the camera←LiDAR transform, the camera intrinsics and a careful treatment of points that do not appear in the image.

2. When is this needed?

  • Validating calibration visually.
  • Colouring a point cloud with image colour.
  • Associating detections with LiDAR points.

3. Selection and design decisions

The CameraInfo message (Jazzy) carries the 3×3 intrinsic matrix K = [fx 0 cx; 0 fy cy; 0 0 1], which “projects 3D points in the camera coordinate frame to 2D pixel coordinates using the focal lengths and principal point”; K is for the raw (distorted) image and P is the projection matrix for the rectified image. If the camera is uncalibrated K[0] is 0. The Image message’s frame_id should be the optical frame with +x right, +y down and +z into the image.

StepDetail
1. TransformPoints from the LiDAR frame into the camera optical frame
2. CullDrop points with Z ≤ a small positive value (behind or at the lens)
3. Projectu = fx·X/Z + cx, v = fy·Y/Z + cy
4. BoundsKeep only 0 ≤ u < width and 0 ≤ v < height
5. DistortionUse the rectified image with P, or apply the distortion model to the normalised coordinates

4. Advantages and disadvantages

  • Rectified image with P is simplest; raw image with K and distortion avoids resampling.
  • Per-point colour lookup costs compute on dense clouds; downsample first.
  • A time offset between the sensors shifts projections during motion even with a perfect calibration.

5. Requirements and constraints

  • Points are in the optical frame (Z forward) when projected.
  • Intrinsics match the image used (resolution, rectification).
  • Image and cloud are close in time.

6. Architecture

cloud (lidar frame) ─▶ T_CL ─▶ camera optical frame ─▶ cull Z ≤ ε ─▶ K ─▶ pixel (u, v) ─▶ bounds check

7. Implementation

  1. Read K (or P) and the image size from CameraInfo.
  2. Transform the cloud into the camera optical frame.
  3. Cull points behind the camera and project the rest (below).
  4. Draw the points on the image coloured by depth.
  5. Check that edges of objects in the cloud line up with edges in the image, at several ranges.
import numpy as np
K = np.array([[600.0, 0, 320], [0, 600.0, 240], [0, 0, 1]])
W, H = 640, 480
def project(points_cam):
    out = []
    for X, Y, Z in points_cam:
        if Z <= 0.1:
            out.append(None); continue                  # behind or too close to the image plane
        u, v, w = K @ np.array([X, Y, Z])
        u, v = u / w, v / w
        out.append((round(float(u), 1), round(float(v), 1)) if 0 <= u < W and 0 <= v < H else None)
    return out
pts = [(0, 0, 5), (1.0, -0.5, 4.0), (-3.0, 0, 2.0), (0, 0, -2.0), (0.2, 0.1, 0.05)]
for p, r in zip(pts, project(pts)):
    print(p, "->", r if r else "dropped")

Printed (checked by the site’s tests):

(0, 0, 5) -> (320.0, 240.0)
(1.0, -0.5, 4.0) -> (470.0, 165.0)
(-3.0, 0, 2.0) -> dropped
(0, 0, -2.0) -> dropped
(0.2, 0.1, 0.05) -> dropped

Reading it: a point 5 m straight ahead lands at the principal point (320, 240); a point at (1.0, −0.5, 4.0) lands at (470, 165); a point 3 m to the left at 2 m depth falls outside the 640-pixel width and is dropped; the point behind the camera and the one at 5 cm depth are dropped. K here is illustrative.

8. Real examples

  • Colouring a cloud: project each point, read the pixel colour, and publish a coloured cloud.

9. Failure scenarios

  • Points behind the camera project to mirrored positions if the Z test is skipped.
  • Using K with a rectified image (it should be P).
  • Image and cloud time stamps differ by 100 ms on a moving robot.

10. Troubleshooting

  1. If the whole overlay is shifted, suspect extrinsic rotation or time offset.
  2. If it is scaled or warped toward the edges, suspect intrinsics or distortion.
  3. If it looks mirrored, check the optical-frame permutation.

11. Validation

  • The example prints the values shown (executed by the site tests).
  • Edges of a flat target line up within the pixel tolerance at 5, 10 and 20 m.

12. Engineering checklist

13. Further references

Related on FixTheRobot