Camera + LiDAR Fusion: How to Build a Perception Stack for an Outdoor UGV
Adrian Krzemiński,
TL;DR: Camera + LiDAR fusion in ROS 2 combines dense color information from an RGB or RGB-D camera with accurate range measurements from a LiDAR to produce a colored point cloud usable for outdoor UGV navigation, semantic mapping, and obstacle classification. The pipeline has four stages: extrinsic calibration between the two sensors, time synchronization of their topics, projection of 3D LiDAR points into the image plane, and per-point color assignment. This article walks through each stage with working ROS 2 code, a TF tree diagram, and integration notes for outdoor platforms such as the Leo Rover and Raph Rover.
Why camera + LiDAR fusion matters for an outdoor UGV
An outdoor unmanned ground vehicle operates in conditions where neither sensor alone is sufficient. A camera gives you texture, color, and semantic cues, but it struggles with low light, lens flare, and metric depth at range. A LiDAR returns precise distances at tens of meters but produces sparse, colorless geometry. Camera + LiDAR fusion in ROS 2 lets you exploit the strengths of both: you keep the geometric accuracy of the LiDAR and gain the perceptual richness of the camera.
For field robotics applications such as agricultural inspection, planetary analog testing, or infrastructure surveying, a fused perception stack is often the minimum viable sensor configuration. Fictionlab’s mobile platforms are frequently used in exactly these scenarios, and a well-built fusion pipeline is what separates a demo robot from a deployable one.
What hardware setup the pipeline assumes
Before describing the software stack, the assumed hardware configuration on the UGV needs to be clear. The example throughout this article uses:
- An Intel RealSense D435i as the RGB-D camera, providing a 1280×720 color stream and an integrated IMU.
- A 3D LiDAR such as a Velodyne VLP-16 or an Ouster OS0, publishing
sensor_msgs/PointCloud2. - A ROS 2 Humble or Jazzy environment running on the rover’s onboard computer.
- A rigid mechanical mount so the camera-to-LiDAR transform stays constant.
The TF tree for this setup looks like this:
base_link → lidar_mount → lidar_link
base_link → camera_mount → camera_link → camera_color_optical_frame
The fusion node needs a transform that maps points from lidar_link into camera_color_optical_frame. This is what extrinsic calibration produces.
How to perform extrinsic calibration between camera and LiDAR
Extrinsic calibration estimates the 6-DoF rigid transform T_camera_lidar that maps points from the LiDAR frame into the camera optical frame. Several open-source tools work in ROS 2; a widely used one is direct_visual_lidar_calibration by Koide et al., which supports targetless calibration using natural edges in the scene. For target-based calibration, a checkerboard plus a LiDAR-visible planar board is a classical approach.
A reproducible workflow looks as follows:
- Record a rosbag with both sensors observing a static scene containing strong geometric features (a building corner, a parked vehicle, a calibration target).
- Run the calibration tool to obtain the transform as roll, pitch, yaw, x, y, z or as a 4×4 matrix.
- Publish the result as a static transform.
The static transform publisher entry in your launch file, with placeholder values replaced by your calibrated transform:
Node(
package='tf2_ros',
executable='static_transform_publisher',
arguments=['0.12', '0.00', '0.18', '0.000', '0.000', '0.000', '1.000', 'camera_color_optical_frame', 'lidar_link']
)
Verify the calibration by overlaying projected LiDAR points on a camera image. Misalignment greater than roughly 5 pixels at 5 m often means the extrinsics, timestamps, or image rectification need refinement.
How to synchronize camera and LiDAR topics in ROS 2
Camera frames and LiDAR scans arrive at different rates (typically 30 Hz and 10 Hz) and with different latencies. Fusing them without synchronization produces motion smear, especially when the UGV is turning. ROS 2 provides message_filters with ApproximateTimeSynchronizer for this purpose.
A minimal synchronizer node in Python:
import rclpy
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
from sensor_msgs.msg import Image, CameraInfo, PointCloud2
from message_filters import Subscriber, ApproximateTimeSynchronizer
class FusionNode(Node):
def __init__(self):
super().__init__('fusion_node')
img_sub = Subscriber(self, Image, '/camera/color/image_rect_raw', qos_profile=qos_profile_sensor_data)
info_sub = Subscriber(self, CameraInfo, '/camera/color/camera_info', qos_profile=qos_profile_sensor_data)
pc_sub = Subscriber(self, PointCloud2, '/velodyne_points', qos_profile=qos_profile_sensor_data)
self.sync = ApproximateTimeSynchronizer([img_sub, info_sub, pc_sub], queue_size=10, slop=0.05)
self.sync.registerCallback(self.callback)
def callback(self, img_msg, info_msg, pc_msg):
self.process(img_msg, info_msg, pc_msg)
A slop of 50 ms is a reasonable starting point for a 10 Hz LiDAR and a 30 Hz camera. For high-speed driving, consider hardware-triggered cameras or PTP/PPS-based time synchronization between the LiDAR and the onboard computer, if supported, to reduce timestamp drift.
How to project LiDAR points into the camera image
With calibrated extrinsics and synchronized messages, the next step is projecting each 3D point from the LiDAR frame into the 2D image plane using the camera intrinsics from CameraInfo. The equations below assume a rectified image; if you use image_raw, first undistort the image or apply the camera distortion model from CameraInfo.
The projection equation for a point P_lidar = (X, Y, Z):
- Transform to camera frame:
P_cam = T_camera_lidar * P_lidar. - Discard points with
P_cam.z <= 0(behind the camera). - Apply intrinsics:
u = fx * X_cam / Z_cam + cx,v = fy * Y_cam / Z_cam + cy. - Keep only points where
0 <= u < widthand0 <= v < height.
In NumPy, vectorized for performance:
import numpy as np
import sensor_msgs_py.point_cloud2 as pc2
def project(points_lidar, T, K, w, h):
ones = np.ones((points_lidar.shape[0], 1))
hom = np.hstack([points_lidar, ones])
pts_cam = (T @ hom.T).T[:, :3]
mask = pts_cam[:, 2] > 0
pts_cam = pts_cam[mask]
uv = (K @ (pts_cam.T / pts_cam[:, 2])).T[:, :2]
in_img = (uv[:, 0] >= 0) & (uv[:, 0] < w) & (uv[:, 1] >= 0) & (uv[:, 1] < h)
return pts_cam[in_img], uv[in_img].astype(int)
For a VLP-16 producing approximately 300,000 points/s and a 720p camera, this vectorized projection usually runs in single-digit milliseconds on a modern x86 CPU, not counting all ROS message conversion overhead.
How to color the LiDAR point cloud
The last stage assigns each projected point an RGB value from the corresponding image pixel and republishes a PointCloud2 with the rgb field populated.
Inside your synchronized callback:
from cv_bridge import CvBridge
import struct
bridge = CvBridge()
img = bridge.imgmsg_to_cv2(img_msg, 'bgr8')
points = np.array(list(pc2.read_points(pc_msg, field_names=('x','y','z'), skip_nans=True)))
K = np.array(info_msg.k).reshape(3, 3)
pts_cam, uv = project(points, T_camera_lidar, K, img.shape[1], img.shape[0])
colors = img[uv[:, 1], uv[:, 0]]
rgb_packed = (colors[:, 2].astype(np.uint32) << 16) | (colors[:, 1].astype(np.uint32) << 8) | colors[:, 0].astype(np.uint32)
Publish the result on /fused/points with fields x, y, z, rgb. If you publish pts_cam, set the cloud frame to camera_color_optical_frame; if you want the cloud in lidar_link or base_link, transform the coordinates accordingly. Visualize in RViz2 by setting the point cloud’s Color Transformer to “RGB8”.
How to integrate the fused cloud into the navigation stack
A colored point cloud is more than a visualization. You can feed it into downstream nodes such as:
- Nav2 costmaps via the
VoxelLayer, treating high points as obstacles while using a preprocessing/color-filter node to ignore vegetation (green-dominant points) in agricultural scenarios. - Semantic segmentation by running the RGB image through a model such as DeepLabV3, then transferring class labels onto the projected LiDAR points.
- SLAM systems like LIO-SAM or Fast-LIO2, where the LiDAR geometry remains the primary input and fused color can help with map inspection, annotation, or external loop-closure validation in visually distinctive environments.
Outdoor UGV platforms used in academic research and in space analog missions increasingly rely on this kind of fused output rather than raw sensor topics.
FAQ
How accurate does the extrinsic calibration need to be?
For obstacle avoidance at speeds below 2 m/s, a rotational error below 0.5 degrees and translational error below 2 cm are usually sufficient. For dense semantic mapping at range, aim for under 0.2 degrees rotational error.
Can you use a 2D LiDAR instead of a 3D one?
Yes, the projection math is identical, but you only get a single scan line of colored points per frame. This is enough for lane-following or row-following tasks but not for full 3D obstacle classification.
What if the camera and LiDAR have very different fields of view?
Only the LiDAR points falling inside the camera FOV will be colored. The remaining points keep no color or a default value. You can mount two or three cameras around the robot to cover more of the LiDAR’s 360-degree field.
How do you handle rolling shutter on the RGB camera?
Rolling shutter introduces a row-dependent timestamp offset. For slow UGV motion this is usually negligible. For fast platforms, either use a global-shutter camera or compensate by interpolating the platform pose across image rows using IMU or odometry data.
Is GPU acceleration necessary?
For the projection and coloring stages described here, no. CPU NumPy is usually fast enough for 10 Hz operation. GPU becomes relevant when you add neural network inference on top, for example semantic segmentation.
Does this pipeline work with stereo cameras instead of RGB-D?
Yes. The fusion math only needs an RGB image and a camera intrinsics matrix. The depth channel from an RGB-D camera is not used in this specific pipeline; the LiDAR provides depth.
How do you debug a misaligned fusion result?
Visualize the projected LiDAR points as colored dots on top of the live camera image in RViz2 or OpenCV. Misalignment patterns often reveal whether the error is in rotation, translation, timestamp synchronization, or image rectification.
Build your fusion stack on a field-tested UGV
If you are designing a perception system for outdoor research, inspection, or analog missions and want a rover chassis that already supports ROS 2, sensor mounting, and onboard compute, Fictionlab’s team builds tailored configurations around the Leo Rover and Raph Rover platforms. Talk to the engineering team through the custom robotics page to discuss your sensor payload and integration requirements.