Problem with fusing a few LIDARS together #378
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 }
