diff --git a/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealLidarSensor.cpp b/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealLidarSensor.cpp index 063606bdd..024fa5b54 100755 --- a/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealLidarSensor.cpp +++ b/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealLidarSensor.cpp @@ -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> 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 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) { @@ -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()); @@ -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++) @@ -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; @@ -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) { diff --git a/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealLidarSensor.h b/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealLidarSensor.h index 8fb057e1d..e9217c466 100755 --- a/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealLidarSensor.h +++ b/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealLidarSensor.h @@ -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: