Navigation Assistance & ADAS (Collision Avoidance System) on Indian Cars

Most vehicles in developing countries do not have advanced driver-assist features like forward-collision warning (FCW) system or autonomous emergency braking (AEB) nor it is affordable to upgrade the car for better safety. This solution aims to augment even the least expensive cars, in less affluent countries, with an ultra-cheap ADAS Level 0, i.e. collision avoidance and smart surround-view. The solution further extends to vehicle localization and mapping that can be deployed on autonomous buggies plying constrained environments, using the same hardware assembly.

The ADAS-CAS idea is to use a battery-powered Pi connected with a LIDAR, Pi Cam, LED SHIM, and NCS 2, mounted on the car bonnet to perceive frontal objects, along with their depth and direction. This not only enables a forward-collision warning but also driver assistance alerts about traffic signs or pedestrians, walking or crossing the road.

1  Raspberry Pi 4 Model B 

2  Intel Movidius Neural Compute Stick 

3  Raspberry Pi Camera Module V2 

4  RPLIDAR A1 M8 by SLAMTEC 

5  NVIDIA Jetson Nano Developer Kit 

6  USB 2.0 Hub, 4 Port 

7  Pimoroni Blinkt! 

8  Pimoroni LED SHIM 

9  iBall CR-222 Memory Card Reader 

10 Sony OnePlus Power Bank P102a 

11 Logitech K480 Wireless Multi-Device Keyboard 

12 Raspberry Pi micro HDMI to HDMI cable 

13 USB-A to Mini-USB Cable 

14 USB Cable, USB Type C Plug 

15 Logitech Mouse 

16 URBN 10000 mAh Power Bank 

17 Flash Memory Card, SD Card 

18 3D Printed LIDAR Mount 

19 Aux-to-Aux Stereo Audio Cable 

20 Monitor, LCD 

21 Android device 

22 Scissor, Electrician 

23 Gizga Essentials Reusable Cable Ties Strap 

24 Cello Tape 

25 Rectangular Plastic Camera mount 

26 Windshield Car holder 

27 Software - Roboflow Data Cleansing/Augment Tool 

28 Software - labelImg Annotation Software 

29 Software - Android Studio 

30 Software - Raspberry Pi Raspbian 

31 Software - balenaEtcher 

32 Software - Google Colab Pro, OpenCV, TensorFlow, LaTeX Equation Editor, Sublime Text Editor, gimp, Ubuntu Audio Recorder, Pychar 

33 3D Printing Services - To 3D print the RPI-LIDAR Mount 

Cameras generally have higher resolution than LiDAR but cameras have a limited FOV and can’t estimate distance. While rotating LIDAR has a 360° field of view, Pi Cam has only 62x48 degrees Horizontal x Vertical FoV. As we deal with multiple sensors here, we need to employ visual fusion techniques to integrate the sensor output, that is to get the distance and angle of an obstacle in front of the vehicle. Let’s first discuss the theoretical foundation of sensor fusion before hands-on implementation.

LIDAR-Camera Sensor Fusion Considerations

When a raw image from a cam is merged with raw data from RADAR or LIDAR then it’s called Low-Level Fusion or Early Fusion. In Late Fusion, detection is done before the fusion. Note that there are many challenges to project the 3D LIDAR point cloud on a 2D image. The relative orientation and translation between the two sensors must be considered in performing fusion.

1. Rotation: The coordinate system of LIDAR and Camera can be different. Distance on the LIDAR may be on the z-axis, while it is x-axis on the camera. We need to apply rotation on the LIDAR point cloud to make the coordinate system the same, i.e. multiply each LIDAR point with the Rotation matrix.
2. Translation: In an autonomous car, the LIDAR can be at the center top and the camera on the sides. The position of LIDAR and camera in each installation can be different. Based on the relative sensor position, translate the LIDAR Points by multiplying with a Translation matrix.
3. Stereo Rectification: For stereo camera setup, we need to do Stereo Rectification to make the left and right images co-planar. Thus, we need to multiply with matrix R0 to align along the horizontal Epipolar line.
4. Intrinsic calibration: Calibration is the step where you tell your camera how to convert a point in the 3D world into a pixel. To account for this, we need to multiply with an intrinsic calibration matrix containing factory calibrated values.

