Problem with fusing a few LIDARS together

[marikf98]

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 }

[wouter-heerwegh]

Hi @marikf98

Your screenshots seem a bit strange, also looking at the data (especially the cones) it looks like the pointclouds were properly detected on the sim side, they just seem to be offset for some reason.

When looking at the config you added, I see that your entire horizontal fov would be around 115 degrees, but in your final picture it goes way beyong 180.

Have you checked your yaw_angles variable and made sure that you transformed the pointclouds properly?