# 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

# Overview
Cepton’s lidar sensors send out two kinds of packet on a regular basis: Point Data Packet and INFO Data Packet. Please refer [Data Format](https://developer.cepton.com/doc/data_format) for detailed structure information. Chapter 2 in this document outlines the implementation of Precision Time Protocol (PTP) in Cepton’s products to illustrate the generation of critical PTP related values reported in those two kinds of packets. These values are mainly used for displaying real-time system information, or run-time self-monitoring and diagnostic. Chapter 3 gives an example of how to use those reported values to calculate universal PTP timestamp for each point. Chapter 2 is for your better understanding, but you can skip this Chapter and directly refer to chapter 3 for usage only. 

# Reported Value Generation 
## Clock adjustment
Each slave clock maintains a local counter calculating the number of microseconds (us) since the sensor last booted up. When connection with PTP master clock becomes active, the clock adjustments shall perform synchronization and syntonization. Therefore, the universal PTP timestamp calculation is generally composed of three parts: 
<center>

$ptp\_timestamp = local\_counter + synchronization\_correction + syntonization\_correction$

</center>

Synchronization eliminates the offset between slave’s counter and master’s clock. Syntonization matches the frequency of local clock to that of the Grandmaster Clock in the domain, which is measured by the slave's counter increases every drift interval, it is one nanosecond slower than the master's clock. Thus, the formula becomes more concrete: 
<center>

$ptp\_timestamp = local\_counter + master\_slave\_offset + \frac{local\_counter\_increase}{drift\_interval}$

</center>

Clock adjustments are carried out through a sequence of PTP timing messages exchange between upstream PTP Instance and local slave. The basic message exchange pattern is illustrated in IEEE Std 1588-2019 Figure 16 as shown below : 

 <img width="460" src="/images/ptp/message_exchange.png">
 

(a) The Master PTP Instance sends a Sync message to the Slave PTP Instance and notes the time `t1` at which it was sent. The timestamp `t1` can be embedded in the `Sync` message or in a `Follow_Up` message. 

(b) The Slave PTP Instance receives the `Sync` message and notes the time of reception `t2`. 

(c) The Slave PTP Instance sends a `Delay_Req` message to the Master PTP Instance and notes the time `t3` at which it was sent. 

(d) The Master PTP Instance receives the `Delay_Req` message and notes the time of reception `t4`. The Master PTP Instance conveys to the Slave PTP Instance the timestamp `t4` by embedding it in a `Delay_Resp` message or in a `Delay_Resp_Follow_Up` message.

The message exchange pattern illustrated in 2.1 is repeated contiguously to maintain latest `master_slave_offset` and `drift_interval`, which will be explained in 2.2 and 2.3 independently.

## Update `master_slave_offset` 
Say master sends a message to the slave at PTP time `m1`, after a communication link propagation delay `d`, the slave receives message at its local counter time `c1`, then the offset currently equals to: $Δ1 = c1 - (m1+d)$. The link propagation delay is required for updating the offset. Assuming symmetric link, that link delay is a mean of tms
 and tsm. However, (t1, t4) and (t2, t3) follows different time metrics, therefore, the delay is calculated by: 
<center>

$d = \frac{{(t_2 - t_1) + (t_4 - t_3)}}{2} = \frac{{(t_2 - t_3) + (t_4 - t_1)}}{2}$

</center>

Initially, it’s set to zero. 

Since link delay can only be updated every time a complete group of $<t1, t2, t3, t4>$ is obtained, the newest offset is calculated based on the last group of $<t1, t2, t3, t4>$.

## Update `drift_interval`
Differentiate from offset, which depends only on the last group of message exchange, The drift_interval that will be reported is an accumulated value, which is updated every time a new pair of (t1, t2) is recorded. As mentioned in 2.1, the slave's counter increases every drift interval, it becomes one nanosecond slower or faster than the master's clock. According to this, what is being maintained is drift_interval, but what is being added to timestamp calculation is: $\frac{local\_counter\_increase}{drift\_interval}$.
The `drift_interval` represents a time range of counter in nanosecond unit and is updated as follows: 
<center>

$drift\_interval = \frac{counter\_range}{master\_range - counter\_range}$

</center>

where:

<center>

$master\_range = current\_master\_arrival\_timestamp - last\_master\_arrival\_timestamp$

$counter\_range = current\_counter\_ns - last\_counter\_ns$

</center>

The drift_interval can be a positive value or negative value: 

 - when drift_interval is positive, it means counter timestamp is increasing slower than master clock; 

 - when drift_interval is negative, it means counter timestamp is increasing faster than master clock; 

 - when drift_interval is zero, which means counter timestamp is increasing in the same speed as master clock, syntonization_correction will also be zero.
 
## Report PTP relevant values 
Recall the PTP timestamp calculation formula: 

<center>

$ptp\_timestamp = local\_counter + master\_slave\_offset + \frac{local\_counter\_increase}{drift\_interval}$

</center>

We have explained the generation of master_slave_offset in 2.2 and the generation of drift_interval in 2.3. The local_counter is the slave’s internal non-decreasing counter timestamp (currently using emac system time, in us unit). The value counts the time interval between the moment of power-up (emac power-up) to the moment of INFO packet sent-out. 

Cepton’s lidar sensors send out two kinds of packet on a regular basis: Point Data Packet and INFO Data Packet. 

In INFO packet header, there are three fields containing PTP related information, which are filled in the local_counter value, master_slave_offset value, and drift_interval value correspondingly: 
| Field Name | Unit | 
|-------------------------|--------|
| time_since_powerup | us |
| time_offset_from_master | us | 
| time_drift_correction | per ns | 

In Point Data Packet’s payload, each point has a relative_timestamp field that holds the relative counter value from the time since last point in the packet. For the very first point, it is time since the packet header's reference time. 

# PTP Usage Example
The Cepton SDK usage is almost all about using Callback APIs, please refer [SDK Guide](https://developer.cepton.com/doc/sdk_guide) for more information. Here, only point’s PTP timestamp calculation relevant part will be explained. 

The master_slave_offset and drift_interval is only reported in INFO packet, therefore, to calculate following points’ timestamp, a local copy of previous INFO packet information is needed, the callback looks like this: 

```c
int64_t last_info_offset = 0;
int64_t last_info_drift = 0; 
int64_t last_info_counter = 0; 

void infoCallback(CeptonSensorHandle handle, 
 const struct CeptonSensor *info, void *user_data) {
 last_info_counter = info->power_up_timestamp; 
 last_info_offset = info->time_sync_offset; 
 last_info_drift = info->time_sync_drift; 
} 
```
Since the calculation is based on previous INFO packet, the local counter increase in the formula becomes the time difference between each point’s counter and the counter timestamp when the previous INFO packet is sent out. Therefore, the power_up_timestamp in INFO packet must also be recorded. 

Each point’s counter timestamp calculation is a litter bit confusing. Recall that each point has a relative_timestamp field that holds the relative counter value from the time since last point in the packet. For the very first point, it is time since the packet header's reference time. Here is how you can get each point’s counter value: 

```c
int64_t prev_point_counter = 0; 

void pointCallback(CeptonSensorHandle handle,
 int64_t start_timestamp, size_t n_points,
 size_t stride, const uint8_t *points,
 void *user_data){
 prev_point_counter = start_timestamp; 
 for (size_t i = 0; i < n_points; i++) { 
 struct CeptonPoint point = *(struct CeptonPoint const*)(points + i * stride); 
 int64_t point_counter = prev_point_counter + (int64_t)point.relative_timestamp; 
 prev_point_counter = point_counter; 
 } 
}
```

Based on above obtained value and formula, each point’s timestamp can be calculated as: 
```c
#include <stdio.h>
int64_t drift_correction = (last_info_drift==0) ? 0 :
 (point_counter - last_info_counter)*1000 / last_info_drift; 
printf("calculated point PTP timestamp(ns): %lld \n", 
 (last_info_counter - last_info_offset)*1000 + drift_correction 
); 
```
Pay attention to the unit conversion. 

Finnally, a complete sample code for your reference: 

```c
#include <stdio.h>
#include <stdlib.h>

#include "cepton_sdk2.h"

void check_api_error(int err, char const *api) {
 if (err != CEPTON_SUCCESS) {
 printf("API Error for %s: %s\n", api, CeptonGetErrorCodeName(err));
 exit(1);
 }
}

void sensorErrorCallback(CeptonSensorHandle handle, int error_code,
 const char *error_msg, const void *error_data,
 size_t error_data_size) {
 printf("Got error: %s\n", error_msg);
}

int n_info = 0;

//(per ns)
int64_t last_info_drift = 0; 
//(1us)
int64_t last_info_counter = 0; 
int64_t last_info_offset = 0;
int64_t prev_point_counter = 0;

void infoCallback(CeptonSensorHandle handle, 
 const struct CeptonSensor *info, void *user_data) {
 printf("Got %d info:", ++n_info);
 printf("ts_info_counter: %lld ts_offset: %lld ts_drift: %lld\n", 
 info->power_up_timestamp, 
 info->time_sync_offset,
 info->time_sync_drift
 );
 last_info_counter = info->power_up_timestamp;
 last_info_offset = info->time_sync_offset;
 last_info_drift = info->time_sync_drift;
 
}

void pointCallback(CeptonSensorHandle handle,
 int64_t start_timestamp, size_t n_points,
 size_t stride, const uint8_t *points,
 void *user_data){
 prev_point_counter = start_timestamp;
 for (size_t i = 0; i < n_points; i++) {
 struct CeptonPoint point = *(struct CeptonPoint const*)(points + i * stride);
 int64_t point_counter = prev_point_counter + (int64_t)point.relative_timestamp; 
 if (i == 0 || i == n_points-1) {
 printf("the raw counter(us) of point %d is: %lld\n", (int)i, point_counter);
 int64_t drift_correction = (last_info_drift==0) ? 0 : (point_counter - last_info_counter)*1000 / last_info_drift;
 printf("calculated point PTP timestamp(ns): %lld \n", 
 (last_info_counter - last_info_offset)*1000 + drift_correction 
 );//temporary - offset
 
 }
 prev_point_counter = point_counter;
 }
}

int main() {
 int ret;

 // Initialize
 ret = CeptonInitialize(CEPTON_API_VERSION, sensorErrorCallback);
 check_api_error(ret, "CeptonInitialize");

 ret = CeptonEnableLegacyTranslation();
 check_api_error(ret, "EnableLegacyTranslation");

 // Start networking listener thread
 ret = CeptonStartNetworking();
 check_api_error(ret, "CeptonStartNetworking");

 // Listen for points
 ret = CeptonListenPoints(pointCallback, 0);
 check_api_error(ret, "CeptonListenPoints");

 // Listen for info
 ret = CeptonListenSensorInfo(infoCallback, 0);
 check_api_error(ret, "CeptonListenSensorInfo");
 
 // Sleep
 while (n_info < 10)
 ;

 // Deinitialize
 ret = CeptonDeinitialize();
 check_api_error(ret, "CeptonDeinitialize");
 return 0;
}
```
This example prints the counter, offset and drift in each INFO packet, and the raw counter and calculated timestamp of the first and last points in each Point Data packet, feel free to modify the output to best fulfil your requirement.

PTP Implementation and Point Timestamp Calculation

## Introduction
Cepton's perception team is actively developing a system to classify point cloud objects acquired from their LiDAR sensor.
Neural networks are widely known as the state-of-the-art approach to tackle the task of classification,
and Cepton LiDAR's high-resolution point cloud can be used with them, although efficient and accurate featurization has to be completed first.

In my internship, I integrated a voxel featurization into their system and tested its capability.
In this post, I will go over how point cloud data are featurized as voxel data and how the neural network architecture is tailored for voxel featurization.
In the end, I briefly report that my voxel featurization showed high efficiency and accuracy for the object classification task.


## Featurization

Data collected from our LiDAR sensors takes the shape of a point cloud, with each point having position information in the form of $x, y, z$ coordinates.
However, these unstructured points can’t easily be used in a neural network, so they must be converted to some structured form first, i.e. the input data has to be featurized.
There are many different ways of doing this, with the one I chose being a 3D voxel grid.


I start with normalizing the points to the origin.
I compute the minimum value for each dimension across the entire point cloud and then translate all points by subtracting these minimum values, aligning the point cloud with the origin.
I use cubical voxels of size $0.1m$, each of which is set to be 1.0 if it contains a point in it, otherwise 0.0.
I set the number of voxels as $24^3$ (2.4m each), to well capture both pedestrians and vehicles.

Once this was done for all of the points, it resulted in a collection of voxels that represented the point cloud.
Finally, I converted the grid into a tensor, which could then be fed into a neural network, which I will explain next. 

```python
import numpy as np
import torch

def featurize(point_cloud):
 # Find min and normalize
 minX = min(data, key=lambda x: x[0])[0]
 minY = min(data, key=lambda x: x[1])[1]
 minZ = min(data, key=lambda x: x[2])[2]
 data = [(x - minX, y - minY, z - minZ) for [x, y, z] in data]
 width = 0.1
 # Create grid and fill with zeros
 grid_size = 24
 voxelGrid = np.zeros((min_grid_size, min_grid_size, min_grid_size), dtype=float)
 # Fill in grid
 for [x, y, z] in data:
 indexX, indexY, indexZ = int(x / width), int(y / width), int(z / width)
 indexX = min(indexX, grid_size - 1)
 indexY = min(indexY, grid_size - 1)
 indexZ = min(indexZ, grid_size - 1)
 voxelGrid[indexX, indexY, indexZ] = 1

 # Convert a NumPy array to a Tensor
 voxelTensor = torch.from_numpy(voxelGrid)
 return voxelTensor
```
<div style="text-align: center;">
<img src="/images/voxel_classifier/pointcloud_pedestrian.png" width="350" height="350">
<img src="/images/voxel_classifier/voxelized_pedestrian.png" width="350" height="350">
</div>

<div style="text-align: center;">
<img src="/images/voxel_classifier/pointcloud_vehicle.png" width="350" height="350">
<img src="/images/voxel_classifier/voxelized_vehicle.png" width="350" height="350">
</div>


## Neural Network Architecture

After featurizing the point cloud data, the next step is to feed them to a convolutional neural network (CNN) to build a classifier.
The reason that this has to take place after featurization is that the point cloud data lacks a clear structure and features to learn from. However, featurizing the points as voxels gives CNN a structure to work with, which greatly benefits the learning process.

This type of neural network consists of multiple convolutional layers with max pooling and activation functions,
followed by fully connected layers and a final output layer.
For my classifier, which I implemented in PyTorch Lightning, I pretty much followed the <a href="https://cs231n.github.io/convolutional-networks/#architectures" target="_blank">standard structure</a>
: I had three convolutional layers, each followed by a max pool and ReLU, then two fully connected layers using a linear transformation, with the first one followed by ReLU, and finally a softmax and output layer.
Because the data was in three dimensions, I used the three-dimensional versions of the convolution and max pool functions.

Throughout this process, the dimensions go from $24 \rightarrow 22 \rightarrow 11 \rightarrow 9 \rightarrow 4 \rightarrow 2 \rightarrow 1$ while the number of filters goes from $1 \rightarrow 16 \rightarrow 32 \rightarrow 64 \rightarrow 32 \rightarrow 3$ and the kernel size stayed constant at $3 \times 3 \times 3$. See the figure below for the details. 
<div style="text-align: center;">
<img src="/images/voxel_classifier/nn.png">

Note: this figure isn't completely accurate because the visualization tool doesn't support three-dimensional convolutions, but it's a fair approximation.
</div>

## Results and Revisions
The classifier achieved good accuracy, with > 99% on pedestrians and > 90% on vehicles.
The speed was also reasonably fast, although not the fastest out of all the classifiers available.
| True/Predicted | Unknown | Pedestrian | Vehicle |
| -------------- | ------- | ---------- | ------- |
| Unknown | 9.69% | 0.0% | 0.0% |
| Pedestrian | 0.0% | 65.39% | 0.06% |
| Vehicle | 0.0% | 2.71% | 22.15% |

| Inference Batch | Object Count | Time (ms) |
| --------------- | ------------ | --------- |
| Single | 1 | 0.83 |
| Small Batch | 10 | 7.28 |
| Large Batch | 100 | 89.47 |

### Further Extension
As an extension, I experimented with dropping the final convolutional layer and its associated max pool and ReLU, while reducing the initial dimensions of the grid to a $10 \times10 \times 10$. This resulted in the number of filters topping out at $32$, and the dimensions going from $10 \rightarrow 8 \rightarrow 4 \rightarrow 2 \rightarrow 1$. These changes were effective, with the classifier's speed improving significantly ($\times 35$ faster) while accuracy remained the same.

| Inference Batch | Object Count | Time (ms) |
| --------------- | ------------ | --------- |
| Single | 1 | 0.26 |
| Small Batch | 10 | 0.81 |
| Large Batch | 100 | 2.59 |


<div style="text-align: center;">
<img src="/images/voxel_classifier/nn2.png">
</div>

## Conclusion
In this post, I went over the voxel featurization and the CNN architectures to featurize and classify point cloud data. 
The classifier I implemented was able to achieve high accuracy and speed. In the future, the accuracy could be improved with greater amounts of training data, and the speed could likely be increased with further refinements to the neural net architecture.

## References
<a href="https://cs231n.github.io/convolutional-networks/#architectures" target="_blank">Convolutional Neural Network Structure from Stanford</a>

Point Cloud Object Classification with Voxel Featurization

## Introduction
With Cepton Inc.'s Lidar sensor,
you can collect high-resolution point cloud data in real-time as below.


 <img width="460" height="300" src="/images/lyft_dataset/cepton_lidar.gif">
 

As you can see, it captures pedestrians and cars.
If you take a close look, you can also see how fast the cars are moving or even how tall the pedestrians are.
Our Lidar provides various informative data to us.

However, this data is only comprehensible to us, humans, not to computers.
That's where our team, the Perception team, comes into play.
Our team's goal is to figure out what information we can extract from the point cloud data captured by our Lidar.

In this post, I go over a public dataset,
[Lyft 3D Object Detection for Autonomous Vehicles](https://www.kaggle.com/code/tarunpaparaju/lyft-competition-understanding-the-data),
release from Lyft to see what information they handle.

### Purpose
Find out the basic usage of data properties of [Lyft 3D Object Detection for Autonomous Vehicles](https://www.kaggle.com/competitions/3d-object-detection-for-autonomous-vehicles/data).

## Procedures and Results 

### Downloading Data

First thing first, go to the page of [Lyft 3D Object Detection for Autonomous Vehicles](https://www.kaggle.com/competitions/3d-object-detection-for-autonomous-vehicles/data) and click **Download All** located at the bottom right of the page.
Here, I note that the total data size is around 90GB.
Make sure you have enough disk space in your environment.


### Read and Write Data
With the `3d-object-detection-for-autonomous-vehicles` directory ready, let's read the dataset.
Kindly enough, Lyft provides their own SDK in Python, `lyft_dataset_sdk` to play with the dataset.

The following code is to read the dataset and export the point cloud data and the bounding boxes data to JSON files.
(Note: Strangely, you need to rename directories for training data as follows: `train_images` -> `images`, `train_lidar` -> `lidar`, `train_maps` -> `maps`, and `train_data` -> `data`)

```python
from lyft_dataset_sdk.lyftdataset import LyftDataset
from lyft_dataset_sdk.utils.geometry_utils import (
 BoxVisibility,
)
from lyft_dataset_sdk.utils.data_classes import LidarPointCloud
import json

def generate_next_token(scene):
 scene = lyft_dataset.scene[scene]
 sample_token = scene["first_sample_token"]
 sample_record = lyft_dataset.get("sample", sample_token)

 while sample_record["next"]:
 sample_token = sample_record["next"]
 sample_record = lyft_dataset.get("sample", sample_token)
 yield sample_token

DATA_PATH = "./3d-object-detection-for-autonomous-vehicles/"
lyft_dataset = LyftDataset(data_path=DATA_PATH, json_path=DATA_PATH + "data")

for scene_id in range(len(lyft_dataset.scene)):
 for frame, sample_token in enumerate(generate_next_token(scene_id)):
 print(f"Processing scene {scene_id}, frame {frame}")
 sample = lyft_dataset.get("sample", sample_token)
 sd_record = lyft_dataset.get("sample_data", sample["data"]["LIDAR_TOP"])

 _, boxes, _ = lyft_dataset.get_sample_data(
 sample["data"]["LIDAR_TOP"],
 box_vis_level=BoxVisibility.ANY,
 flat_vehicle_coordinates=False,
 )

 pc, _ = LidarPointCloud.from_file_multisweep(
 lyft_dataset,
 lyft_dataset.get("sample", sd_record["sample_token"]),
 sd_record["channel"],
 "LIDAR_TOP",
 num_sweeps=5,
 )

 points = pc.points[:3, :].T.tolist()

 box_data = [{"corners": box.corners().T.tolist(), "label": box.name} for box in boxes]

 data = {
 "points": points,
 "boxes": box_data,
 }
 with open(f"exported/scene_{scene_id}_frame_{frame}.json", "w") as f:
 json.dump(data, f)
```

Here I visualize one of the scenes on our Cepton Viewer.
It seems like one scene is about 6-seconds long.
This scene particularly is captured by a car taking a left turn at an intersection.


 <img width="460" height="300" src="/images/lyft_dataset/cepton_view.gif">
 

### What objects are in it?
To know more about the dataset, let's see what types of objects it contains.
Following is the code to show the demographics of the objects in the whole dataset.

```python
# Overlapped code omitted for brevity. ...

import matplotlib.pyplot as plt
from collections import Counter

# Overlapped code omitted for brevity. ...
all_box_names = []
for scene_id in range(len(lyft_dataset.scene)):
 for frame, sample_token in enumerate(generate_next_token(scene_id)):
 print(f"Processing scene {scene_id}, frame {frame}")
 sample = lyft_dataset.get("sample", sample_token)

 _, boxes, _ = lyft_dataset.get_sample_data(
 sample["data"]["LIDAR_TOP"],
 box_vis_level=BoxVisibility.ANY,
 flat_vehicle_coordinates=False,
 )

 all_box_names.extend([box.name for box in boxes])

# Create the histogram.
counts = Counter(all_box_names)
labels, values = zip(*counts.items())
plt.figure(figsize=(10,5))
plt.bar(labels, values)
plt.xlabel('Object Names')
plt.ylabel('Count')
plt.title('Distribution of Object Names')
plt.xticks(rotation=90)
plt.tight_layout()
plt.show()
```

![Distribution](/images/lyft_dataset/object_demographics.png)

As it shows, the large majority consists of cars.
Unexpectedly, this dataset has a small population of pedestrians because detecting people tends to be a critical task in many perception applications.


## Conclusion

In this post, I briefly investigated the [Lyft 3D Object Detection for Autonomous Vehicles](https://www.kaggle.com/competitions/3d-object-detection-for-autonomous-vehicles/data).
The dataset was very handy to use, thanks to the SDK provided by Lyft.
But it is important for us to be aware that there is a large imbalance between the population of cars and pedestrians.

## References
- [Lyft 3D Object Detection for Autonomous Vehicles](https://www.kaggle.com/competitions/3d-object-detection-for-autonomous-vehicles/data)
- [Lyft Dataset SDK](https://pypi.org/project/lyft-dataset-sdk/)


---

About the author: Hikaru Ibayashi is a Software Engineer at Cepton Inc. Perception team. 
He holds a Ph.D. degree in Computer Science from the University of Southern California, where his research focused on the foundation of deep neural networks and physic-informed machine learning.

---

Lyft Dataset Investigation

# Introduction
The goal of this project was to develop an algorithm that can identify the trailer tongue on a trailer based on 3D point cloud data. The trailer tongue is the part of the trailer that often has a stick-like shape, and extends out in the front of a trailer; its tip can be hitched to a trailer hitch. When a driver would like to hitch a trailer, they can use the algorithm I developed to locate the trailer tongue without having to get down from the car. The driver can then continuously track the relative position of the trailer tongue while backing close to the trailer, which would allow them to easily park the trailer hitch to the tip of the trailer. In my project, I was able to visualize the trailer tongue detected and report the distance of the tip of the tongue from a trailer hitch in a 3D file format.


# Ground Recognition and Alignment
First, I used RANSAC to perform ground detection, then I removed the ground points. Using the equation for the ground plane, I also transformed the point cloud such that it is aligned with Z=0 being the ground.
# Trailer Tongue Recognition
## For a trailer with a flat surface
I first assumed that there is only the trailer in the point cloud and that it is a trailer with a flat back, like a U-haul cargo trailer.

With this assumption in mind, I clustered the point cloud to remove outliers, then I found the front plane using RANSAC, and then assumed that everything in front of the plane would be the trailer tongue. In the image below the yellow points are from the trailer, the blue is the trailer tongue identified, the red is the "tip" of the tongue and the green lines form a bounding box for it.


 <img src="/images/trailer_hitching/plane_based.PNG" width=50% height: auto/>



Of course, these assumptions do not apply very often in real life since there are many types of trailers and most likely there would be other objects close to the trailer, hence I needed to develop a more robust algorithm
## Initial Recognition

### Clustering
I used DBSCAN as a clustering method. Clustering is necessary to remove outliers as well as to separate different objects when there are multiple in view. With the version of DBSCAN that I used, I can specify a parameter which is the euclidean distance for what could be considered "within cluster". This parameter has been, in my algorithm carefully adjusted based on the approximate distance of the trailer from the sensor. If the object were further, this parameter is larger since there would be fewer points but we still want to cluster the object as a whole. If the object is really close, the parameter needs to be smaller, otherwise, with a lot of points, DBSCAN would take a very long time if the algorithm also has to search for neighbors within a large radius. Hence I have specified DBSCAN parameters for different ranges of how far the trailer is from the sensor.
 
 

### Object tracking
After clustering, I am also tracking the object which is identified as the trailer so that other vehicles in view don't affect the tracking results. I do this by trying to see which object is most likely to be the trailer(using a loss function explained later), then continuously tracking the mean of that cluster. So in future frames, I would filter out all other clusters and only keep the only with the mean closest to the mean that I am continuously tracking to ensure I am just working with the cluster that is the trailer.
 

### Loss function
To be able to find the trailer tongue, I am using the optimize. minimize function from SciPy, it is used to find the minimum value of a function. I aggregated a lot of point clouds scanned in the first few seconds to increase accuracy for detection. To be able to use my loss function, I first stored a 3d point cloud model of the trailer tongue. Then the loss function's goal is to find the part of the point cloud that had the "best fit" with the model.

In mathematical terms, my loss function had 5 parameters: alpha which is the angle of rotation up and down, beta which is the angle of rotation left and right, and shift in the x, y, and z direction. The output would be, after I rotate the model by alpha and beta, then shift it by x, y, and z, (in each of the respective directions), return the mean distance of each point in the model to its closest two points in the point cloud of the entire trailer. Then, theoretically, this would return the smallest value when the model is the best fit with the point cloud, allowing the algorithm to locate where the trailer tongue is within the point cloud. I also added a penalty term that will add to the loss function if there are too many points in front of the model once it is been placed. The trailer tongue must be in the front of the trailer cluster hence adding the penalty term helps eliminate some cases when a shape similar to the trailer tongue is somewhere else in the point cloud. After finding the optimal position to place the model, I extracted the points closest to the positioned model in the point cloud and collected and stored those points as the trailer tongue, which I will need to keep track of in future frames.

 <img src="/images/trailer_hitching/detected.PNG" width=60% height: auto/>

In the image above, the green part is the trailer tongue detected from the trailer point cloud (red).

 

## Tracking after Initial Recognition
After the initial detection, I had to continuously track the position of the tongue. One way I thought of doing so was using iterative closest point (ICP) which was an algorithm that can match two point clouds and find the transformation that can change one to the other. I had several attempts at this as well.

### Attempt 1: ICP for each adjacent frame
Here I used ICP to calculate the transformation from the trailer in the previous frame to the trailer in the next frame. Then I continuously applied this transformation to the trailer tongue from the previous frame. My idea was that if the trailer had "moved" by this much according to ICP, then the tongue would have moved by the same amount so this should allow me to track it. But this led to some issues in the results produced. Sometimes there may be a small error each time I use ICP. Then continuously applying these transformations to the trailer tongue stored will lead to more and more errors adding up. This can cause the trailer tongue to be significantly off from where it should be in the first place. As visible in the video below, the green (the trailer tongue identified) is slowly drifting away from where it should be matched with the trailer.


 <img src="/images/trailer_hitching/error_1.gif" width=60% height: auto/>

 

### Attempt 2: ICP from the first frame to each frame
In this version, I kept track of a "source frame", this was the first frame but for each next frame, I found the transformation from the "source frame" to the next frame and applied this transformation for the "source frame". Then I also apply this transformation to the trailer tongue from the previous frame. This resolves an issue from Attempt 1 because now, an error in the ICP can be corrected in the future because this error was also applied to the "source frame", so in a future time when ICP is used to find a transformation from the source frame to a current frame, it will take into account the error earlier and adjust accordingly to that. However, there is another issue with this which is that the points for the "source frame" are points from the first frame. The trailer as scanned in the first frame may be very different from the trailer scanned in a much later frame. The image below is a bottom view of how well the source and the current frame match. The backplane for the two point clouds (in different colors) is matched well by ICP but the front tongue part is not being matched together.


 <img src="/images/trailer_hitching/bottom.PNG" width=50% height: auto/>

Hence when these two are being matched, even if they are relatively close in terms of where they are, there may still be errors that lead to the transformation to be quite off. In the video below, in the beginning the trailer tongue is still in the right position relative to the trailer but after a few seconds it is moving slightly to the left.


 <img src="/images/trailer_hitching/error_2.gif" width=60% height: auto/>



### Solution: ICP for each adjacent frame + local adjustment
I realized from Attempt 2 that I couldn't try to match points from the first to a much later frame. Hence I decided to use the idea of using ICP from the previous frame to the next, but also with local adjustments. I used a similar loss function as what I used for initial detection, but made it to search within a smaller range for the different variables. This resolved the issue that the ICP errors build up because it gets adjusted in each frame. I also added another penalty term that ensures that the tongue is not too much in front of where it should be, by increasing the loss function when there are many points in the trailer tongue detected that are in front of the trailer point cloud. This algorithm worked quite well in tracking the trailer tongue.

 <img src="/images/trailer_hitching/detection-working.gif" width=60% height: auto/>


### Kalman filtering
I also kept track of the tip of the trailer tongue, which was calculated each frame based on how much the trailer tongue moved. But in order to make it less sensitive to inaccuracies or outliers, I applied a Kalman filter to it on the velocity of its movement. This allowed the hitch tip position to move in a more smooth way.

# Visualization
To visualize the results, I added the points which are the trailer identified and the trailer tongue identified in a different color. I also added some text that shows the distance of the trailer tip from the truck. There are also lines illustrating the distance and direction from the truck - which turn from green to red once the trailer is within a very close distance to the truck


I was also able to add in 3D models of both the trailer and the truck to make the results more easily interpreted. The location of the trailer was determined by the position of the tip, and its angle is also aligned with the angle of the trailer tongue.


 <img src="/images/trailer_hitching/visualization.gif" width=60% height: auto/>




# Results
Here is a video showing the algorithm and visualization working together. The trailer tongue can be tracked quite accurately even when the trailer is parked closely next to other vehicles!

 <img src="/images/trailer_hitching/utility_trailer.gif" width=80% height: auto/>


## Limitations and Edge cases
### Distance from the truck
The trailer should be approximately within 8 meters away from the sensor for the algorithm to work, otherwise, there may be too few points for the algorithm to be able to detect the trailer tongue properly.
### Distance from other vehicles
If other vehicles or objects are very close to the trailer (< 0.4m) there is a chance they get clustered together with the trailer and this may cause issues in the initial detection or ICP tracking algorithm

Detecting and Tracking a Trailer Tongue

## Introduction
###
&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;Towing a trailer and traveling high speeds can be very dangerous if the driver is unaware of the trailer’s condition. This project implements a detection and tracking algorithm in the point cloud to monitor the angles of a towed trailer using Cepton’s LiDAR sensors. Trailer detection is used to alert the driver of the jackknifing or trailer detachment risks, and angle tracking is used to alert the driver of the trailer’s sway. These features would help decrease the risk of an accident.

&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;For our setup, we mounted a Vista-X90 sensor on the truck’s bed using a tripod and collected video footage with the same placement. 

## Recognizing the Trailer
### 
&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; The first task was to recognize the trailer face. We cropped the point cloud to a certain distance range where the trailer was sure to be located: in this case, from 0.5 to 10 meters. Uniform gridding was also applied across all data points. This particular dataset had the following point cloud:


 <img src="/images/trailer_sway/cropping.png" width=60% height: auto/>


&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;Then, background points were removed from the data and the DBSCAN algorithm was performed, which returned multiple clusters of points that are close to each other based on the Euclidean distance. Since the trailer face has significantly more points clustered together than the background, the cluster with the most points was labeled as the trailer face. Based on the trailer face data subset and the angle at which the trailer was positioned, we could calculate and display the width of the trailer.

&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;To determine the starting angle of the trailer face relative to the LiDAR position, we performed a linear regression operation on the trailer face points. We are assuming that the LiDAR sensor is always capturing data at an angle that is directly perpendicular to the back of the truck. Using the equation generated by the linear regression operation, we drew the following bounding box around the trailer face:

 <img src="/images/trailer_sway/bounding-box.png" width=60% height: auto/>


## Tracking the Trailer
### 
&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;To track the angle from each frame, the ICP (Iterative Closest Point) algorithm was applied on the current frame with respect to the starting frame of the data. This returned the rotation and translation as a 4x4 transformation matrix from which we could extract the angle. The figure below demonstrates the effectiveness of the algorithm on the data. The pink points are the source, the blue points are the target, and the orange points are the source with the transformation matrix applied. The orange points were found to be 98% accurate with the blue points.

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


&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;An alternative method that was attempted was iterating through each frame, calculating the transformation matrix between each cumulative frame, and adding the resulting angles together to determine the final angle that is with respect to the starting frame. However, the accuracy of the calculated frames vs. the actual frames dropped to 60%, as the propagation of error for each ICP iteration likely increased significantly. Therefore, it was modified so that one ICP iteration would have to be performed for each frame. 

## Visualization
&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;This is the final visualization that was generated using the Cepton Viewer. 


 <img src="/images/trailer_sway/weaving_gif.gif" width = 50% height: auto/>


&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;The angle changes with each frame based on the current frame’s angle position and Low, Medium, and High Sway levels; Jackknifing Warning and Trailer Detachment Warning will also be highlighted as necessary. Medium Sway is defined as the trailer swinging between 5-10 degrees for at least 4 counts, High Sway occurs if the trailer swings for more than 10 degrees for at least 4 counts, otherwise the system is considered Low Sway. Jackknifing Warning occurs if the trailer is at more than a 70 degree angle to the truck, and the Trailer Detachment Warning occurs if the trailer is calculated to be at a difference of more than 1 meter from the initial trailer distance.

Detection and Tracking of a Towed Trailer

Pedestrian Classification

## Key Points
- Usage of synthetic datasets to augment deep learning solutions to improve accuracy.
- Shedding of neural network layers to meet speed requirements on CPU.

## Synthetic Dataset
Two common problems that plague deep-learning based solutions are data acquisition and computational resources. While the former can sometimes be alleviated with public datasets, these data are only available for common objects of study such as pedestrians. To obtain data for more niche objects, such as bikers in this case, one would often have to resort to manual data collection. This process can be time consuming or even infeasible when trying to collect large volumes of data. To overcome this challenge, a method to generate synthetic datasets is proposed.

### Data Baseline
A complete model of a biker can be constructed by collecting and aligning point cloud data of bikers when viewed from various angles.

 <img src="/images/biker_classifier/fullmodelright.png" width=20% height: auto/>
 <img src="/images/biker_classifier/fullmodelfront.png" width=20% height: auto/>
 <img src="/images/biker_classifier/fullmodelfront2.png" width=20% height: auto/>
 <img src="/images/biker_classifier/fullmodelleft.png" width=20% height: auto/>


### Sampling
From this baseline, the complete biker can be uniformly sampled to generate synthetic datasets. By applying occlusion to the cluster based on the viewing angle, a manufactured biker cluster can achieve a similar appearance to real data.


 <img src="/images/biker_classifier/syn_right1.png" width=40% height: auto/>
 <img src="/images/biker_classifier/right1.png" width=40% height: auto/>


 <img src="/images/biker_classifier/syn_front1.png" width=40% height: auto/>
 <img src="/images/biker_classifier/front1.png" width=40% height: auto/>


 <img src="/images/biker_classifier/syn_left1.png" width=40% height: auto/>
 <img src="/images/biker_classifier/left1.png" width=40% height: auto/>


The clusters on the left are synthetic clusters. For comparison, examples of live-recorded clusters with the same viewing angle are displayed to the right.

Upon inspection, it can be seen that while the contours of the clusters are largely preserved, the uniform sampling of a merged cluster will cause the synthetic cluster to lose the unique laser pattern of the lidar. This deviation from real data could be significant enough to cause a reduction in model accuracy.

To address this, points in the baseline model are tagged according to their viewing angle during data collection. When generating synthetic clusters, the viewing angle of each point during data collection acts as weights during the weighted sampling process. The result is that points that have a similar viewing angle to the current simulation's viewing angle are more likely to be sampled. After applying occlusion to this weighted cluster, the result is synthetic clusters that better retain the lidar's laser pattern.


 <img src="/images/biker_classifier/syn_rightv2.png" width=40% height: auto/>
 <img src="/images/biker_classifier/right1.png" width=40% height: auto/>


 <img src="/images/biker_classifier/syn_frontv2.png" width=40% height: auto/>
 <img src="/images/biker_classifier/front1.png" width=40% height: auto/>


 <img src="/images/biker_classifier/syn_leftv2.png" width=40% height: auto/>
 <img src="/images/biker_classifier/left1.png" width=40% height: auto/>

Once again, the synthetic cluster is shown on the left while a real-life counterpart is shown on the right.

Using this method, the available biker data for training was expanded from 4681 manually collected clusters to 59356 clusters. The dataset breakdown is as follows:

| | biker | non-biker |
| ---------- | ----- | --------- |
| Training | 46820 | 45828 |
| Validation | 6268 | 6238 |
| Testing | 6268 | 6237 |

Code for generating such clusters can be found [here](/src/biker_classifier/cluster_generator.zip).

## Model Architecture
During deployment, the on-board hardware often has limited computational resources — whether it's a restriction on time, memory, or hardware capability. Without GPU acceleration, a lightweight model is mandatory in order for the classifier to completely process all clusters in a given frame in a timely manner. A model architecture is presented that attempts to achieve acceptable levels of accuracy and speed when operating in a CPU only environment.

### Related Work
The model modifies the existing [PointNet](https://arxiv.org/pdf/1612.00593.pdf) architecture by Charles R. Qi et al.

 <img src="/images/biker_classifier/pointnet_baseline.png" width: auto height: auto/>


### Objective
The goal for the model is to achieve real-time classification on CPU-only machines. The model is trained and evaluated on a LENOVO laptop with an 11th Gen Intel(R) Core(TM) i7-1165G7 processor without GPU acceleration. The estimated budget for the classifier during deployment is roughly 5 ms to classify all clusters present in the frame. On average, one can expect an average of 100 clusters in a given frame, which leaves a time budget of roughly 0.05 ms or 50 us per cluster.

### Computational Graph

 <img src="/images/biker_classifier/model_architecture.png" width: auto height: auto/>

The final model has 132 trained	parameters and 8326 number of operations. The model outputs two likelihood values, one for the biker class and one for the non-biker class. This score would then be used to determine if the cluster is a biker or not.

## Results
### Model Performance

 <img src="/images/biker_classifier/conf_mat_float.png" width: auto height: auto/>

The model achieved an accuracy of 0.9832, with a precision and recall of 1 and 0.9665, respectively.

### Speed Evaluation
Using the Pytorch profiler, the average inference time for 25011 clusters was 499 us.

While the model speed was not able to hit the desired benchmark of 50 us, during actual deployment there are ways to reduce the number of clusters that the model needs to classify — such as filtering out overly large clusters (closer biker clusters tend to have 900 to 1500 points). Doing so can greatly increase the time budget allotted to each classifier call. 

#### Speed of Other Experimental Model Architectures
A logarithmic relationship could be observed between the inference speed and the number of operations in the model.

 <img src="/images/biker_classifier/speed_trend.png" width: auto height: auto/>


| Model | Parameters| Num Ops | Inference Time (us) |
| ------------------| -------- | ----- | ------------------ |
| Final model | 132 | 8326 | 499 |
| pointnet_cls-v19 | 132 | 8326 | 526 |
| pointnet_cls-v18 | 132 | 1158 | 529 |
| pointnet_cls-v17 | 132 | 8326 | 560 |
| pointnet_cls-v12 | 113 | 23674 | 648 |
| pointnet_cls-v13 | 113 | 11898 | 661 |
| pointnet_cls-v21 | 292 | 65850 | 722 |
| pointnet_cls-v11 | 423 | 67944 | 808 |
| pointnet_cls-v20 | 2715 | 514844 | 818 |
| pointnet_cls-v9 | 2715 | 514844 | 851 |

Based on the observed trend, the pointnet_cls-v18 model is expected to have a lower inference time than all other architectures since it uses the least number of operations. However, that was not the case. This deviation could be attributed to the profiler precision or it could be a limit imposed by hardware capabilities. 

The specific architecture for each model can be inspected [here](/src/biker_classifier/classifier_architectures.zip).

## Future Work

### Handling Merged Clusters
The training and testing dataset for bikers are comprised of clean data only. In other words, the biker clusters do not include any background or obstacle points. In order to better handle real world data, the model will need to be modified so it can isolate biker clusters when it has merged with other objects.

### Increased Range of Synthetic Dataset
The data generation process can be further extended to generate synthetic datasets for clusters at various distances from the lidar by applying projection. This has yet to be implemented.

## Quantization
By reducing the precision from float32 to fixed point or int8 one can expect the model to see significant improvements in speed at the potential expense of accuracy. This is particularly prevalent for hardware without FPUs or if memory usage is a concern.

### Python to C
Python programs are slower than their C counterparts in general. By transferring the existing framework, which is written in Python, into C code, an improvement in speed is expected.

### Confidence Level
During deployment, the classifier would be used to track bikers from frame to frame. The inclusion of confidence levels would be useful for such a purpose.

### Termination Policy
During runtime, it is possible for the model to take more than the allotted time to process all clusters. In these cases, to avoid delaying the processing of the next frame, an efficient heuristic to quickly assign a label to the remaining clusters is necessary. Possible solutions include inheriting the labels from the previous frame; in the absence of prior labels, a search can be performed: if a cylindrical clusters exists and is surrounded by spherical clusters in the query, then it has a high likelihood of being a biker.

### Hardware Change
Once the deployment hardware specs are determined, more profiling could be done on said hardware to get a better gauge of actual speed performance.

Real-Time Biker Classifier With Synthetic Dataset Generator

Imagine you are drawing a picture of a baseball game. You start drawing a batter hitting a ball. With the dynamic swing you draw, it’s very convincing to see that he has just hit a home run. Great job!! This is the most amazing drawing that I’ve ever seen befo…. 


 <img src="/images/instance_rendering/baseball_hit_man.png" width=25% height: auto/>


Wait a second. The background is missing. I can't see where he is. This home run should be in a huge baseball stadium with thousands of people. Draw them now. Of course, you should draw them with the same drawing style as the rest.

Are you reluctant to draw the audience individually? Okay, then let’s learn the "instanced rendering technique." To get ready, please forget about the baseball game. Today's focus is on the GPU rendering technique on the screen.

Instanced rendering is a technique in computer graphics
to efficiently draw tons of objects.
We can even draw hundreds or thousands of objects by reusing a single instance.
This article goes over basic notions of the instanced rendering in OpenGL
and shares some example use cases that I developed in Cepton. 
I provide sample programs at the end of each section. 

## OpenGL Basics 
Let's consider the simplest possible shape that GPU can process, a triangle. We know that a triangle consists of three points
and each vertex is represented by a 3D vector of float values.
To draw a triangle, we pass
the float array data to GPU.
However, since GPU is fully detached from CPU, we need to specify every single thing for the simple data sending. But OpenGL APIs help us to write those specifications.
The following is the essential summary of how we use OpenGL APIs to send three vertices data.
``` cpp
// Location in GPU to store the vertices data
#define VERTEX_LOCATION 0
// Generate a buffer on GPU to store data
glGenBuffers(1, &VBO);
// Specify the property of the buffer
glBindBuffer(GL_ARRAY_BUFFER, VBO);
// Vertices data
float vertices[9] = {-0.2f, -0.3f, 0.0f,
 0.2f, -0.3f, 0.0f,
 0.0f, 0.3f, 0.0f };
// Specify the pointer to the data array and its size 
glBufferData(GL_ARRAY_BUFFER,
 9*sizeof(float),
 vertices,
 GL_STATIC_DRAW);
// Tell GPU how to read the data
// I.e., the array is supposed to be interpreted as a series of 3D vectors
glVertexAttribPointer(VERTEX_LOCATION,
 3,
 GL_FLOAT, // Value type in GPU 
 GL_FALSE, 
 (GLsizei)3*sizeof(float), // Size of a 3D vector 
 NULL); // Where to start reading
// Enable the data location in GPU before drawing
glEnableVertexAttribArray(VERTEX_LOCATION);
```
Now the vertices data are ready.
Let's tell GPU to draw them on the screen,
or more formally, let's execute a "draw call."
In our example, as we want to draw a triangle,
we use GL_TRIANGLES mode
so that GPU regards the series of vectors as the vertices of a triangle, and nicely fills the inside of the triangle.

``` cpp
glDrawArrays(GL_TRIANGLES, 0, 3);
```

Here is what GPU draws.


 <img src="/images/instance_rendering/basic.png" width=60% height: auto/>


[Sample Program 1](/src/instance_rendering/basic.zip)

## Cost of Draw Calls 
Now we know how to draw a triangle on the screen.
Given that any shape can be represented by a bunch of triangles (i.e. "mesh" representation),
we can practically draw anything.
However, it's worth noting here that draw calls cannot be executed unlimitedly to ensure a reasonable refresh rate.

As a quick experiment, let's see what happens if we replace the single draw call with 1000,000 drawcalls.
``` cpp
// glDrawArrays(GL_TRIANGLES, 0, 3);
for (int i=0; i< 1000000; i++) glDrawArrays(GL_TRIANGLES, 0, 3);
```
You would find a significant delay until the triangle shows up
and have a hard time manipulating the window (below).



 <img src="/images/instance_rendering/FPS_lag.png" width=60% height: auto/>


## Instanced Rendering
OpenGL has various tricks to reduce draw call executions, one of which is 
the instanced rendering.
Suppose we want to draw 1000 objects, the instanced rendering technique encloses all the instructions into a single draw call.

Again, let's use a triangle as an example.
As we have already learned how to send the shape information of a triangle to GPU, let's go over how to specify the position information of 1000 triangle objects. 

In computer graphics, positions information is typically represented by a 4x4 transformation matrix (in the so-called "homogeneous coordinate.")

``` cpp
// Location in GPU to store the vertices data
#define MATRICES_LOCATION 1
// Number of instances
#define NUM_INSTANCES 1000

// Generate a buffer to store transformation matrices
glGenBuffers(1, &(MBO));
// Specify the property of the buffer
glBindBuffer(GL_ARRAY_BUFFER, MBO);
// Matrices data
float mat[NUM_INSTANCES * 16];
// Positions of triangles 
for (int matrix_id=0; matrix_id < NUM_INSTANCES; matrix_id++) {
 // Spiral arrangement of triangles
 float pos_x = 0.002f * matrix_id * cos(40*M_PI*matrix_id / NUM_INSTANCES);
 float pos_y = 0.002f * matrix_id * sin(40*M_PI*matrix_id / NUM_INSTANCES);
 float scale = 0.0004f * matrix_id;
 int i = 16 * matrix_id;
 mat[i+0] = scale; mat[i+4] = 0.0f; mat[i+8] = 0.0f; mat[i+12] = pos_x; 
 mat[i+1] = 0.0f; mat[i+5] = scale; mat[i+9] = 0.0f; mat[i+13] = pos_y; 
 mat[i+2] = 0.0f; mat[i+6] = 0.0f; mat[i+10] = scale; mat[i+14] = 0.0f; 
 mat[i+3] = 0.0f; mat[i+7] = 0.0f; mat[i+11] = 0.0f; mat[i+15] = 1.0f; 
}
// Specify the pointer to the array and its length
glBufferData(GL_ARRAY_BUFFER, NUM_INSTANCES * 16 * sizeof(float),
 mat, GL_DYNAMIC_DRAW);
// Tell GPU how to read the data
// The array is supposed to be interpreted as a series of 4x4 matrices 
for (unsigned int i = 0; i < 4; i++) {
 glEnableVertexAttribArray(MATRICES_LOCATION + i);
 glVertexAttribPointer(MATRICES_LOCATION + i, 4, GL_FLOAT, GL_FALSE,
 16 * sizeof(float),
 (const GLvoid *)(sizeof(GLfloat) * i * 4));
 glVertexAttribDivisor(MATRICES_LOCATION + i, 1);
}
```
Now GPU knows 1000 position information as well as the triangle shape information.
I always wish that GPU could automatically place the triangle using those matrices but there is another layer of instruction for GPU.
We need to specify how to use those matrices with the "shader program."
Although the shader programming itself is the very essence of GPU rendering, this article won’t dig into it for simplicity.
Rather let's regard it as an extra code to add more instructions on the usage of transformation matrices.
The following C-like code is the shader program.
``` glsl
layout (location = 0) in vec3 Vertex;
layout (location = 1) in mat4 Matrix;
void main()
{
 gl_Position = Matrix*vec4(Vertex.x, Vertex.y, Vertex.z, 1.0);
};
```
This shader program specifies how to process each transformation matrix and each vertex, i.e. taking the matrix product. Notice that the `Vertex` is placed at the location 0 (= `VERTEX_LOCATION`) and the `Matrix`
is in the location 1 ( = `MATRIX_LOCATION`). This shows that GPU properly receives the data at the locations that we specified.

Now that GPU finally knows everything, let's call a draw call. 
For instanced rendering, we need to use the following draw call API to draw multiple instances.
But this is counted as a single draw call, and therefore efficient.

``` cpp
glDrawArraysInstanced(GL_TRIANGLES, 0, 3, (GLsizei)NUM_INSTANCES);
```
Here is the result of instanced rendering.


 <img src="/images/instance_rendering/instanced.png" width=60% height: auto/>


Confirm that there is no delay in rendering.

[Sample Program 2](/src/instance_rendering/instanced.zip)
## Application Examples at Cepton 
Here, I share some application examples in Cepton.
In recent years, Cepton's development has been dedicated to
the object detection using the data captured by the Cepton's Lidar sensor. In my internship, I introduced the instanced rendering technique to visualize the results of object detection. 
Since the instanced rendering technique is easily extended to more complicated shapes, we used it to visualize objects commonly found during driving, such as cars and trees.

![Instances](/images/instance_rendering/instances.png)

The results were quite successful.
Although they contain a lot of triangles, (car: ~1000 triangles, tree: ~30000 triangles), they can be visualized in real-time with the instanced rendering technique (see the demo below).

![Car Demo](/images/instance_rendering/car_demo.gif)

![Tree Demo](/images/instance_rendering/tree_demo.gif)

Thank you for reading so far, and I hope this article helps your development as well.

Finally, here I would like to thank the Cepton Perception team members for the great internship experience.
I appreciated how they allowed me to try various approaches while being always willing to help me.
Without their help, I wouldn't have even hit on instanced rendering.

Instanced Rendering with OpenGL

**tl;dr** Please use Cepton SDK v2 to write your lidar applications even if you are working with Vista-P series products.

Since the very beginning of the company's history, Cepton has been releasing lidar sensors every few months. Over the last 4-5 years, we have released HR80 series (5T, W, 4T), Sora series, Vista-P series (860, P60, P90, P61, P91), and now Vista-X series (X90) and Nova. It has been our goal from day one that the software should work across all lidar sensors without API changes. With this goal in mind, we released [Cepton SDK v1](https://github.com/ceptontech/cepton_sdk_redist) in 2017. The SDKv1 supports all Cepton sensors up to Vista-P series.

In 2020, to support Vista-X series sensors, we have released an all new SDKv2 (SDKv2 will be available for download at developer.cepton.com). SDKv2 is a new SDK with a modular design to facilitate easier integration with all the 3rd party frameworks. Another goal is to be free of legacy proprietary code so that we can open source the SDK in the near future. Although different in API interfaces, SDKv2 has full support for all our old sensor products through a `LegacyTranslation` mode. We still fully support SDKv1 for our earlier generations of sensors, but we ask all new developers to use SDKv2 to be future proofed and better supported by us.

Comparison between SDKv2 and SDKv1 features:

| | SDKv2 | SDKv1 |
| ------------------------ | ----- | ----- |
| Support Vista-X and Nova | Yes | No |
| Support Vista-P and Sora | Yes | Yes |
| Modular Design | Yes | No |
| Advanced Capture Replay | Yes | No |
| Open Source | Yes* | No |

(*Source code of SDKv2 will be provided in the near future)

Comparison between these two styles of SDKs (both extracted from cepton_exporter sample app):

SDKv2:
```cpp
int InitializeSDK() {
 int err;
 err = CeptonInitialize(CEPTON_API_VERSION, CbCeptonSensorError);
 if (err != CEPTON_SUCCESS) ReportError(err, "CeptonInitialize", true);
 sdk_initialized = true;

 // Enable legacy
 CeptonEnableLegacyTranslation();

 // Listen to point data
 if (streaming) {
 err = CeptonListenPoints(CbCeptonSensorImageData, nullptr);
 if (err != CEPTON_SUCCESS) ReportError(err, "CeptonListenPoints", true);
 } else {
 err = CeptonListenFrames(aggregation_mode, CbCeptonSensorImageData, nullptr);
 if (err != CEPTON_SUCCESS) ReportError(err, "CeptonListenFrames", true);
 }

 // Listen to sensor detection
 CeptonListenSensorInfo(CbCeptonSensorInfo, nullptr);

 if (!capture_file.empty()) {
 err = CeptonReplayLoadPcap(capture_file.c_str(), 0, &capture_handle);
 if (err != CEPTON_SUCCESS) ReportError(err, "CeptonReplayLoadPcap", true);
 err = CeptonReplaySetSpeed(capture_handle, 0); // No delay replay
 if (err != CEPTON_SUCCESS) ReportError(err, "CeptonReplaySetSpeed", true);
 err = CeptonReplayPlay(capture_handle);
 if (err != CEPTON_SUCCESS) ReportError(err, "CeptonStartReplay", true);
 } else {
 err = CeptonStartNetworking();
 if (err != CEPTON_SUCCESS) ReportError(err, "CeptonStartNetworking", true);
 }
 return 0;
}
```

SDKv1:
```cpp
int InitializeSDK() {
 CeptonSensorErrorCode err;
 CeptonSDKOptions opts = cepton_sdk_create_options();
 if (streaming) {
 opts.frame.mode = CEPTON_SDK_FRAME_STREAMING;
 } else {
 // Use cover mode as the default mode
 opts.frame.mode = CEPTON_SDK_FRAME_COVER;
 }

 if (!capture_file.empty())
 opts.control_flags |= CEPTON_SDK_CONTROL_DISABLE_NETWORK;
 err = cepton_sdk_initialize(CEPTON_SDK_VERSION, &opts, CbCeptonSensorError,
 nullptr);
 if (err != CEPTON_SUCCESS) {
 ReportError(err, "initialize");
 return -1;
 }

 if (!capture_file.empty()) {
 // attempt to discover the sensors in the pcap
 err = cepton_sdk_capture_replay_open(capture_file.c_str());
 if (err != CEPTON_SUCCESS) {
 ReportError(err, "load capture");
 return -1;
 }
 err = cepton_sdk_capture_replay_set_speed(0);
 if (err != CEPTON_SUCCESS) {
 ReportError(err, "set replay speed");
 return -1;
 }
 err = cepton_sdk_capture_replay_resume_blocking(2.0f);
 if (err != CEPTON_SUCCESS) {
 ReportError(err, "pre-replay");
 return -1;
 }
 err = cepton_sdk_capture_replay_seek(0);
 if (err != CEPTON_SUCCESS) {
 ReportError(err, "rewind");
 return -1;
 }
 }

 err = cepton_sdk_listen_image_frames(CbCeptonSensorImageData, nullptr);
 if (err != CEPTON_SUCCESS) {
 ReportError(err, "listen frames");
 return -1;
 }

 if (!capture_file.empty()) {
 err = cepton_sdk_capture_replay_resume();
 if (err != CEPTON_SUCCESS) {
 ReportError(err, "load capture");
 return -1;
 }
 }

 return 0;
}
```

For more information, please visit [SDKv2 page](https://developer.cepton.com/sdk)

Blogs

Join Our Team