To sum it up, we need to multiply LIDAR points with all the 4 matrices to project on the camera image.

To project a point X in 3D onto a point Y in 2D, use the LIDAR-Camera Projection Formula.

• P = Camera Intrinsic Calibration matrix
• R0 = Stereo Rectification matrix
• R|t = Rotation & Translation to go from LIDAR to Camera
• X = Point in 3D space
• Y = Point in 2D Image

Note that we have combined both the rigid body transformations, rotation, and translation, in one matrix, R|t. Putting it together, the 3 matrices, P, R0, and R|t account for extrinsic and intrinsic calibration to project LIDAR points onto the camera image. However, the matrix values highly depend on our custom sensor installation.

This is just one piece of the puzzle. Our aim is to augment any cheap car with an end-to-end collision avoidance system and smart surround view. This would include our choice of sensors, sensor positions, data capture, custom visual fusion, and object detection, coupled with a data analysis node, to do synchronization across sensors in order to trigger driver-assist warnings to avoid danger.

Real-World Implementation with RPi & RPLIDAR A1

First, we need to assemble the Pi with RPLIDAR A1, Pi Cam, LED SHIM, and NCS 2. 2D LIDAR is used instead of 3D LIDAR as we aim to make the gadget, the cheapest possible. The unit is powered by a 5V 3A 10K mAH battery. To ease the LIDAR-RPi assembly, a LIDAR mount is 3D printed and attached on top of the RPi. Part of the mount design is taken from the STL files obtained from here

Connect RPLIDAR A1 with the USB adapter that is connected to the Pi USB using a micro-USB cable. LIDAR’s adapter provides power and converts LIDAR’s internal UART serial interface to a USB interface. Use an Aux-to-Aux cable to connect RPi to speakers. Due to physical constraints, an LED SHIM is used instead of Blinkt to signal warning messages. While the total cost of the ADAS gadget is just around US$ 150–200, one may have to shell out at least $10–20K more, to get a car model with such advanced features.

Let’s imagine, a 3D LIDAR is connected the same way as above. First, we will try to solve 3D LIDAR-Camera Sensor Fusion on the above gadget. Then we will see the variation for 2D LIDAR-Camera Fusion, so as to make it work on RPLIDAR A1.

It is clear from the above discussion that we need to do rotation, translation, stereo rectification, and intrinsic calibration to project LIDAR points on the image. We will try to apply the above formula based on the custom gadget that we built.

From the above image, you can estimate the Pi Cam is 10 mm below the LIDAR scan plane. i.e. a translation of [0, -10, 0] along the 3D-axis. Consider Velodyne HDL-64E as our 3D LIDAR, which requires 180° rotation to align the coordinate system with Pi Cam. We can compute the R|t matrix now.

As we use a monocular camera here, the stereo rectification matrix will be an identity matrix. We can make the intrinsic calibration matrix based on the hardware spec of Pi Cam V2.

For the RaspberryPi V2 camera,

• Focal Length (FL) = 3.04 mm
• FL Pixels = focal length * sx, where sx = real world to pixels ratio
• Focal Length * sx = 3.04mm * (1/ 0.00112 mm per px) = 2714.3 px

Due to a mismatch in shape, the matrices cannot be multiplied. To make it work, we need to transition from Euclidean to Homogeneous coordinates by adding 0’s and 1’s as the last row or column. After doing the multiplication we need to convert back to Homogeneous coordinates.

You can see the 3DLIDAR-CAM sensor fusion projection output after applying the projection formula on the 3D point cloud. The input sensor data from 360° Velodyne HDL-64E and camera is downloaded and fed in.

