# 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

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

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

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

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

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

Developer Portal

Downloads

Documentations

Blogs

Videos

FAQs

Join Our Team