Skip to content

Cast each LiDAR tick's rays in one parallel batch #170

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We鈥檒l occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
WouterJansen merged 1 commit into from
Oct 5, 2026
Merged

Cast each LiDAR tick's rays in one parallel batch #170

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
70 changes: 45 additions & 25 deletions Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealLidarSensor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -145,7 +145,45 @@ bool UnrealLidarSensor::getPointCloud(const msr::airlib::Pose& lidar_pose, const
point_cloud_draw_.assign(points_to_scan_with_one_laser * number_of_lasers, FVector());
}

// shoot lasers
// Shoot the lasers of consecutive horizontal angles in one parallel batch: one batch per angle (one
// ray per channel) spent more time handing out and waiting for the work than tracing. Noise is drawn
// here, as the generator can't be shared between threads.
TArray<TPair<uint32, uint32>> scan_angles; // angle index, step (1-based)
scan_angles.Reserve(points_to_scan_with_one_laser);
auto shoot_lasers = [&]() {
const int32 ray_count = scan_angles.Num() * number_of_lasers;
TArray<float> noise_samples;
if (params.generate_noise) {
noise_samples.SetNumUninitialized(ray_count);
for (float& sample : noise_samples)
sample = dist_(gen_);
}
ParallelFor(ray_count, [&](int32 ray) {
const uint32 laser = ray % number_of_lasers;
const uint32 angle_index = scan_angles[ray / number_of_lasers].Key;
const uint32 i = scan_angles[ray / number_of_lasers].Value;
float vertical_angle = laser_angles_[laser];
uint32 current_point_index = number_of_lasers * angle_index + laser;
uint32 draw_index = number_of_lasers * (i - 1) + laser; // steps start at 1
Vector3r point;
FVector draw_point;
std::string label;

// shoot laser and get the impact point, if any
if (shootLaser(lidar_pose, vehicle_pose, laser, horizontal_angles_[angle_index], vertical_angle, params,
params.generate_noise ? noise_samples[ray] : 0.0f, point, label, draw_point))
{
point_cloud[current_point_index * 3] = point.x();
point_cloud[current_point_index * 3 + 1] = point.y();
point_cloud[current_point_index * 3 + 2] = point.z();
groundtruth[current_point_index] = label;
if (sensor_params_.draw_debug_points)
point_cloud_draw_[draw_index] = draw_point;
}
});
scan_angles.Reset();
};

for (uint32 i = 1; i <= points_to_scan_with_one_laser; ++i)
{
if (current_horizontal_angle_index_ == horizontal_angles_.Num() - 1) {
Expand All @@ -160,6 +198,8 @@ bool UnrealLidarSensor::getPointCloud(const msr::airlib::Pose& lidar_pose, const


if ((previous_horizontal_angle > horizontal_angle) && (point_cloud.size() != 0)) {
// the sweep is complete once the lasers of its last angles are shot
shoot_lasers();
if ((((int)point_cloud.size() / 3) != params.measurement_per_cycle * number_of_lasers) || (groundtruth.size() != params.measurement_per_cycle * number_of_lasers))
{
UE_LOG(LogTemp, Warning, TEXT("Pointcloud or labels incorrect size! points:%i labels:%i"), (int)(point_cloud.size() / 3), groundtruth.size());
Expand All @@ -186,30 +226,10 @@ bool UnrealLidarSensor::getPointCloud(const msr::airlib::Pose& lidar_pose, const
continue;
}

ParallelFor(number_of_lasers, [&](uint32 laser) {
float vertical_angle = laser_angles_[laser];
uint32 current_point_index = number_of_lasers * current_horizontal_angle_index_ + laser;
uint32 draw_index = number_of_lasers * i + laser;
Vector3r point;
FVector draw_point;
std::string label;

// shoot laser and get the impact point, if any
if (shootLaser(lidar_pose, vehicle_pose, laser, horizontal_angle, vertical_angle, params, point, label, draw_point))
{
point_cloud[current_point_index * 3] = point.x();
point_cloud[current_point_index * 3 + 1] = point.y();
point_cloud[current_point_index * 3 + 2] = point.z();
groundtruth[current_point_index] = label;
if (sensor_params_.draw_debug_points)
point_cloud_draw_[draw_index] = draw_point;
}
});



scan_angles.Emplace(current_horizontal_angle_index_, i);
previous_horizontal_angle = horizontal_angles_[current_horizontal_angle_index_];
}
shoot_lasers();

if (sensor_params_.draw_debug_points) {
for (uint32 j = 0; j < point_cloud_draw_.size(); j++)
Expand All @@ -234,7 +254,7 @@ FVector UnrealLidarSensor::Vector3rToFVector(const Vector3r& input_vector) {
// simulate shooting a laser via Unreal ray-tracing.
bool UnrealLidarSensor::shootLaser(const msr::airlib::Pose& lidar_pose, const msr::airlib::Pose& vehicle_pose,
const uint32 laser, const float horizontal_angle, const float vertical_angle,
const msr::airlib::LidarSimpleParams params, Vector3r &point, std::string &label, FVector& raw_point)
const msr::airlib::LidarSimpleParams& params, const float noise_sample, Vector3r& point, std::string& label, FVector& raw_point)
{
// start position
Vector3r start = VectorMath::add(lidar_pose, vehicle_pose).position;
Expand Down Expand Up @@ -293,7 +313,7 @@ bool UnrealLidarSensor::shootLaser(const msr::airlib::Pose& lidar_pose, const ms
// If enabled add range noise
if (params.generate_noise) {
// Add noise based on normal distribution taking into account scaling of noise with distance
float distance_noise = dist_(gen_) * (1 + ((hit_result.Distance / 100) / params.range) * (params.noise_distance_scale - 1));
float distance_noise = noise_sample * (1 + ((hit_result.Distance / 100) / params.range) * (params.noise_distance_scale - 1));

Vector3r impact_point_local = VectorMath::rotateVector(VectorMath::front(), ray_q_w, true) * ((hit_result.Distance / 100) + distance_noise) + start;
if (params.external) {
Expand Down
2 changes: 1 addition & 1 deletion Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealLidarSensor.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,7 @@ class UnrealLidarSensor : public msr::airlib::LidarSimple
void createLasers();
bool shootLaser(const msr::airlib::Pose& lidar_pose, const msr::airlib::Pose& vehicle_pose,
const uint32 channel, const float horizontal_angle, const float vertical_angle,
const msr::airlib::LidarSimpleParams params, Vector3r &point, std::string &label, FVector& raw_point);
const msr::airlib::LidarSimpleParams& params, const float noise_sample, Vector3r& point, std::string& label, FVector& raw_point);
FVector Vector3rToFVector(const Vector3r& input_vector);

private:
Expand Down