Lidar Obstacle Detector — project output (available on github)

Building a LiDAR Obstacle Detector in C++

Understanding LiDAR, Point Cloud Processing, and Sensor Fusion Technologies

13 min readJun 1, 2025

Upon completing the Sensor Fusion Nanodegree from Udacity, I built a Lidar-based obstacle detection pipeline in C++. To get it up and running, I first adapted the original codebase to work with modern libraries and toolchains — updating the build system, resolving dependency issues, and ensuring compatibility with the latest versions of PCL and C++ compilers.

This project focuses on processing Lidar point cloud data in urban driving scenarios. But before we dive into the implementation, I’ll walk through the fundamentals of Lidar — why it matters, how it works, and how it addresses the limitations of other sensors like cameras and radar. (Hint: sensor redundancy is not just nice to have, it’s also highly critical for safety.)

Why Lidar Matters

LiDAR (Light Detection and Ranging) is a laser-based sensing technology that captures highly accurate 3D measurements of the environment. Unlike cameras, which rely on ambient light and are sensitive to lighting conditions, Lidar actively emits thousands — and up to millions per second —of infrared laser pulses across a wide field of view, often 360°, and calculates the time it takes for each pulse to bounce back. This enables precise distance measurements, even in poor lighting or high-glare conditions where cameras typically struggle.

Beyond depth, Lidar also captures intensity values, which can provide insight into surface materials. The result is a dense, high-resolution 3D map of the environment, making it a powerful tool for perception in autonomous systems. Despite its current cost, Lidar offers critical advantages in robustness and reliability that are essential for real-time decision-making and collision avoidance.

How the Detector Works

Our Lidar point cloud data (in .pcd files) consists of 3D data points (x, y, z + intensity), capturing spatial snapshots of the surroundings in a highway driving. The data is recorded at a frequency of approximately 10 Hz (roughly every 0.1 seconds). In this project, the raw point cloud is processed through a series of filtering, segmentation, and clustering techniques — to be more specific:

Voxel Grid Filtering: Downsamples the point cloud to speed up processing while preserving spatial structure.
RANSAC Plane Segmentation: Identifies the road surface by fitting a planar model, separating ground points from objects.
Euclidean Clustering with KD-Trees: Groups nearby points to identify distinct obstacles like vehicles or pedestrians.

These steps transform unstructured Lidar data into structured obstacle detections with bounding boxes, essential for path planning, situational awareness, and safe navigation. This is basically the bulk of the obstacle detection project, but it’s more than just a coding exercise.

LiDAR-based obstacle detection is a foundational component of autonomous driving safety.

Unlike cameras, Lidar provides reliable depth data regardless of lighting conditions. Detecting and tracking obstacles with high spatial accuracy is critical for path planning, collision avoidance, and safe vehicle operation. That’s what makes this project so essential — it’s a stepping stone toward more intelligent and responsible mobility systems.

GitHub - moorissa/lidar-obstacle-detector: Lidar Obstacle Detector

Lidar Obstacle Detector. Contribute to moorissa/lidar-obstacle-detector development by creating an account on GitHub.

github.com

You can find the step-by-step instructions on Github. In this post, I’ll walk through with you how the Lidar data is filtered, segmented, and clustered to detect obstacles in a 3D environment — and share some key lessons from building this from scratch using C++.

The Lidar Model and Simulated Point Clouds

To simulate the point cloud generation process, a simplified lidar model is used. This model takes in parameters such as the maximum and minimum ray distances, angular resolution, and a list of surrounding vehicles. Based on these inputs, it generates a set of rays that represent the lidar beams.

The lidar’s scan function then performs raycasting — a 3D rendering technique that simulates the path of light rays from a camera perspective. Each ray is checked for collisions with nearby vehicles or the ground plane. The result is a point cloud that captures the scene, with a small amount of Gaussian noise added to mimic real-world sensor imperfections. A simple highway scene illustrating this process is shown below.

Simple Highway Lidar Visualization (Raw PCD File Data)

Point Cloud Data Preprocessing

Next, we’ll work with real world point cloud data collected from an actual lidar. This data is typically stored in .pcd files (github link to data used for this project). The same processing techniques used on a single point cloud can also be extended to handle a continuous stream of incoming point clouds.

The image above shows a high-resolution point cloud that covers a large area. To help the processing pipeline handle the data more efficiently, the point cloud needs to be filtered down. There are two main techniques used for this: Voxel Grid Filtering and Region of Interest (ROI) Cropping.

1. Voxel Grid Filtering

Voxel Grid Filtering is a downsampling method for point clouds. This technique utilizes a voxelized grid approach to reduce the number of points in the dataset. It first creates a 3D voxel grid (a voxel can be thought of a small 3D box in space that uses a lot less memories than individual point clouds), with the resolution being controlled by input parameters. The resolution should be low enough to help speed up processing, but not so low that object definition is completely lost. Then, all the points in each of these voxels will be approximated with their centroid.

