Skip to content

Commit

Permalink
Fixed GPULiDAR horizontal FOV scanning and resetting for none full 36…
Browse files Browse the repository at this point in the history 
…0 LiDARs. Fixes #33
  • Loading branch information
@WouterJansen
WouterJansen committed Nov 25, 2024
1 parent 8ed7941 commit 87c2be0
Show file tree
Hide file tree
Showing 2 changed files with 39 additions and 18 deletions.
52 changes: 35 additions & 17 deletions Unreal/Plugins/AirSim/Source/LidarCamera.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -272,6 +272,9 @@ void ALidarCamera::InitializeSensor()
capture_2D_segmentation_->HiddenActors = actors;
capture_2D_intensity_->HiddenActors = actors;

sensor_cur_angle_ = FMath::Fmod(horizontal_fov_min_, 360);
hfov_ = abs(horizontal_fov_max_ - horizontal_fov_min_);

initialized = true;
}

Expand All @@ -288,16 +291,24 @@ bool ALidarCamera::Update(float delta_time, msr::airlib::vector<msr::airlib::rea
bool refresh_pointcloud = false;

// Calculate the added rotation of the sensor by this update based on the time that has pased and the rotational speed of the sensor
float sensor_rotation_angle_ = 360 * delta_time * sensor_rotation_frequency_;
float sensor_rotation_angle_ = hfov_ * delta_time * sensor_rotation_frequency_;
sensor_sum_rotation_angle_ += sensor_rotation_angle_;

// If the rotation in this frame is larger than the minimum horizontal FOV delta, a new calculation of points needs to be made
if (sensor_sum_rotation_angle_ > h_delta_angle_) {

// If the full horizontal fov was completed last frame, reset the starting angle again
if (reset_hfov_) {
sensor_cur_angle_ = FMath::Fmod(horizontal_fov_min_, 360);
sensor_prev_rotation_angle_ = 0;
completed_hfov_ = 0;
reset_hfov_ = false;
}

// The ideal virtual camera FOV is set based on the vertical FOV, but if it is a large rotation that has to be calculated, set it to be a even number that is equal to the
// rotation + 2 horizontal FOV deltas. However, to make the pixel to LiDAR laser coordinates work, the FOV needs to be a minimum of 90 degrees.
// rotation + 3 horizontal FOV deltas. However, to make the pixel to LiDAR laser coordinates work, the FOV needs to be a minimum of 90 degrees.
int32 cur_fov = target_fov_;
if (sensor_sum_rotation_angle_ > target_fov_) cur_fov = FMath::CeilToInt(FMath::Min(sensor_sum_rotation_angle_ + (2 * h_delta_angle_), 178.0f));
if (sensor_sum_rotation_angle_ > target_fov_) cur_fov = FMath::CeilToInt(FMath::Min(sensor_sum_rotation_angle_ + (3 * h_delta_angle_), 178.0f));
if (cur_fov % 2 != 0) cur_fov += 1;
if (cur_fov < 90) cur_fov = 90;
capture_2D_depth_->FOVAngle = cur_fov;
Expand All @@ -311,11 +322,13 @@ bool ALidarCamera::Update(float delta_time, msr::airlib::vector<msr::airlib::rea
// If the rotation is bigger than the allowed FOV, cap the rotation (it will be further completed in the next frame)
if (sensor_sum_rotation_angle_ > cur_fov)sensor_sum_rotation_angle_ = cur_fov;

// Perform the camera to LiDAR pointcloud conversion
//if(!first_frame_){
// refresh_pointcloud = SampleRenders(sensor_sum_rotation_angle_, cur_fov, point_cloud, point_cloud_final);
//}
//first_frame_ = false;
// If the rotation will extend beyond the total horizontal FOV if it is not full 360, cap the rotation to the remaining FOV
// And also make sure to reset the start rotation next frame
if (sensor_sum_rotation_angle_ >= hfov_ - completed_hfov_ && hfov_ != 360) {
sensor_sum_rotation_angle_ = hfov_ - completed_hfov_;
reset_hfov_ = true;
}
completed_hfov_ += sensor_sum_rotation_angle_;

if (waited_frames_ < wait_frames_) {
waited_frames_++;
Expand All @@ -327,7 +340,8 @@ bool ALidarCamera::Update(float delta_time, msr::airlib::vector<msr::airlib::rea
capture_2D_intensity_->CaptureScene();
refresh_pointcloud = SampleRenders(sensor_sum_rotation_angle_, cur_fov, point_cloud, point_cloud_final);
}




// Set up the values for the next frame
sensor_prev_rotation_angle_ = sensor_sum_rotation_angle_;
Expand All @@ -351,6 +365,7 @@ void ALidarCamera::GenerateLidarCoordinates() {
v_angles_ = LinearSpacedArray(vertical_fov_min_, vertical_fov_max_, num_lasers_);
for (int32 h_cur_index = 0; h_cur_index < horizontal_samples_; h_cur_index++)
{
h_angles_atan2_.Add(FMath::Fmod(h_angles_[h_cur_index], 360));
float h_angle_0 = h_angles_[h_cur_index];
for (int32 v_cur_index = 0; v_cur_index < num_lasers_; v_cur_index++)
{
Expand Down Expand Up @@ -459,14 +474,16 @@ bool ALidarCamera::SampleRenders(float sensor_rotation_angle, float fov, msr::ai
// Calculate the first and last horizontal angle of the LiDAR that will be captured in this frame
int32 h_first_index = h_cur_atan2_index_ + 1;
float h_max_angle = FMath::Fmod(sensor_cur_angle_ + sensor_rotation_angle, 360);
int32 h_last_index = getIndexLowerClosest(h_angles_, h_max_angle);
int32 h_last_index = getIndexLowerClosest(h_angles_atan2_, h_max_angle);

// variable that keeps the current horizontal angle index that is being calculated
int32 h_cur_index = h_first_index;

// Calculate the last horizontal angle that was measured in the previous frame, used to seeing if the full pointcloud is completed
float h_prev_angle = 10000; // This is done to avoid an issue with the very first measurement
if(h_cur_atan2_index_ != -1) h_prev_angle = FMath::Fmod(h_angles_[h_cur_atan2_index_], 360);
if (h_cur_atan2_index_ != -1) {
h_prev_angle = h_angles_[h_cur_atan2_index_];
}

// get the current rain intensity from the AirSim API
float rain_value;
Expand All @@ -487,7 +504,7 @@ bool ALidarCamera::SampleRenders(float sensor_rotation_angle, float fov, msr::ai

// Get the current horizontal angle, also in Eucledian plane form (between 0 and 360 degrees)
float h_cur_angle = h_angles_[h_cur_atan2_index_];
float h_cur_atan2_angle = FMath::Fmod(h_cur_angle - sensor_cur_angle_, 360) - (fov / 2);
float h_cur_atan2_angle = FMath::Fmod(FMath::Fmod(FMath::Fmod(h_cur_angle, 360) - sensor_cur_angle_, 360) - (fov / 2), 360);

// Calculate the cosine and sine of the horizontal angle and calculate the pixel index from the render texture target that matches this laser's horizontal angle
float h_cur_angle_cos = FMath::Cos(FMath::DegreesToRadians(h_cur_atan2_angle));
Expand All @@ -501,17 +518,18 @@ bool ALidarCamera::SampleRenders(float sensor_rotation_angle, float fov, msr::ai
{
// if the previous horizontal angle was larger than the current one, it means the sensor has done a full circle and a new pointcloud can be started
if (used_by_airsim_) {
if ((h_prev_angle > FMath::Fmod(h_cur_angle, 360)) && (point_cloud.size() != 0)) {
if ((h_prev_angle >h_cur_angle) && (point_cloud.size() != 0)) {
if (v_cur_index == 0) {
// Check edge cases where the amount of points in the pointcloud doesnt equal the desired full pointcloud size. In this case, throw away the current data
if ((((int)point_cloud.size() / 5) != horizontal_samples_ * num_lasers_))
{
UE_LOG(LogTemp, Warning, TEXT("Pointcloud incorrect size! points:%i %f %f"), (int)(point_cloud.size() / 5), h_prev_angle, FMath::Fmod(h_cur_angle, 360));
UE_LOG(LogTemp, Warning, TEXT("Pointcloud incorrect size! points:%i %f %f"), (int)(point_cloud.size() / 5), h_prev_angle, h_cur_angle);
point_cloud.clear();
refresh_pointcloud = false;
}
// Else, save the completed pointcloud into the right array and clear the current one
else {
//UE_LOG(LogTemp, Warning, TEXT("REFRESH at angle: %f, with prev angle: %f"), h_cur_angle, h_prev_angle);
point_cloud_final = point_cloud;
point_cloud.clear();
refresh_pointcloud = true;
Expand Down Expand Up @@ -639,8 +657,8 @@ bool ALidarCamera::SampleRenders(float sensor_rotation_angle, float fov, msr::ai
}
}
else {
if (h_pixel >= resolution_ || h_pixel < 0)UE_LOG(LogTemp, Warning, TEXT("Point H index out of bounds: %i"), h_pixel);
if (v_pixel >= resolution_ || v_pixel < 0)UE_LOG(LogTemp, Warning, TEXT("Point V index out of bounds: %i"), v_pixel);
//if (h_pixel >= resolution_ || h_pixel < 0)UE_LOG(LogTemp, Warning, TEXT("Point H index out of bounds: %i"), h_pixel);
//if (v_pixel >= resolution_ || v_pixel < 0)UE_LOG(LogTemp, Warning, TEXT("Point V index out of bounds: %i"), v_pixel);
if (used_by_airsim_) {
point_cloud.emplace_back(0);
point_cloud.emplace_back(0);
Expand All @@ -652,7 +670,7 @@ bool ALidarCamera::SampleRenders(float sensor_rotation_angle, float fov, msr::ai
}

// Set the variables right for the next horizontal angle
h_prev_angle = FMath::Fmod(h_cur_angle, 360);
h_prev_angle = h_cur_angle;
h_cur_index += 1;
}
return refresh_pointcloud;
Expand Down
5 changes: 4 additions & 1 deletion Unreal/Plugins/AirSim/Source/LidarCamera.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -156,6 +156,7 @@ class AIRSIM_API ALidarCamera : public AActor
UArrowComponent* arrow_;

TArray<float> h_angles_;
TArray<float> h_angles_atan2_;
TArray<float> v_angles_;
float h_delta_angle_ = 0;
float v_delta_angle_ = 0;
Expand All @@ -174,7 +175,9 @@ class AIRSIM_API ALidarCamera : public AActor
bool initialized = false;
int32 wait_frames_ = 100;
int32 waited_frames_ = 0;

float hfov_ = 0;
float completed_hfov_ = 0;
bool reset_hfov_ = false;

//bool saved_DisableWorldRendering_ = false;
//UGameViewportClient* game_viewport_;
Expand Down

0 comments on commit 87c2be0

Please sign in to comment.