Improve lidar performance

AirLib/include/common/AirSimSettings.hpp

@@ -249,6 +249,11 @@ namespace airlib
 
         struct LidarSetting : SensorSetting
         {
+            enum class DataFrame
+            {
+                VehicleInertialFrame,
+                SensorLocalFrame
+            };
         };
 
         struct VehicleSetting

AirLib/include/sensors/lidar/LidarSimpleParams.hpp

@@ -36,7 +36,7 @@ namespace airlib
         };
 
         bool draw_debug_points = false;
-        std::string data_frame = AirSimSettings::kVehicleInertialFrame;
+        AirSimSettings::LidarSetting::DataFrame data_frame;
 
         bool external_controller = true;
 
@@ -53,7 +53,16 @@ namespace airlib
             points_per_second = settings_json.getInt("PointsPerSecond", points_per_second);
             horizontal_rotation_frequency = settings_json.getInt("RotationsPerSecond", horizontal_rotation_frequency);
             draw_debug_points = settings_json.getBool("DrawDebugPoints", draw_debug_points);
-            data_frame = settings_json.getString("DataFrame", data_frame);
+            std::string frame = settings_json.getString("DataFrame", AirSimSettings::kVehicleInertialFrame);
+            if (frame == AirSimSettings::kVehicleInertialFrame) {
+                data_frame = AirSimSettings::LidarSetting::DataFrame::VehicleInertialFrame;
+            }
+            else if (frame == AirSimSettings::kSensorLocalFrame) {
+                data_frame = AirSimSettings::LidarSetting::DataFrame::SensorLocalFrame;
+            }
+            else {
+                throw std::runtime_error("Unknown requested data frame");
+            }
             external_controller = settings_json.getBool("ExternalController", external_controller);
 
             vertical_FOV_upper = settings_json.getFloat("VerticalFOVUpper", Utils::nan<float>());

Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp

@@ -838,11 +838,10 @@ void ASimModeBase::drawLidarDebugPoints()
 
                         FVector uu_point;
 