Why is downsampling important?

Due to the high volume of data generated, transmitting raw LiDAR point clouds across a vehicle’s internal network is inefficient. To address this, the data is downsampled and converted into a more compact representation known as stixels — short for “stick” and “pixel”. Originally developed for stereo camera processing, stixels are vertical rectangular segments (like matchsticks) that approximate object surfaces in the scene.

2. Region of Interest (ROI) Cropping

The lidar scan extends over a large distance from the ego vehicle. This can be cropped to retain only useful information and hence reduce processing time. An appropriate region of interest includes a good amount of space in front of the ego vehicle so that it is able to react quickly in time to any obstacles moving towards it. For the sides, at least the width of the road should be covered. It would also be beneficial to remove points that are hitting the roof of the ego car.

The filtered and cropped point cloud data is shown below.

Point Cloud Preprocessing Guidelines

To get the best results from your point cloud processing pipeline, consider the following adjustments:

Voxel Grid Downsampling: Use a voxel size large enough to improve performance, but small enough to preserve object shapes. Start with values like 0.2m or 0.3m, and tune based on your scene and processing speed.
Region of Interest (ROI): Define a region that captures enough space in front of the vehicle to detect and react to approaching obstacles, road width on both sides to detect nearby objects or vehicles, and one that ensures all critical obstacles are within the ROI for effective perception.
Camera Views (Optional): Adjust camera angles in environment.cpp to help choose and visualize your ROI. Use a top-down view for layout context. Use a side view to inspect vertical filtering and bounding boxes.
Remove Ego Vehicle Roof Points: Use pcl::CropBox to isolate the ego car’s roof point indices. Also pass those indices to pcl::ExtractIndices to remove them, just like in point cloud segmentation. This helps prevent false detections from the car’s own structure.
Debug with Bounding Boxes: Use the renderBox function to visualize the size and placement of bounding boxes in the scene. Helpful for checking if your ROI, voxel size, and filtering logic are working as intended.

Point Cloud Segmentation

One of the key objectives of lidar point cloud processing is to separate the road plane from potential obstacles. If the road is flat it’s fairly straightforward to pick out road points from non-road points. To achieve this, planar segmentation based on the random sampling consensus (RANSAC) algorithm is used.

RANSAC (Random Sample Consensus) Plane Segmentation

RANSAC stands for random sampling consensus and is a method for detecting outliers in data. The algorithm runs for a set number of iterations and returns the model that best fits the data. Each of these iterations randomly picks a subset of data and fits a model such as a line or plane through it. The iteration with the highest number of inliers or lower noise is then used as the best model. To be precise, here is a step-by-step instruction to detect obstacles in a point cloud scene by separating them from the road surface (which is not an obstacle).

Assumptions: Use RANSAC-based Planar Segmentation to find the road, assuming it’s flat. The largest planar surface is assumed to be the road.
Implementation Overview — use the ProcessPointClouds class in processPointClouds.cpp for Filtering, Segmentation (using RANSAC), Clustering, and Loading/saving PCD files.
Steps:

Implement the SegmentPlane function: (1) Set MaxIterations and distanceThreshold for RANSAC, and (2) Extract inliers (points that lie on the road plane)
Implement the SeparateClouds function: Use PCL’s ExtractIndices to separate roadCloud (inliers) from obstacleCloud (non-inliers)

The output (visualization of segmented point clouds) would be something like Road as one cloud (e.g. colored in green), and Obstacles like cars or trees as another cloud (e.g. colored in red).

There are different variations to the RANSAC algorithm. One type selects the smallest possible subset of points to fit. For a line, that would be two points, and for a plane three points. Then the number of inliers are counted, by iterating through every remaining point and calculating its distance to the model. The points that are within a certain distance to the model are counted as inliers. The iteration that has the highest number of inliers is then the best model

Other methods of RANSAC could sample some percentage of the model points, for example 20% of the total points, and then fit a line to that. Then the error of that line is calculated, and the iteration with the lowest error is the best model. This method might have some advantages since not every point at each iteration needs to be considered. It’s good to experiment with different approaches and time results to see what works best. The following graphic shows a 2D RANSAC algorithm

The output of the planar segmentation process is a pair of point clouds — one that represents the road and the other than represents obstacles. The segmented PCD data is shown below.

Euclidean Clustering

Once the obstacles and road points have been segmented, the next step is to cluster the points that represent the different obstacles. Clustering is like drawing boundaries across a group of points and group them as an object eg “car” or “pedestrian”, and in essence different from segmentation in the following ways.

One way to perform clustering is using the Euclidean clustering algorithm. The idea is to create association between groups of points depending on how close they are. This involves performing a nearest neighbor search and to do this efficiently, a data structure such as a KD-Tree is required.

KD-Tree Implementation

