Cover photo courtesy Andrew Lauer.

## Introduction

Lidar camera calibration is the process of estimating the pose of a camera relative to a lidar. To achieve this, one must find corresponding points in space between the two perspectives. The most straightforward way to do this is by locating an object that appears in both the lidar and camera views.

As with any model, the more data points available, the more accurate the resulting calibration will be. There are two simple methods of achieving this:

(a) Detect multiple objects in different locations simultaneously

(b) Track one object that moves to different positions over time

This blog explains the second approach, and the challenges/methodology associated with each step along the way. 

## Part 1: Defining Prerequisites

Before beginning calibration, one must first consider the form and collection of the data used. The goal is to have many matching lidar and camera frames of the same object in different locations. The simplest solution? A lidar and a camera recording of the same moving object.

Lidar recording is simple; Cepton already has a well-defined architecture for the collection and handling of lidar data.

Camera recording, on the other hand, is unfortunately less trivial. Different camera manufacturers have different SDKs, and most of them don't play well with Rust—the language of choice for Cepton software. Luckily, the camera manufacturer we chose to support, Basler, provided a C++ library for which someone wrote a Rust linking wrapper for: [Pylon CXX](https://crates.io/crates/pylon-cxx).

But just having two recordings isn't enough; The lidar and camera data must be synchronized. Since the object is moving, we need lidar and camera frames of the same timestamp to get corresponding points.

Thus, we require three things to start calibration:

(a) A lidar recording

(b) A camera recording

(c) Some way of syncing the lidar and camera

Ideally, for ease of handling and future-proofing the data, we can have one piece of data/software that collects and handles all three pieces.

Luckily, such a system already exists:

## Part 2: The CR (Cepton Record) Format