After doing 3D LIDAR-Camera Sensor Fusion, we need to do the mathematical variation for 2D, as the assembled gadget is equipped with 2D RP LIDAR A1 to minimize the cost.

2D LIDAR scans the environment in a 2D plane, orthogonal to the camera plane. The rotating scan will estimate the distance to the obstacle, for each angle from 0° to 360°. Due to the placement of LIDAR w.r.t. Pi Cam in the gadget, the camera is at +90° on the LIDAR geometry. However, note that the Field of View of Pi cam V2 is 62°x48° in horizontal x vertical direction respectively.

As both the LIDAR and Camera sensor data is available in the frontal 62° arc, we need to fuse the data. In the LIDAR scan plane, the camera data starts from +59° to +59° + 62° = 121°. We can run object detection on the image to get bounding boxes for the objects of interest. Eg: human, car, bike, traffic light, etc. Since 2D LIDAR has only width information, consider only the x_min and x_max of each bounding box.

We need to compute the LIDAR angle corresponding to an image pixel, in order to estimate the distance to the pixel. To find the distance to the object inside the bounding box, compute θ_min and θ_max corresponding to x_min & x_max using the formula to compute θ, as derived from the diagram.

Now you can find the distance to each angle between θ_min and θ_max based on the latest LIDAR scan data. Then compute the median distance of all LIDAR points that subtends the object bounding box to estimate the object depth. If the distance is below a threshold, then trigger a warning based on the angle. Repeat warning, if the box center shift by a significant distance in subsequent frames.

for detection in detections:

    if detection.score > threshold:
        class_id = int(detection.id)-1 # object class
        det_label = labels[class_id] if labels and 
                    len(labels) >= class_id else '#{}'.format(class_id)

        xmin = max(int(detection.xmin), 0)
        ymin = max(int(detection.ymin), 0)
        xmax = min(int(detection.xmax), x_width)
        ymax = min(int(detection.ymax), y_height)

        x_mid = np.mean([xmin, xmax])
        y_mid = np.mean([ymin, ymax])

        if not isAnnounced(det_label, x_mid, y_mid):

            # theta min and max corresponds to Pi cam FoV angle
            # Picam has 62 degrees horizontal FoV. Need to
            # convert to LIDAR angles at LIDAR node.
            theta_min = xmin / (x_width / 62) + 59
            theta_max = xmax / (x_width / 62) + 59

            now = time.localtime()

            client.publish("object/getdistance", str(det_label) + '|' +
                           str(theta_min) + '|' + str(theta_max) + '|' + 
                           str(now.tm_min * 60 + now.tm_sec))

            objectsInFrame.append(det_label)
            objectMidPts.append((x_mid, y_mid))

# List of objects and its mid points in last 30 frames will be stored in dqueue
if len(objectsInFrame) > 0:
    objLastFrames.extend([objectsInFrame])
    objMidsLastFrames.extend([objectMidPts])
    noObjFrames = 0
else:
    noObjFrames += 1
    
# If no objects found in last 30 frames, reset the queue
if noObjFrames >= 30:
    objMidsLastFrames.clear()
    objLastFrames.clear()
    noObjFrames = 0

To build a non-blocking data flow, each independent node is associated to different hardware components. i.e., "object detection" node uses Movidius for inference, whereas the "distance estimation" node takes LIDAR data as input, while the "Alert" module signals to Pimoroni Blinkt and the speaker. The modules are communicated via MQTT messages on respective topics.

The time synchronization module takes care of the "data relevance factor" for Sensor Fusion. For ADAS, the location of detected objects by 'Node 1' may change, as the objects move. Thus, the distance estimate to the bounding box could go wrong after 2-3 seconds (while the message can remain in the MQTT queue). In order to synchronize, current time = 60*minutes + seconds is appended to the message (to ignore lagged messages at receiving end).

