
## 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.

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

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.

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

### 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.

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


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.
<br/>
<br/>

### 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.
<br/>

### 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.
<p align="center">
  <img src="/images/trailer_hitching/detected.PNG"  width=60% height: auto/>
</p>
In the image above, the green part is the trailer tongue detected from the trailer point cloud (red).

<br/>

## 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.

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

### 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.

<p align="center">
  <img src="/images/trailer_hitching/bottom.PNG"  width=50% height: auto/>
</p>
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.

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


### 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.
<p align="center">
  <img src="/images/trailer_hitching/detection-working.gif"  width=60% height: auto/>
</p>

### 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.

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



# 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!
<p align="center">
  <img src="/images/trailer_hitching/utility_trailer.gif"  width=80% height: auto/>
</p>

## 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

<p>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.</p>

<p>Currently 802.1AS is supported, with full support planned for later.</p>

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

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

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

Developer Portal

Downloads

Documentations

Blogs

Videos

FAQs

Join Our Team