LidarNoiseModel.cpp

Go to the documentation of this file.

// Copyright(c) 2022 FrostBit Software Lab at the Lapland University of Applied Sciences
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.
 
#include "LidarNoiseModel.h"
#include "Agrarsense/Weather/WeatherParameters.h"
 
#include "Math/UnrealMathUtility.h"
#include <stdlib.h>
 
bool LidarNoiseModel::CheckSnowflakeHit(FHitResult& HitInfo, const FVector EndTrace, const FVector LidarLocation, const FWeatherParameters& WeatherParameters)
{
    return CheckSnowflakeHitWinterSim(HitInfo, WeatherParameters.Precipitation, WeatherParameters.PrecipitationParticleSize, EndTrace, LidarLocation);;
}
 
bool LidarNoiseModel::CheckSnowflakeHitWinterSim(FHitResult& HitInfo, const float PrecipitationAmount, const float ParticleSize, const FVector EndTrace, const FVector LidarLocation)
{
    // Create random value between 0.0 and 1.0
    const float RandomValue = FMath::FRandRange(0.0, 1.0f);
 
    FVector MaxDistance = EndTrace;                                 // lidar max range
    FVector StartPoint = LidarLocation;                             // start point is lidar position
 
    float PrecipitationClass = CalculatePrecipitationClass(PrecipitationAmount, ParticleSize);
 
    if (HitInfo.bBlockingHit)
    {
        MaxDistance = HitInfo.ImpactPoint;
    }
 
    FVector DirectionVector = EndTrace - StartPoint;
    FVector NewStartPoint = StartPoint + 0.01 * DirectionVector;    // make start point away from center of lidar
    FVector NewVector = MaxDistance - NewStartPoint;                // new vector from new start point to end point
    FVector NewHitpoint = NewStartPoint + RandomValue * NewVector;  // generate new point from new start point to end point
    NewHitpoint *= 1e-2;
    float Distance = FVector::Dist(StartPoint, NewHitpoint) / 100;  // distance between NewHitpoint and NewStartPoint divided by 100 to get meters
 
    // Actual probability formula calculation
    const float ProbabilityFormula = PrecipitationClass / 10 * exp(-pow(Distance - (4 + 0.2 * PrecipitationClass), 2.0)
        / pow(2, 2.0)) + 0.2 * PrecipitationClass / 10 * exp(-pow(Distance - (7 + 0.2 * PrecipitationClass), 2.0) / pow(1, 2.0));
 
    // If RandomValue is smaller than ProbabilityFormula from formula we 'hit' the trace to snowflake
    bool HitSnowflake = RandomValue < ProbabilityFormula;
    if (HitSnowflake)
    {
        // Assign new hitpoint
        HitInfo.ImpactPoint = NewHitpoint;
 
        // To identify the snowflake later, set component to nullptr if hitpoint is snowflake
        HitInfo.Component = nullptr;
    }
 
    return HitSnowflake;
}
 
float LidarNoiseModel::CalculatePrecipitationClass(const float PrecipitationAmount, float ParticleSize)
{
    float PrecipitationClass = 0;
 
    // Ensure ParticleSize is within the specified bounds (1.0 to 3.0) and adjust if necessary
    if (ParticleSize < 1.0f) ParticleSize = 1.0f;
    if (ParticleSize > 3.0f) ParticleSize = 3.0f;
 
    if (PrecipitationAmount > 0 && PrecipitationAmount <= 33.33f)           PrecipitationClass = 1 + ParticleSize;
    else if (PrecipitationAmount > 33.33 && PrecipitationAmount <= 66.66f)  PrecipitationClass = 2 + ParticleSize;
    else if (PrecipitationAmount > 66.66 && PrecipitationAmount <= 100.00f) PrecipitationClass = 3 + ParticleSize;
 
    return PrecipitationClass;
}