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

Problem with fusing a few LIDARS together

Offen
#378 1 Kommentar 0 Reaktionen 0 zugewiesene Personen Auf GitHub ansehen
Vorherrschende Sprache
C++
Sterne
286
Forks
126
PR-Merge-Kennzahlen
Keine gemergten PRs in 30 T.

Beschreibung

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)

Beitragsleitfaden

Für dieses Repository ist kein Beitragsleitfaden indexiert

Rechercherichtung

Beginne damit, die blinden Bereiche mithilfe des bereitgestellten Python-lidar-fusion-Codes und der Konfiguration settings.json zu reproduzieren. Vergleiche den konfigurierten horizontalen Bereich jedes Lidars mit der Rotation und den Koordinatentransformationen, die für seine Punktwolke verwendet werden. Die Aufgabe ist abgeschlossen, wenn eine reproduzierbare Ursache identifiziert und eine Korrektur für die fusionierte Visualisierung und die endgültige Ausgabe dokumentiert oder validiert wurde.

Vom Indexierungsmodell aus dem Issue-Text verfasst.

Bewertung

Tech-Stack
python
Bereich
computer-vision, robotics
Issue-Typ
Bug
Schwierigkeit
4/5
Geschätzter Aufwand
3-5 Tage
Aktivitätsstatus
Veraltet
Klarheit
Muss geklärt werden
Anfängerfreundlichkeit
20/100

Neue Issues direkt in Ihr Postfach

Eine kurze Übersicht über anfängerfreundliche GitHub-Issues.