The speaker is be wired internally to make the announcement more audible to the driver. This ADAS device can be connected to the CAN bus to let it accelerate, steer, or apply the brake. RPi doesn't have a built-in CAN Bus, but its GPIO includes SPI Bus, which is supported by a number of CAN controllers like MCP2515. So, autonomous emergency braking (AEB) can also be done with a collision-avoidance system (CAS) by connecting this device to the CAN bus.

We can estimate the depth of moving objects h using LIDAR by computing the median distance of LIDAR laser points between two subtending angles, based on LIDAR -Picam position (triangulation)

def getObjectDistance (angle_min, angle_max):

    minDist = 0
    lidar = RPLidar(None, PORT_NAME)

    try:
            for scan in lidar_scans(lidar):

                for (_, angle, distance) in scan:
                    scan_data[min([359, floor(angle)])] = distance

                # fetching all non zero distance values between subtended angles
                allDists = [scan_data[i] for i in range(360)
                      if i >= angle_min and i <= angle_max and scan_data[i] > 0]

                # if half the distance values are filled in then break
                if (2 * len(allDists) > angle_max - angle_min):

                    minDist = np.min(allDists)
                    lidar.stop()
                    lidar.disconnect()
                    return minDist

    except KeyboardInterrupt:
        print('Stoping LIDAR Scan')

I wanted to tweak the solution to cater the solution for the Indian situation. The Indian traffic conundrum is so unique that it demands custom solutions. To start with, we need to train object detection models with Indian vehicles such as trucks, tempos, vans, autos, cycle rickshaws, etc.

Further, to enhance smart surround view, we need to train the model with Indian traffic signs and signboards to give more meaningful driver-assist warnings on Indian roads. It's a common sight in India, the animals like cows, pigs, buffaloes, goats, dogs, etc., cross the roads and highways. Hence, it's beneficial to detect them as well.

In the above animation, you can see the animals plying the road are detected as obstacles and classified accordingly. Another classification attempt on Indian traffic signboards is also successfully done. The annotated Indian Traffic Sign dataset is provided by Datacluster Labs (Bangalore, India) and I have captured the 'animal on the road' videos myself.

The complete source code of ADAS-CAS solution is available here.

SLAM (Simultaneous Localization And Mapping) is the core algorithm being used in autonomous cars, robot navigation, robotic mapping, virtual reality, and augmented reality. SLAM algorithms use LiDAR and IMU data to simultaneously locate the robot in real-time and generate a coherent map of surrounding landmarks such as buildings, trees, rocks, and other world features, at the same time.

This classic chicken and egg problem has been approximately solved using methods like Particle Filter, Extended Kalman Filter (EKF), Covariance intersection, and Graph SLAM. SLAM enables accurate mapping where GPS localization is unavailable, such as indoor spaces. If we can do robot localization on RPi then it is easy to make a moving car or walking robot that can ply indoors autonomously.

Let's make use of the same hardware assembly, minus PiCam, to do SLAM along with LIDAR point cloud visualization to assist navigation. Finally, the LIDAR point cloud map is visualized on a remote machine using MQTT. First, let’s discuss Graph SLAM and do a custom implementation.

Assume a robot in the 2D world, that tries to move 10 units to the right from x to x’. Due to motion uncertainty, x’ = x + 10 may not hold, but it will be Gaussian centered around x + 10. The Gaussian will peak when x’ approaches x + 10

If x1 is away from x0 by 10 units, the Kalman Filter models the uncertainty using the Gaussian with (x1 – x0 –10). Hence, there is still a probability associated with locations < 10 and > 10.

There is another similar Gaussian at x2 with a higher spread. The total probability of the entire route is the product of the two Gaussians. We can drop the constants, as we just need to maximize the likelihood of the position x1, given x0. Thus the product of Gaussian becomes the sum of exponent terms, i.e. the constraint has only x’s and sigma.