The Cepton Record format was designed for this exact purpose: To collect multiple streams of data and combine them into one `.cr` file. It supports streams of lidar frames, images, and IMU data (although IMU is an orientation sensor, and isn't used in this project).

Perfect right? But there's one catch: the CR format supports *images*, not *video*. This means that in order to utilize the existing CR format framework, we would need to write every single image for every camera frame to the resulting file.

Optimally, for machine learning purposes, we want to support video frames that are both frequent and high resolution. But both of these factors make it extremely space-intensive to write every single camera image.
### Example: Why Writing Individual Frames Isn't Feasible

Let's start with some back-of-the-envelope math.

With the Basler acA2440-20gm, we can get up to 2448 x 2048 frames at 23 fps.

At no image compression (writing `.bmp` files), a `.cr` of a ~15-second recording would be estimated to look something like this:

${15 sec} \times {23 frames/sec} \times {{(2448 \times 2048)} pixels/frame} \times {24 bits/pixel} = {{41511813120} bits} = {{5188976640} bytes} \approx {{5189} MB}$

Yikes. 5 gigabytes for a 15-second recording. Not to mention that this is an underestimate; We're not even taking into account the size of the bitmap header.

We can try compressing the image to different formats, like `.jpg`. Assuming an aggressive (and unrealistic) 50 to 1 compression ratio, we get:

${{5189} MB} \times \frac{1}{50} \approx {{104} MB}$

Definitely better. But compressing images along with recording takes time, and is difficult to make fast enough to match the frames per second. This means we end up with a backlog that grows the longer we record (eating up memory and performance).

In comparison, an `.mp4` is usually magnitudes smaller than the JPGs. This is because video formats (like `.mp4`) get most of their space savings due to their ability to encode differences between adjacent frames rather than the entire frame itself.

To illustrate this:
### A Real Example

Here is a 30-second video of a moving chessboard:


 <img src="/images/lidar_camera_calibration_pipeline/video.png" width=50% height: auto/>


It's 10.1 MB.

Using ffmpeg, the video is split into separate images, one for every frame. This is done three times, with the images being in the formats `.bmp`, `.png`, and `.jpg`. Each time, the resulting images are put in a folder named bmp, png, and jpg, respectively.

Here is the BMP folder for reference:


 <img src="/images/lidar_camera_calibration_pipeline/folder-example.png" width=50% height: auto/>


Now, here are the sizes of the resulting splits:


 <img src="/images/lidar_camera_calibration_pipeline/sizes.png" width=50% height: auto/>


Surprisingly, JPG compression is much better than expected. However, it is still 6x larger than the MP4.

This example makes the limitations of frame-by-frame storage clear.

### The Solution: Use Videos

As the header says, the solution is to just use videos. They are better in both space efficiency and accessibility.

But this introduces a new challenge: How do we integrate videos into the CR format?

Simply writing the entire `.mp4` into the `.cr` is both clunky and hard to validate. Not to mention, we'll need to write a custom `.mp4` parser if we want to efficiently read the video(s) from and to part(s) of a `.cr` file—far outside the scope of this task.

Let's take a step back. Consider the third piece of data, the offset. Notice, one will never have an offset without a corresponding video.

What if each offset record also came with a relative file path to a `.mp4` file written outside the `.cr`?

Unfortunately, this solution means compromising on the original goal of not having multiple files. But, it comes with multiple upsides as well:

(a) Easier parsing

(b) Easier validation (you can just open up the `.mp4` manually)

(c) Faster development time

As such, this is the solution we chose.

## Part 3: Recording Data

The Cepton Record format is accompanied by a corresponding Rust tool: The Cepton Record Recorder. It already supports recording with lidar and IMU, and modifying it to support a camera as well is not too difficult with the existence of the previously mentioned [Pylon CXX](https://crates.io/crates/pylon-cxx).

However, we encountered several challenges.

### Capturing Video from Camera Frames

The first issue was that Pylon provides access to each frame's image when recording; It doesn't support native video recording. Thankfully, tools [ffmpeg](https://ffmpeg.org) which turn images into video already exist. Using it through the [ffmpeg-sidecar](https://crates.io/crates/ffmpeg-sidecar) crate, we can write a camera's separate image frames into one aggregate `.mp4` at a speed which keeps up with the frame rate.

### Synchronizing Lidar and Camera

The second issue was related to time syncing. Since we only record the offset between the beginning of the video and the lidar, we need to accurately determine the time between a frame and the start of the recording.

This is trivial for the lidar: Every frame has its own timestamp.

The video, however, is trickier. While videos have a constant frame rate, the actual delivery of the frames from the camera is inconsistent: Every once in a while, there will be a frame that is "dropped" (not received). Thus, we need a way to "fill" in the gaps.

The solution: Assume the fetching of images is non-deterministic. At every timestamp where we expect a frame, we write the **most recent successfully fetched** frame.


 <img src="/images/lidar_camera_calibration_pipeline/frames.png" width=50% height: auto/>


This method handles dropped frames rather gracefully. While it does introduce small timing inaccuracies, it is mostly negligible for our purposes; The calibration objects should not be moving fast enough for that to matter.

## Part 4: Lidar Camera Calibration

With both synchronized recordings, we can finally feed the data into a pipeline that takes corresponding frames, finds a common object, and then estimates the pose of the camera.

While the input portion of the pipeline (reading in recordings and matching the frames by timestamps) is still in progress, a basic version of the calibration pipeline has been implemented.

Andrew Lauer has both implemented and written an excellent explanation of the calibration process in repository [ceptontech/lidar_camera_calibration](https://github.com/ceptontech/lidar_camera_calibration), which he has graciously given permission for me to share:

---

Lidar camera calibration is the process of finding the projection matrix $P$, a 3 by 4 matrix that projects a 3d world point $X_i$ to a 2d image point $x_i$ in a camera. The projection matrix also encodes useful information about the position of the camera and the camera itself. World and image points are expressed in homogeneous coordinates.

The process starts by matching 2d points in the image to corresponding 3d points in the point cloud. This can be done using any standard method using chessboards, fiducial markers, or feature detection. However, since the point cloud contains 3d information, the points must be projected to a plane, similar to a camera. The following equation shows how the projection matrix relates world points to image points.

$$PX_i = x_i$$

$$\begin{bmatrix}p_{11} & p_{12} & p_{13} & p_{14} \\\ p_{21} & p_{22} & p_{23} & p_{24} \\\ p_{31} & p_{32} & p_{33} & p_{34}\end{bmatrix}\begin{bmatrix}X \\\ Y \\\ Z \\\ 1\end{bmatrix}=\begin{bmatrix}u\\\ v\\\ 1 \end{bmatrix}$$

This can be rewritten to express the image point as follows:

$$x_i = \begin{bmatrix}\frac{p_1^TX_i}{p_3^TX_i}\\\ \frac{p_2^TX_i}{p_3^TX_i}\\\ 1\end{bmatrix}=\begin{bmatrix}u\\\ v\\\ 1\end{bmatrix}$$

And then for each world/image pair:

$$p_1^TX_i-u_ip_3^TX_i=0$$

$$p_2^TX_i-v_ip_3^TX_i=0$$

Doing this for all pairs of world and image points, we can construct the matrix A as follows:

$$A \vec{p} = \begin{bmatrix} X_1^T & \vec{0} & -X_1^Tu_1 \\\ \vec{0} & X_1^T & -X_1^Tv_1 \\\ \vdots & \vdots & \vdots \\\ X_n^T & \vec{0} & -X_n^Tu_n \\\ \vec{0} & X_n^T & -X_n^Tv_n \end{bmatrix}_{\text{2nx12}} \begin{bmatrix} \vec{p_1} \\\ \vec{p_2} \\\ \vec{p_3} \end{bmatrix} = \begin{bmatrix} 0 \\\ 0 \\\ \vdots \\\ 0 \\\ 0 \end{bmatrix}$$

Applying SVD to $A$ gives us an initial guess for $p$ that minimizes the $Ap$ product. I.e. $p\in Null(A)$.

This guess can be improved by applying the Levenberg-Marquardt non-linear iterative least squares method. The cost function for the LM optimization is the distance between the ground truth image points and the 3d points projected into the image plane.

Once a good estimation for $P$ has been found, the camera intrinsics, rotation, and translation matrices can be found by decomposing the projection matrix with RQ decomposition. This is a straightforward decomposition, since the projection matrix is defined by:

$$P = K[R|t]$$

### Summary

To estimate the pose of the camera relative to the lidar frame, we must find the projection matrix $P$ which maps lidar points to the image plane. This can be done in the following steps:

(a) Match 3D and 2D point pairs

(b) Create a linear system with the point data

(c) Find an initial guess for the solution with SVD

(c) Use Levenberg-Marquardt to refine the solution

(d) Decompose the result to find the camera intrinsics, rotation, and translation matrices


## Part 5: Future Usage

With the entire pipeline implemented, we open ourselves up many possibilities such as:

(a) Depth camera: By projecting all the lidar points into the camera frame, we can effectively simulate a depth camera

(b) Validation and debugging: With synchronized video and lidar data, future lidar recordings will be easier to visually inspect and debug

(c) Lidar-camera system integration: Any system with both lidar and camera will benefit from having an accurate calibration system

However, the most useful application is as follows:

### Utilization of Imagery CV

First, it is important to understand that computer vision models for camera are much more sophisticated than the corresponding models for lidar. This is simply by virtue of the focus of the computer vision industry being primarily on imagery.

By having a way to convert between imagery and lidar, we open ourselves up to the possibility of using computer vision models for imagery to help develop corresponding tools for lidar.

Unfortunately, while the projection matrix provides a way to convert from camera to lidar data, the reverse isn't as simple. Notice that camera data is in two dimensions, and lidar data is in three. This means a degree of information is lost when converting from lidar to camera. And as such, when we convert from camera back to lidar, one degree of freedom is introduced. Each camera point will correspond not to a point, but a line in the lidar.

Despite this ambiguity, this data is still valuable—as a constraint for data expected from our lidar computer vision models.

Consider a computer vision model run on a video of a parking lot. With the objects detected in the frame we can:

(a) Cross-reference it with output generated by a lidar model run on the corresponding lidar data, providing weak supervision/validation

(b) Quickly generate labels for lidar data to train future models on

Both of which will help streamline the process of training future lidar computer vision models.

## Conclusion

The lidar camera calibration pipeline unifies two separate perspectives, paving the way for improved data handling/collection, easier validation/debugging, and also enhanced sensor fusion.

More importantly, by utilizing the maturity of current image computer vision models, we hope to be able to accelerate the training and accuracy of lidar-based models.

Lidar camera calibration is not just about calibration—it's about combining the strengths of lidar and camera models to accelerate computer perception as a whole.

Lidar Camera Calibration Pipeline

## Introduction

The successful development of LiDAR (Light Detection and Ranging) systems is expedited by simulations, i.e. to test in extreme scenarios and reduce costs compared to extensive real-world testing. Though, Cepton's internal real-time simulator, while substantial in simulation cases and objects, cannot always replicate real-world environments due to the requirements of dense, accurate mesh maps that are difficult to replicate procedurally. 
Henceforth, we add new logic to the simulator that lets us load in data recorded from a live sensor to overlay the simulated geometries for more accurate simulation. 


## Background Profile Generator 

The first step in enabling real-world data overlay involves creating a background profile generator that captures the static environment from PCAP recordings. This component processes LiDAR frames to build a depth map representing the furthest observable surfaces.
Our approach was to process the number of frames and update an internal 2D buffer and write to a bitmap file to create the foundational depth data needed for AR simulation overlays.

For example, something like this:

 <img src="/images/pcap_overlay/depth_map.png" width=50% height: auto/>


```rust
depth_map[ez][ex] = distance_mm; //store the max distance per pixel (e.g. depth_map[0][0] = 15420.5mm, 15.42 meters)
```
You can store a lot of information in a 2d bitmap image! Each distance gets packed into a 32-bit RGBA pixel using Cepton's SDK bit-packing scheme:



 <img src="/images/pcap_overlay/encoding.png" width=50% height: auto/>


Encode pixels by converting depth map to sdk pixel format:
```rust
for ez in 0..self.image_height as usize {
 for ex in 0..self.image_width as usize {
 let distance_mm = self.depth_map[ez][ex];

 if distance_mm > 0.0 {
 // it is a valid pixel, so create a proper Pixel struct and encode it correctly
 let mut pixel = Pixel::default();

 // set distance (uses 20 bits: 10 bits in green + 10 bits in blue)
 let distance_u32 = (distance_mm as u32).min(0xFFFFF); 
 pixel.set_distance(distance_u32);

 // set flags (uses 2 bits in alpha channel)
 pixel.set_flags(3); // Valid pixel flag

 // convert pixel to bytes 
 let pixel_bytes = pixel.as_bytes();
 image_data.extend_from_slice(pixel_bytes);
 } else {
 // otherwise this is invalid
 let mut invalid_pixel = Pixel::default();
 invalid_pixel.set_distance(0);
 invalid_pixel.set_flags(0);
 invalid_pixel.set_intensity(0);

 let pixel_bytes = invalid_pixel.as_bytes();
 image_data.extend_from_slice(pixel_bytes);
 }
 }

 // row padding
 image_data.extend(std::iter::repeat_n(0, padding_per_row as usize));
}
```

We output two complementary outputs:

(1) SDK-Compatible Depth Image ready to load into the simulator.

(2) A standard grayscale bitmap for visual sanity checking, where the pixel intensity represents distance. 


 <img src="/images/pcap_overlay/bitmap.png" width=50% height: auto/>



 <img src="/images/pcap_overlay/bitmap_visualized.png" width=50% height: auto/>


## AR Background Integration Pipeline 
From a software engineering perspective, one must consider how to seamlessly integrate this new overlay pipeline into complex preexisting rendering engine architecture. 


 <img src="/images/pcap_overlay/pipeline.png" width=50% height: auto/>


While Cepton's simulation already features a robust multi target rasterization system for synthetic objects, we needed to figure out how to add a parallel AR Pipeline that operates alongside the existing render targets. This addition would preserve the original simulation capabilieis while enabling the new ability to blend with recorded LiDAR data. 

We introduce a new dedicated texture pipeline specifically for background profile data. Our design decision here was to use the R32Float format to store millimeter-precision distance values directly, rather than RGBA position textures which store 3D coordinates. 


```rust
// new pipeline for overlay-specific texture creation
let ar_texture = Texture::create_ar_background_profile_texture(device, &dims, "ar texture");

pub fn create_ar_background_profile_texture(
 device: &Device,
 size: &[u32; 2],
 label: &str,
) -> Self {
 let texture = device.create_texture(&TextureDescriptor {
 label: Some(label),
 size: Extent3d {
 width: size[0],
 height: size[1],
 depth_or_array_layers: 1,
 },
 format: TextureFormat::R32Float, // single-channel distance data
 usage: TextureUsages::TEXTURE_BINDING | TextureUsages::COPY_DST,
 // ... other properties
 });
}

```

We implement the AR pipeline system that does full BMP parsing and uploading,operating 
independently of the rest of the engine rasterization:



```rust
impl ARSimExt {
 fn new(
 device: &Device,
 queue: &Queue,
 layout: &BindGroupLayout,
 texture: Texture,
 bmp_path: String,
 ) -> Self {
 // ...logic to parse and extract encoded distnace values from bitmap
 //...logic to load in file
 let bmp_image = Self::parse_bmp_manually(&bmp_bytes)

 // upload this to the GPU Texture
 Self::write_bmp_to_texture(queue, &texture, &bmp_image);
 
 // create dedicated bind group for AR pipeline
 let bind_group = device.create_bind_group(&wgpu::BindGroupDescriptor {
 label: Some("ar_sim_bind_group"),
 layout,
 entries: &[BindGroupEntry {
 binding: 0,
 resource: wgpu::BindingResource::TextureView(&texture.view),
 }],
 });

 ARSimExt { bmp_image, bind_group, background_profile_texture: texture }
 }
}

```

We then do a GPU texture upload and a parallel pipeline integration. Once again, the AR background profile operates as a parallel data source alongside the traditional rasterization pipeline:

```rust
// traditional rasterization still occurs normally
let mut render_pass_1 = encoder.begin_render_pass(&wgpu::RenderPassDescriptor {
 color_attachments: &[
 Some(position_texture), // Synthetic object positions
 Some(reflectivity_texture), // Synthetic material properties
 Some(object_id_texture), // Synthetic object IDs
 Some(label_texture), // Synthetic semantic labels
 ],
 // ... render synthetic geometry
});
// (consumed later in compute shader, not during rasterization)
```

We then create a separate bind group layout for this overlay to integrate with the existing compute pipeline.

Overall, this integration adds minimal overhead to the existing rasterization -- minimal GPU memory added (a single R32Float texture would be ~16 MB for 2048x2048 resolution), minimal impact to rasterization performance because we preload the data. 

The key insight here is that the AR background integration operates as a data augmentation layer rather than a rendering modification. The existing multi-target rasterization continues unchanged, while the AR system provides an additional data source that gets blended in the subsequent compute shader stage. This design preserves all existing simulation capabilities while enabling seamless real-world data integration.


### Background Integration Compute Shader

The heart of our overlay system lies in a custom WGSL compute shaders that blends real-world data with synthetic geometry. This shader processes each LiDAR ray individually and makes per-pixel decisions about whether to use simulated objects or recorded background surfaces.

The compute shader receices data through distinct bind groups, each serving a different purpose. We integrate a bind group for AR simulation:


```wgsl
// Group 0: TX/RX Buffers
// read write buffers here 

// Group 1: Rasterization Outputs 
// position, reflectivity, object_id, label textures

// Group 2: AR Background Profile
@group(2) @binding(0) var ar_sim_buffer: texture_2d<f32>;

// Group 4: Runtime Configuration
@group(4) @binding(0) var<uniform> ar_config: ARSimConfig;
```

We optimize performance with 256 threads per workgroup for optimal GPU utilization. 

We then utilize compute threads to process a single LiDAR "fire" by calculating the ray direction from encoder coordinates. We then turn the angles into a unit vector pointing exactly where the LiDAR beame is firing. Afterwards, our shader implements a decision tree for each ray: 

```wgsl
// Step 1: sample the synthetic geometry
let point_position = textureLoad(position_texture, pixel, 0);
let sim_distance = length(point_position.xyz);

// Step 2: Check if our AR option is enabled!
if (ar_config.enabled != 0u) {
 // Step 3: Map encoder coords to AR texture coordinates
 let ar_x = u32(norm_x * f32(ar_dims.x - 1u));
 let ar_y = u32(norm_z * f32(ar_dims.y - 1u));
 
 // Step 4: sample the background texture
 let background_distance_mm = textureLoad(ar_sim_buffer, ar_pixel, 0).r;
 
 if background_distance_mm > 100.0 && background_distance_mm < 200000.0 {
 let background_distance_m = background_distance_mm / 1000.0;
 
 // Step 5: choose the closest surface (synthetic vs background)
 if sim_distance > 0.0 {
 // Both surfaces available always use background for consistent environment
 output_position = transform_to_wgpu_coords(background_distance_m);
 output_object_id = 999999u; // Background marker
 output_label = 100u; // Background label
 } else {
 // Only background available
 output_position = transform_to_wgpu_coords(background_distance_m);
 output_object_id = 888888u; // Background-only marker
 output_label = 99u; // Background-only label
 }
 }
 // If background invalid, fall back to synthetic geometry
}
```

### Point Cloud Generation
The final stage of AR Pipeline transforms the GPU-processed ray data into a unifed point cloud to finally combine this background data with synthetic objects. We must move the data from GPU to CPU, and apply coordinate transforms.

Each point undergoes validation to track whether it came from synthetic geometry or background data. Then, we must apply the correct coordinate transforms to convert points from LIDAR's local frame to world coordinates.

Our pipeline involves three different coordinate systems:

(1) Cepton LiDAR coordinates: x=right, y=forward, z=up, which matches the physical sensor orientation

(2) WGPU rendering coordinates: x=right, y=up, z=forward, which is standard graphics convention

(3) World coordinates: the final output space for perception algorithms

For PCAP background data, we try to preserve the real-world spatial relationships while only applying minimal transformations needed to account for the sensor's physical mounting. Hence, we use a "ground-only" transformation that applies only height and pitch--the sensor's mounting height above ground plane and the sensor's tilt angle, respectively. 

The complete transformation matrix is constructed as:

$$T_{final} = T_{coord}^{-1} \times T_{height} \times R_{pitch} \times T_{coord}$$

Where each component is: 

T_coord (Coordinate System Transform):

```markdown
┌ 1 0 0 0 ┐
│ 0 0 -1 0 │
│ 0 1 0 0 │
└ 0 0 0 1 ┘
```
T_height (Height Translation):

```markdown
┌ 1 0 0 0 ┐
│ 0 1 0 h │ 
│ 0 0 1 0 │
└ 0 0 0 1 ┘
Where h = sensor mounting height:
```



R_pitch (Pitch rotation around Z):

```markdown
┌ cos(θ) -sin(θ) 0 0 ┐
│ sin(θ) cos(θ) 0 0 │ 
│ 0 0 1 0 │
└ 0 0 0 1 ┘
where θ = pitch angle

```
This approach ensures that real-world geometry (buildings, roads, trees), maintains its correct spatial relationships, the sensor appears to be mounted at correct height and angle, and synethetic objects added to the scene align properly with the background. This is an example of what an AR overlay looks like at this point:


 <img src="/images/pcap_overlay/overlay_1.png"
 width=50% height: auto/>



 <img src="/images/pcap_overlay/overlay_2.png"
 width=50% height: auto/>



## Scene Editor Integration

We apply similar logic to the simulator's scene editor--the pipeline in the simulator that sets spatial properties of each simulated geometry. We add this 3d overlay just so that we can see the 3d points in the scene editor such that we can place objects around it. 


 <img src="/images/pcap_overlay/scene_editor.png"
 width=50% height: auto/>




## Conclusion
With this pipeline implemented, we create more accurate simulations and data that we hope safely accelerates LiDAR development. 

Additionally, given the maturity of current computer vision models, we hope this improved data expedites both the training efficiency and accuracy of LiDAR-based models. 


## References

Kong, F., Liu, X., Tang, B., Lin, J., Ren, Y., Cai, Y., Zhu, F., Chen, N., & Zhang, F. (2023). MARSIM: A light-weight point-realistic simulator for LiDAR-based UAVs. IEEE Robotics and Automation Letters, 8(5), 2626–2633. https://doi.org/10.1109/LRA.2023.3252599

Real-Time PCAP Data Overlay Over Synthetic Geometries

# Introduction
In the development of StudioViz, a simulator created by Cepton’s in-house software engineers, we aim to implement a feature for detecting collisions between objects. This feature, known as Collision Detection, addresses the computational problem of identifying when two or more spatial objects intersect. It is a critical component in computer graphics, simulation, and various other applications where accurate spatial interactions between objects are necessary.

In the realm of games and simulation engines, collision detection plays a crucial role in ensuring realistic interactions between objects and their environment. This process involves calculating whether and when objects intersect, providing essential feedback for simulating real-world physics and behaviors. By accurately detecting collisions, developers can create environments where objects respond naturally to impacts, enhancing the realism and immersion of the simulation. While this doesn't involve direct obstacle avoidance, it establishes a foundational framework for evaluating collision scenarios during development and testing. This capability is particularly valuable for refining perception algorithms, ultimately contributing to the broader vision of safe and autonomous transportation by ensuring that simulated models behave as expected in various scenarios.

To achieve collision detection, various methods such as bounding volume hierarchies, spatial partitioning, and proximity queries may be employed. These techniques aim to minimize computational overhead while maintaining high accuracy in detecting collisions. The implementation of Collision Detection feature in StudioViz enhances the simulator's capabilities, enabling more realistic and interactive simulations for users. In this blog post, we will cover the algorithm behind collision detection and demonstrate various scenarios that simulate real-world car collisions using Pygame, a comprehensive set of Python modules designed for writing video games.


# Collision Detection Algorithm Explanation
When it comes to collision detection for cars, several methods are available, each with distinct advantages and disadvantages. These methods include Axis-Aligned Bounding Box (AABB), Oriented Bounding Box (OBB), and Separating Axis Theorem (SAT). Here is a detailed examination of each method:

| Methods | Pros | Cons |
|:-------------:|:--------------|:--------------|
| AABB | Computationally inexpensive Easy implementation | Inaccurate for objects that are not axis-aligned |
| OBB | Better accuracy and versatility | More complex implementation |
| SAT | Even higher accuracy and versatility | Most complex implementation |

We have ultimately selected the Separating Axis Theorem (SAT) for car collision detection due to its superior accuracy, despite its complex implementation. When dealing with lidar systems, it is crucial to ensure that collisions are detected with the highest precision. SAT provides the necessary level of accuracy, making it the most suitable choice for our application.

SAT is quite simple in concept. Basically, if you are able to draw at least one straight line between two objects, then those two objects are not colliding. If you are unable to, then they are colliding.



 <img src="/images/2d_collision_detection/Figure1.png" width=50% height: auto/>

Figure 1: In this diagram, the two squares are not colliding, and you are able to draw a straight line through them. 



 <img src="/images/2d_collision_detection/Figure2.png" width=50% height: auto/>

Figure 2: In this diagram, the two squares are colliding, and it is impossible to draw a straight line that separates the two. 


We refer to each straight line as an “axis”. But unlike the usual x-axis and y-axis that you may be used to, any straight line is considered an axis. SAT axes are not just limited to horizontal and vertical axes.



 <img src="/images/2d_collision_detection/Figure3.png" width=50% height: auto/>

Figure 3: All four of these lines are considered “axes”. 


We quickly ran into an issue when writing code for this algorithm. If any straight line is considered an axis, then there are any infinite number of axes to test. This makes it impossible to compute collision detection through sheer brute force.



 <img src="/images/2d_collision_detection/Figure4.png" width=50% height: auto/>

Figure 4: Since any straight line is considered an “axis”, there are technically an infinite number of axes. 


The beauty of SAT comes from its systematic methodology of determining which axes to test. Generally speaking, the SAT operates by projecting the vertices of the shapes onto potential separating axes, which are typically the normal vectors to the edges of the shapes. For each axis, the algorithm checks whether the projections of the two shapes overlap. If there exists at least one axis where the projections do not overlap, it indicates a separating axis, and the shapes are not colliding. Conversely, if the projections overlap on all tested axes, the shapes are intersecting. This method ensures precise collision detection by accounting for the actual geometry and orientation of the objects involved.



 <img src="/images/2d_collision_detection/Figure5.png" width=50% height: auto/>

Figure 5: Example showing colliding objects. 


This example demonstrates the collision of convex objects in a 2D coordinate system. The green dashed line represents a potential separating axis, while the red and orange lines depict the projections of Polygons A and B onto this axis. As the projections overlap along the green line, a collision occurs between the two polygons. 

# Visualizing in Pygame

## Created an object class
We created a self-defined class called ‘SimplifiedObjects’ (class) to represent a two-dimensional rectangular object with various functionalities for manipulating its position, rotation, and checking for collisions with other objects of the same type.

### Attributes
This class stores essential attributes such as the vertices of the rectangle, its center, dimensions (width and length), the direction it is facing, the angle of rotation, and a flag indicating if it has collided with another object.

### Methods
Getters and Setters: Allow us for easy access and modification on attributes, such as angle and coordinate positions.

Movements: Allow us to intuitively manipulate the moving directions.

Collision Detection: Apply the SAT to the class to check collisions.

Helper Methods: Calculate distances between points, updating the object's center, direction, and angle, and rotating its vertices.

## Show_objects
We created another function called ‘show_objects’ to render a collection of objects onto a Pygame screen. Each object is represented by an image, which is scaled to fit within the screen dimensions while maintaining its aspect ratio. The function also handles rotation of the images based on the angle provided by each object. It detects and highlights collisions by overlaying a semi-transparent red bounding box on the colliding objects. 

# Demo

## Basic Movements
In this part, we will demonstrate the basic movement of a car within the Pygame. This example will showcase the fundamental functionalities to users for simulating vehicular motion.



<div style="display: flex; justify-content: center; gap: 10px;">
 <img src="/images/2d_collision_detection/Move_Forward.gif" style="width: 25%; height: auto;" />
 <img src="/images/2d_collision_detection/Move_Backward.gif" style="width: 25%; height: auto;" />
 <img src="/images/2d_collision_detection/Coordinate_movements.gif" style="width: 25%; height: auto;" />
 <img src="/images/2d_collision_detection/Rotate.gif" style="width: 25%; height: auto;" />
</div>



## Scenarios
In this section, we aim to showcase various scenarios demonstrating car movements within Pygame, specifically focusing on collision and non-collision events. These scenarios will highlight the ability to accurately visualize and handle different collision dynamics, providing valuable insights into the behavior of vehicles in diverse situations.



<div style="display: flex; flex-wrap: wrap; justify-content: center; gap: 10px;">
 <img src="/images/2d_collision_detection/Head-On_Collision.gif" style="width: 30%; height: auto;" />
 <img src="/images/2d_collision_detection/Head-On_Miss.gif" style="width: 30%; height: auto;" />
 <img src="/images/2d_collision_detection/Backing_Up_Collision.gif" style="width: 30%; height: auto;" />
 <img src="/images/2d_collision_detection/Turning_Collision.gif" style="width: 30%; height: auto;" />
 <img src="/images/2d_collision_detection/Turning_Miss.gif" style="width: 30%; height: auto;" />
 <img src="/images/2d_collision_detection/Parallel_Parking.gif" style="width: 30%; height: auto;" />
</div>



# Future Improvements

The next step in enhancing StudioViz involves transitioning from 2D to 3D visualization, incorporating more complex and realistic elements such as car models, trees, and pedestrians. This will provide a more immersive and accurate simulation of real-world scenarios. This comes with the challenge of creating collision detection for irregular shapes - making sure that objects know whether they are coming into contact with the ground, buildings, and trees. Eventually, we would also like to use machine learning to predict and prevent collisions based on historical data and real-time inputs.

# References

"Separating Axis Theorem," *Wikipedia*, https://en.wikipedia.org/wiki/Hyperplane_separation_theorem.

Kah Shiu Chong, "Collision Detection Using the Separating Axis Theorem", envatotuts+, last modified August.6th 2012, https://code.tutsplus.com/collision-detection-using-the-separating-axis-theorem--gamedev-169t

OpenAI, ChatGPT, response to an question in Separating Axis Theorem example , June.19th 2024.

Collision Detection

Firmware should only be updated when it is necessary to fix a bug or access a new feature. Customer should consult with a Cepton field support engineer before undertaking an update. The tool to update firmware and latest firmware image will be provided by field support engineers upon request. Publicly available update tool and current firmware image may be provided for download in the future.

Currently 802.1AS is supported, with full support planned for later.

Yes. Full support for Python (Python 3.6 or later) and Python specific APIs are part of our SDK release.

Yes. Javascript bindings to SDK is currently in pre-release beta, available upon request.

Yes. C# bindings to SDK is currently in pre-release beta, available upon request.

Developer Portal

Downloads

Documentations

Showcase

Blogs

Videos

FAQs

Join Our Team