FS-Driverless / FS-Driverless/Formula-Student-Driverless-Simulator

Problem with fusing a few LIDARS together

Open
#378 1 comment 0 reactions 0 assignees View on GitHub
Dominant language
C++
Stars
286
Forks
126
PR merge metrics
No merged PRs in 30d

Description

Hey,
We are trying to enhance the lidar read, we fused 6 lidar together and divided the horizontal axes into 6 parts and used rotation matrixes to fuse all the readings, it does seem that we get all the cones and objects that the LIDAR sees but we keep on getting blind spots in the visualisation (as can be seen below) and in the final output
would really appreciate some help with this
thanks!
This is our code:

`def rotation_matrix(yaw, pitch=0, roll=0):
Rz = np.array([
[np.cos(yaw), -np.sin(yaw), 0],
[np.sin(yaw), np.cos(yaw), 0],
[0, 0, 1]
])

Ry = np.array([
[np.cos(pitch), 0, np.sin(pitch)],
[0, 1, 0],
[-np.sin(pitch), 0, np.cos(pitch)]
])

Rx = np.array([
[1, 0, 0],
[0, np.cos(roll), -np.sin(roll)],
[0, np.sin(roll), np.cos(roll)]
])
return Rz @ Ry @ Rx
# Combine the rotations in ZYX order

def transform_lidar_points(points, yaw, pitch=0, roll=0):

R = rotation_matrix(yaw, pitch, roll)
return points @ R.T

## adds the fsds package located the parent directory to the pyhthon path
sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), '..')))
import fsds

# connect to the AirSim simulator
client = fsds.FSDSClient()

# Check network connection
client.confirmConnection()

lidardata = client.getLidarData(lidar_name = 'Lidar')
lidardata2 = client.getLidarData(lidar_name = 'Lidar2')
lidardata3 = client.getLidarData(lidar_name = 'Lidar3')
lidardata4 = client.getLidarData(lidar_name = 'Lidar4')
lidardata5 = client.getLidarData(lidar_name = 'Lidar5')
lidardata6 = client.getLidarData(lidar_name = 'Lidar6')

point_list1 = lidardata.point_cloud
point_list2 = lidardata2.point_cloud
point_list3 = lidardata3.point_cloud
point_list4 = lidardata4.point_cloud
point_list5 = lidardata5.point_cloud
point_list6 = lidardata6.point_cloud

# Convert the list of floats into a list of xyz coordinates
points = numpy.array(point_list1, dtype=numpy.dtype('f4'))
points = numpy.reshape(points, (int(points.shape[0]/3), 3))

# Convert the list of floats into a list of xyz coordinates
points2 = numpy.array(point_list2, dtype=numpy.dtype('f4'))
points2 = numpy.reshape(points2, (int(points2.shape[0]/3), 3))

# Convert the list of floats into a list of xyz coordinates
points3 = numpy.array(point_list3, dtype=numpy.dtype('f4'))
points3 = numpy.reshape(points3, (int(points3.shape[0]/3), 3))

# Convert the list of floats into a list of xyz coordinates
points4 = numpy.array(point_list4, dtype=numpy.dtype('f4'))
points4 = numpy.reshape(points4, (int(points4.shape[0]/3), 3))

# Convert the list of floats into a list of xyz coordinates
points5 = numpy.array(point_list5, dtype=numpy.dtype('f4'))
points5 = numpy.reshape(points5, (int(points5.shape[0]/3), 3))

# Convert the list of floats into a list of xyz coordinates
points6 = numpy.array(point_list6, dtype=numpy.dtype('f4'))
points6 = numpy.reshape(points6, (int(points6.shape[0]/3), 3))

mini_lidar_data = [points, points2, points3, points4, points5, points6]

# Define the yaw angles for each mini LiDAR (in radians)
# Assume the horizontal field of view is 115° divided into 6 segments
fov_horizontal = 115 # Total horizontal field of view in degrees
n_lidars = 6
overlap = 2
step_size = (fov_horizontal + (overlap * (n_lidars - 1))) / n_lidars
yaw_angles = [np.deg2rad(i * step_size) for i in range(n_lidars)]

# Transform each mini LiDAR's data to the global frame
transformed_lidar_data = []
for mat, yaw in zip(mini_lidar_data, yaw_angles):
transformed_points = transform_lidar_points(mat, yaw)
transformed_lidar_data.append(transformed_points)

# Combine all transformed points into a single global matrix
global_lidar_data = np.vstack(transformed_lidar_data)`