Graph SLAM models the constraints as System of Linear Equations (SLEs), with a Ω matrix containing the coefficients of the variables and a ξ vector that contains the limiting value of the constraints. Every time an observation is made between 2 poses, a ‘local addition’ is done on the 4 matrix elements (as the product of gaussian becomes the sum of exponents).

Let’s say, the robot moves from x0 to x1 to x2 which are 5 and -4 units apart. The coefficient of x’s and RHS values are added to corresponding cells. Consider the landmark L0 is at a distance of 9 units from x1.

Once the Ω matrix and ξ vector is filled in like in the diagram, take the inverse of Ω matrix and multiply with ξ vector.

The complete source code and results of Custom SLAM implementation can be found in the IPython notebook here.

## Optimized implementation of Graph SLAM.

## slam takes in 6 arguments and returns mu, 
## mu is the entire path traversed by a robot (all x,y poses) *and* all landmarks locations
def slam(data, N, num_landmarks, world_size, motion_noise, measurement_noise):
    
    coefficients = [1, -1, -1, 1]
    
    # initialize the constraints
    initial_omega_1, initial_xi_1, initial_omega_2, initial_xi_2 = \\
                                   initialize_constraints(N, num_landmarks, world_size)
    
    ## get all the motion and measurement data as you iterate
    for i in range(len(data)):
        
        landmarks = data[i][0]  # measurement
        motion = data[i][1]  # motion
        
        # setting measurement constraints
        for landmark in landmarks:
            
            # calculate indices in the same order as coefficients (to meaningfully add)
            index1 = [i, i, N+landmark[0], N+landmark[0]]
            index2 = [i, N+landmark[0], i, N+landmark[0]]
            
            # dx update
            initial_omega_1[index1, index2] = initial_omega_1[index1, index2] + \\
                              np.divide(coefficients, measurement_noise)
            initial_xi_1[[i, N+landmark[0]]] = initial_xi_1[[i, N+landmark[0]]] + \\
                np.divide([-landmark[1], landmark[1]], measurement_noise)
            
            # dy update
            initial_omega_2[index1, index2] = initial_omega_2[index1, index2] + \\
                                        np.divide(coefficients, measurement_noise)
            initial_xi_2[[i, N+landmark[0]]] = initial_xi_2[[i, N+landmark[0]]] + \\
                          np.divide([-landmark[2], landmark[2]], measurement_noise)
        
        index1 = [i, i, i+1, i+1]
        index2 = [i, i+1, i, i+1]
        
        # dx update
        initial_omega_1[index1, index2] = initial_omega_1[index1, index2] + \\
                                       np.divide(coefficients, motion_noise)
        initial_xi_1[[i, i+1]] = initial_xi_1[[i, i+1]] + \\
                                np.divide([-motion[0], motion[0]], motion_noise)
        
        # dy update
        initial_omega_2[index1, index2] = initial_omega_2[index1, index2] + \\
                                       np.divide(coefficients, motion_noise)
        initial_xi_2[[i, i+1]] = initial_xi_2[[i, i+1]] + 
                            np.divide([-motion[1], motion[1]], motion_noise)
        
    ## To update the constraint matrix/vector to account for all measurements, 
    # measurement noise, motion and motion noise. Compute best estimate of poses
    # and landmark positions using the formula, omega_inverse * Xi
    mu_1 = np.linalg.inv(np.matrix(initial_omega_1)) * \\
                    np.expand_dims(initial_xi_1, 0).transpose()
    mu_2 = np.linalg.inv(np.matrix(initial_omega_2)) * \\
                    np.expand_dims(initial_xi_2, 0).transpose()
    
    mu = []
    for i in range(len(mu_1)):
        mu.extend((mu_1[i], mu_2[i]))
    
    return mu

In the video, we can see SLAM in action, when deployed on a Raspberry Pi with RPLidar A1 M8, running on a 5V 3A battery. As you can see in the video at the top, the portable unit is taken across various rooms of my house and a real-time trajectory is transmitted to the MQTT server and also stored on the SD card of RPi.