A KD-Tree is a K-dimensional binary search tree that organizes data spatially by splitting points between alternating dimensions. By doing this, KD-Tree enables efficient nearest neighbor search with a time complexity of O(log(n)) as opposed to O(n). This is primarily because, by grouping the points into regions in a KD-Tree, the search space is narrowed down drastically and expensive distance computations for potentially thousands of points can be avoided. The algorithm used to construct a KD-Tree is explained here. The 2D points before and after spacial splitting are shown below. Here, the blue lines indicate X dimension splits and red lines indicate Y region splits

Once points are able to be inserted into the tree, the next step is being able to search for nearby points inside the tree compared to a given target point. Points within a distance tolerance are considered to be nearby.

The naive approach of finding nearby neighbors is to go through every single point in the tree and compare their distances with the target, selecting point indices that fall within the distance tolerance of the target.

Instead with the KD-Tree, a boxed square of size 2 X distance tolerance, centered around the target point is used. If the current node point is within this box, only then the Euclidean distance is calculated and depending on this, it can be determined if the point should be added to the list of nearby points. Further, if this box does not cross over the node division region, the branch on the other side of the region is completely skipped. If it crosses over, then that side is recursively explored. This is shown in the image below. The red crosses indicate regions which were entirely skipped.

Once the KD-Tree method for searching nearby points is implemented, the next step is to implement a euclidean clustering method that groups individual cluster indices based on their proximity. Inside cluster.cpp there is a function called euclideanCluster which returns a vector of vector ints, this is the list of cluster indices.

To perform the clustering, iterate through each point in the cloud and keep track of which points have been processed already.
For each point add it to a list of points defined as a cluster, then get a list of all the points in close proximity to that point by using the search function from the previous exercise.
For each point in close proximity that hasn’t already been processed, add it to the cluster and repeat the process of calling proximity points.
Once the recursion stops for the first cluster, create a new cluster and move through the point list, repeating the above process for the new cluster.
Once all the points have been processed, there will be a certain number of clusters found, return as a list of clusters.

The clustered 2D space is shown below.

The clustered real world PCD data is shown below.

How to improve the tree

To optimize search performance in a k-d tree, it’s important to keep the tree balanced by evenly dividing the point space during insertion. This is achieved by alternating splits between the x and y axes and always inserting the median point along the current axis.

For example, given the following four 2D points:

(-6.3, 8.4), (-6.2, 7), (-5.2, 7.1), (-5.7, 6.3)

We would insert the points in this order: 1. (-5.2, 7.1) — median of the x-values (first x-split) 2. (-6.2, 7) — median of the remaining points sorted by y (y-split) 3. (-5.7, 6.3) — x-split (lower median of remaining two) 4. (-6.3, 8.4) — last point

Choosing medians at each level ensures that the regions are split as evenly as possible, resulting in a well-balanced tree and significantly faster nearest-neighbor or range queries.

Bounding Boxes

As a final touch, bounding boxes can be added around the clusters. The bounding box volume could also be thought of as space the car is not allowed to enter, or it would result in a collision.

In this method of generating bounding boxes, the boxes are always oriented along the X and Y axis. This is acceptable if the cluster has the majority of its points oriented along these axes. However, if the cluster has a very long rectangular object at a 45 deg angle to the X axis, then the resulting bounding box would be unnecessarily large and would constrain the ego vehicle’s available space to move around.

In the image above, the bounding box on the right and is more efficient, taking into account the rotation about the Z axis and containing all the points with the minimum area required. This project does not include code for generating such a box, but techniques like principal components analysis can be used to identify the primary axis of the points and a quaternion member can be used for the rotation.

Concluding Thoughts

We’ve covered understanding from how LiDAR sensors work, to simulating point cloud data, then to implementing filtering, segmentation and clustering techniques. Building a LiDAR obstacle detector from scratch in C++ was more than just an exercise in point cloud processing — it was a deep dive into the core perception systems that enable autonomous vehicles to understand and safely navigate the world. It was a reminder of a fundamental principle in robotics and autonomy, that perception is power.

The ability to accurately detect and classify obstacles in a dynamic environment forms the backbone of collision avoidance, path planning, and ultimately, trust in autonomous systems.

As autonomy continues to evolve, LiDAR remains a critical part of the sensor suite — offering depth, precision, and redundancy in complex driving scenarios. By implementing these techniques at the low level in C++, I’ve gained a much deeper appreciation for the complexity behind seemingly simple driving decisions.

We can also see this exercise as a framework that applies to any safety-critical system relying on visual perception — drones, surveillance, robotics, even defense applications. Feel free to check out the full code, documentation, and step-by-step instructions on my GitHub repo and try it out for yourself. Whether you’re a student, researcher, or engineer breaking into the world of robotics, I hope this project helps demystify LiDAR and inspires you to build the future of intelligent mobility.

CodeX