and this is out settings.json configuration:
`"Lidar": {
"SensorType": 6,
"Enabled": true,
"X": 1,
"Y": 0,
"Z": 0.5,
"Roll": 0,
"Pitch": -5,
"Yaw": 0,
"NumberOfLasers": 64,
"PointsPerScan": 48000,
"VerticalFOVUpper": 7.5,
"VerticalFOVLower": -7.5,
"HorizontalFOVStart": -57.5,
"HorizontalFOVEnd": -38.33,
"RotationsPerSecond": 10,
"DrawDebugPoints": true
},
"Lidar2": {
"SensorType": 6,
"Enabled": true,
"X": 1,
"Y": 0,
"Z": 0.5,
"Roll": 0,
"Pitch": -5,
"Yaw": 0,
"NumberOfLasers": 64,
"PointsPerScan": 48000,
"VerticalFOVUpper": 7.5,
"VerticalFOVLower": -7.5,
"HorizontalFOVStart": -38.33,
"HorizontalFOVEnd": -19.16,
"RotationsPerSecond": 10,
"DrawDebugPoints": true
},
"Lidar3": {
"SensorType": 6,
"Enabled": true,
"X": 1,
"Y": 0,
"Z": 0.5,
"Roll": 0,
"Pitch": -5,
"Yaw": 0,
"NumberOfLasers": 64,
"PointsPerScan": 48000,
"VerticalFOVUpper": 7.5,
"VerticalFOVLower": -7.5,
"HorizontalFOVStart": -19.16,
"HorizontalFOVEnd": 0,
"RotationsPerSecond": 10,
"DrawDebugPoints": true
},
"Lidar4": {
"SensorType": 6,
"Enabled": true,
"X": 1,
"Y": 0,
"Z": 0.5,
"Roll": 0,
"Pitch": -5,
"Yaw": 0,
"NumberOfLasers": 64,
"PointsPerScan": 48000,
"VerticalFOVUpper": 7.5,
"VerticalFOVLower": -7.5,
"HorizontalFOVStart": 0,
"HorizontalFOVEnd": 19.16,
"RotationsPerSecond": 10,
"DrawDebugPoints": true
},
"Lidar5": {
"SensorType": 6,
"Enabled": true,
"X": 1,
"Y": 0,
"Z": 0.5,
"Roll": 0,
"Pitch": -5,
"Yaw": 0,
"NumberOfLasers": 64,
"PointsPerScan": 48000,
"VerticalFOVUpper": 7.5,
"VerticalFOVLower": -7.5,
"HorizontalFOVStart": 19.17,
"HorizontalFOVEnd": 38.33,
"RotationsPerSecond": 10,
"DrawDebugPoints": true
},
"Lidar6": {
"SensorType": 6,
"Enabled": true,
"X": 1,
"Y": 0,
"Z": 0.5,
"Roll": 0,
"Pitch": -5,
"Yaw": 0,
"NumberOfLasers": 64,
"PointsPerScan": 48000,
"VerticalFOVUpper": 7.5,
"VerticalFOVLower": -7.5,
"HorizontalFOVStart": 38.33,
"HorizontalFOVEnd": 57.5,
"RotationsPerSecond": 10,
"DrawDebugPoints": true
}`
![WhatsApp Image 2024-12-17 at 10 54 12_d81e4d04](https://github.com/user-attachments/assets/458c4b63-790e-4a55-9eb9-1e41f01ad76b)
![WhatsApp Image 2024-12-17 at 10 54 13_20fb7a81](https://github.com/user-attachments/assets/cfedda37-b75d-4f69-8c9d-a7bbe97c75df)
![WhatsApp Image 2024-12-17 at 10 54 13_4b18d939](https://github.com/user-attachments/assets/b2658eb1-bd46-46b4-b2b1-cd915fde574e)

Contributor guide

No contributing guide indexed for this repository

Assessment

This issue has not been assessed yet.

Get new issues in your inbox

A short digest of beginner-friendly GitHub issues.