You can see the visualization of the LIDAR point cloud map and estimated vehicle trajectory using PyRoboViz. In the above video, you can see me going across different rooms on the floor.

Interestingly, we can re-route the real-time visualization to a remote machine using MQTT, for real-time visualization. The vehicle position, angle, and map can be encoded as a byte array that is decoded by the MQTT client.

# At the MQTT Transmission side

    data2Transmit = np.array([x, y, theta])

    # Map which is saved as a bytearray is appended at the end
    if scan_count % 30 == 0:
        client.publish("safetycam/topic/slamviz", \\
                           data2Transmit.tobytes() + mapbytes)
            
# At the MQTT receiving side

    # As 3 float values takes 8*3 = 24 bytes
    robotPos_bytes = msg.payload[:24]
    map_bytes = msg.payload[24:]

    robotPos = np.frombuffer(robotPos_bytes, dtype='float64')
    robotPos = np.array(robotPos)

    x, y, theta = robotPos
    viz.display(x / 1000., y / 1000., theta, map_bytes)

Note that, the high-dense linear point cloud in the LIDAR maps represents stable obstacles like walls. Hence, we can use algorithms like Hough Transform to find the best fit line on these linear point clouds to generate floor maps.

From 3D LIDAR point cloud, we can even construct a 3D map of surroundings using Structure from Motion techniques,

• Use Detectors such as SIFT, SURF, ORB, Harris to find features like corners, gradients, edges, etc.
• Use Descriptors such as HOG to encode these features.
• Use Matchers such as FLANN to map features across images.
• Use 3D Triangulation to reconstruct a 3D Point Cloud.

We can use the idea of SLAM navigation to deploy an autonomous mobile robot inside closed environments like airports, warehouses, or industrial plants. The ADAS-CAS solution can also be integrated for collision warning or autonomous driving in constrained environments, while it is suitable outdoors as well.

The source code of this solution is available in Repo 1 and Repo 2.

If you have any queries or suggestions, you can reach me here

References

1. Breezy Implementation of SLAM: https://github.com/simondlevy/BreezySLAM

Note: Have contacted the author of Breezy SLAM, Prof. Simon D. Levy, CSE Dept., Washington and Lee University for LIDAR scan data error, during visualization. I have fixed the code by separating lidar scan code as a separate thread and implemented inter-thread communication and he acknowledged the fix.

2. LIDAR-Camera Sensor Fusion High Level: https://www.thinkautonomous.ai/blog/?p=lidar-and-camera-sensor-fusion-in-self-driving-cars

3. LIDAR data scan code stub by Adafruit: https://learn.adafruit.com/remote-iot-environmental-sensor/code

4. Camera Calibration and Intrinsic Matrix Estimation: https://www.cc.gatech.edu/classes/AY2016/cs4476_fall/results/proj3/html/agartia3/index.html

5. Visual Fusion For Autonomous Cars Course at PyImageSearch University: https://www.pyimagesearch.com/pyimagesearch-university/

6. LIDAR Distance Estimation: https://en.wikipedia.org/wiki/Lidar

7. RPLIDAR A1 M8 Hardware Specification: https://www.generationrobots.com/media/rplidar-a1m8-360-degree-laser-scanner-development-kit-datasheet-1.pdf

8. Model Training, Data Cleansing & Augmentation: www.roboflow.com

9. LIDAR Point Cloud Visualization: https://github.com/simondlevy/PyRoboViz

10. Udacity Computer Vision Nanodegree: https://www.udacity.com/course/computer-vision-nanodegree--nd891

11. LIDAR data scan code stub by Adafruit: https://learn.adafruit.com/remote-iot-environmental-sensor/code

12. RPLIDAR A1 M8 Hardware Specification: https://www.generationrobots.com/media/rplidar-a1m8-360-degree-laser-scanner-development-kit-datasheet-1.pdf