-                        if (lidar->getParams().data_frame == AirSimSettings::kVehicleInertialFrame) {
+                        if (lidar->getParams().data_frame == AirSimSettings::LidarSetting::DataFrame::VehicleInertialFrame) {
                             uu_point = pawn_sim_api->getNedTransform().fromLocalNed(point);
                         }
-                        else if (lidar->getParams().data_frame == AirSimSettings::kSensorLocalFrame) {
-
+                        else if (lidar->getParams().data_frame == AirSimSettings::LidarSetting::DataFrame::SensorLocalFrame) {
                             Vector3r point_w = VectorMath::transformToWorldFrame(point, lidar_data.pose, true);
                             uu_point = pawn_sim_api->getNedTransform().fromLocalNed(point_w);
                         }

Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealLidarSensor.cpp

@@ -6,6 +6,7 @@
 #include "common/Common.hpp"
 #include "NedTransform.h"
 #include "DrawDebugHelpers.h"
+#include "Runtime/Core/Public/Async/ParallelFor.h"
 
 // ctor
 UnrealLidarSensor::UnrealLidarSensor(const AirSimSettings::LidarSetting& setting,
@@ -18,7 +19,7 @@ UnrealLidarSensor::UnrealLidarSensor(const AirSimSettings::LidarSetting& setting
 // initializes information based on lidar configuration
 void UnrealLidarSensor::createLasers()
 {
-    msr::airlib::LidarSimpleParams params = getParams();
+    const msr::airlib::LidarSimpleParams params = getParams();
 
     const auto number_of_lasers = params.number_of_channels;
 
@@ -46,7 +47,7 @@ void UnrealLidarSensor::getPointCloud(const msr::airlib::Pose& lidar_pose, const
     point_cloud.clear();
     segmentation_cloud.clear();
 
-    msr::airlib::LidarSimpleParams params = getParams();
+    const msr::airlib::LidarSimpleParams params = getParams();
     const auto number_of_lasers = params.number_of_channels;
 
     // cap the points to scan via ray-tracing; this is currently needed for car/Unreal tick scenarios
@@ -73,28 +74,33 @@ void UnrealLidarSensor::getPointCloud(const msr::airlib::Pose& lidar_pose, const
     const float laser_start = std::fmod(360.0f + params.horizontal_FOV_start, 360.0f);
     const float laser_end = std::fmod(360.0f + params.horizontal_FOV_end, 360.0f);
 
-    // shoot lasers
-    for (auto laser = 0u; laser < number_of_lasers; ++laser) {
-        const float vertical_angle = laser_angles_[laser];
+    const uint32 total_jobs = number_of_lasers * points_to_scan_with_one_laser;
 
-        for (auto i = 0u; i < points_to_scan_with_one_laser; ++i) {
-            const float horizontal_angle = std::fmod(current_horizontal_angle_ + angle_distance_of_laser_measure * i, 360.0f);
+    point_cloud.assign(total_jobs * 3, FLT_MAX);
+    segmentation_cloud.assign(total_jobs, -1);
 
-            // check if the laser is outside the requested horizontal FOV
-            if (!VectorMath::isAngleBetweenAngles(horizontal_angle, laser_start, laser_end))
-                continue;
+    ParallelFor(total_jobs, [&](int32 idx) {
+        int32 laser_idx = (idx / points_to_scan_with_one_laser) % number_of_lasers;
+        const float vertical_angle = laser_angles_[laser_idx];
+        const float horizontal_angle = std::fmod(current_horizontal_angle_ + angle_distance_of_laser_measure * (idx % points_to_scan_with_one_laser), 360.0f);
 
+        // check if the laser is outside the requested horizontal FOV
+        if (VectorMath::isAngleBetweenAngles(horizontal_angle, laser_start, laser_end)) {
             Vector3r point;
             int segmentationID = -1;
             // shoot laser and get the impact point, if any
-            if (shootLaser(lidar_pose, vehicle_pose, laser, horizontal_angle, vertical_angle, params, point, segmentationID)) {
-                point_cloud.emplace_back(point.x());
-                point_cloud.emplace_back(point.y());
-                point_cloud.emplace_back(point.z());
-                segmentation_cloud.emplace_back(segmentationID);
+            if (shootLaser(lidar_pose, vehicle_pose, horizontal_angle, vertical_angle, params, point, segmentationID)) {
+                point_cloud[idx * 3] = point.x();
+                point_cloud[idx * 3 + 1] = point.y();
+                point_cloud[idx * 3 + 2] = point.z();
+                segmentation_cloud[idx] = segmentationID;
             }
         }
-    }
+    });
+
+    // erase�remove idiom to handle non-valid elements
+    point_cloud.erase(std::remove(point_cloud.begin(), point_cloud.end(), FLT_MAX), point_cloud.end());
+    segmentation_cloud.erase(std::remove(segmentation_cloud.begin(), segmentation_cloud.end(), -1), segmentation_cloud.end());
 
     current_horizontal_angle_ = std::fmod(current_horizontal_angle_ + angle_distance_of_tick, 360.0f);
 
@@ -103,8 +109,8 @@ void UnrealLidarSensor::getPointCloud(const msr::airlib::Pose& lidar_pose, const
 
 // 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, int& segmentationID)
+                                   const float horizontal_angle, const float vertical_angle,
+                                   const msr::airlib::LidarSimpleParams& params, Vector3r& point, int& segmentationID)
 {
     // start position
     Vector3r start = VectorMath::add(lidar_pose, vehicle_pose).position;
@@ -136,8 +142,10 @@ bool UnrealLidarSensor::shootLaser(const msr::airlib::Pose& lidar_pose, const ms
         if (hitActor != nullptr) {
             TArray<UMeshComponent*> meshComponents;
             hitActor->GetComponents<UMeshComponent>(meshComponents);
-            for (int i = 0; i < meshComponents.Num(); i++) {
-                segmentationID = segmentationID == -1 ? meshComponents[i]->CustomDepthStencilValue : segmentationID;
+            for (auto* comp : meshComponents) {
+                segmentationID = comp->CustomDepthStencilValue;
+                if (segmentationID != -1)
+                    break;
             }
         }
 
@@ -155,12 +163,13 @@ bool UnrealLidarSensor::shootLaser(const msr::airlib::Pose& lidar_pose, const ms
         }
 
         // decide the frame for the point-cloud
-        if (params.data_frame == AirSimSettings::kVehicleInertialFrame) {
+        switch (params.data_frame) {
+        case AirSimSettings::LidarSetting::DataFrame::VehicleInertialFrame:
             // current detault behavior; though it is probably not very useful.
             // not changing the default for now to maintain backwards-compat.
             point = ned_transform_->toLocalNed(hit_result.ImpactPoint);
-        }
-        else if (params.data_frame == AirSimSettings::kSensorLocalFrame) {
+            break;
+        case AirSimSettings::LidarSetting::DataFrame::SensorLocalFrame:
             // point in vehicle intertial frame
             Vector3r point_v_i = ned_transform_->toLocalNed(hit_result.ImpactPoint);
 
@@ -178,9 +187,8 @@ bool UnrealLidarSensor::shootLaser(const msr::airlib::Pose& lidar_pose, const ms
 
             // TODO: Optimization -- instead of doing this for every point, it should be possible to do this
             // for the point-cloud together? Need to look into matrix operations to do this together for all points.
+            break;
         }
-        else
-            throw std::runtime_error("Unknown requested data frame");
 
         return true;
     }

Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealLidarSensor.h

@@ -30,8 +30,8 @@ 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, int& segmentationID);
+                    const float horizontal_angle, const float vertical_angle,
+                    const msr::airlib::LidarSimpleParams& params, Vector3r& point, int& segmentationID);
 
 private:
     AActor* actor_;