p71924506
/
carla
mirror of https://github.com/carla-simulator/carla.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Rewrite of RayCastLidar to inherit from RayCastRawLidar 

to reuse code
This commit is contained in:
Daniel Santos-Olivan 2020-07-27 12:26:51 +02:00 committed by DSantosO
parent 12cf111a45
commit 92892ec8e9
6 changed files with 104 additions and 239 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
LibCarla/source/carla/sensor/data/LidarData.h
View File

@ -77,20 +77,32 @@ namespace data {
LidarData &operator=(LidarData &&) = default;
void SaveDetections() {
const uint32_t SizeLidarDetection = 4;
_points.clear();
_points.reserve(SizeLidarDetection * GetChannelCount() * _max_channel_points);
~LidarData() = default;
for (auto idxChannel = 0u; idxChannel < GetChannelCount(); ++idxChannel) {
_header[Index::SIZE + idxChannel] = static_cast<uint32_t>(_aux_points.size());
for (auto& pt : _aux_points[idxChannel]) {
_points.emplace_back(pt.point.x);
_points.emplace_back(pt.point.y);
_points.emplace_back(pt.point.z);
_points.emplace_back(-1.0f); // FIXME Compute good Intensity here
}
}
virtual void ResetSerPoints(std::vector<uint32_t> points_per_channel) {
DEBUG_ASSERT(GetChannelCount() > points_per_channel.size());
std::memset(_header.data() + Index::SIZE, 0, sizeof(uint32_t) * GetChannelCount());
for (auto idxChannel = 0u; idxChannel < GetChannelCount(); ++idxChannel)
_header[Index::SIZE + idxChannel] = points_per_channel[idxChannel];
uint32_t total_points = static_cast<uint32_t>(
std::accumulate(points_per_channel.begin(), points_per_channel.end(), 0));
_points.clear();
_points.reserve(total_points * 4);
}
void WritePointSync(LidarDetection &detection) {
_points.emplace_back(detection.point.x);
_points.emplace_back(detection.point.y);
_points.emplace_back(detection.point.z);
_points.emplace_back(detection.intensity);
}
virtual void WritePointSync(LidarRawDetection &detection) {
(void) detection;
DEBUG_ASSERT(false);
}
private:

31
LibCarla/source/carla/sensor/data/LidarRawData.h
View File

@ -96,6 +96,8 @@ namespace data {
LidarRawData &operator=(LidarRawData &&) = default;
virtual ~LidarRawData() {}
float GetHorizontalAngle() const {
return reinterpret_cast<const float &>(_header[Index::HorizontalAngle]);
}
@ -108,42 +110,21 @@ namespace data {
return _header[Index::ChannelCount];
}
/// TO BE REMOVED, only kept to avoid breaking LidarData class
std::vector<std::vector<LidarRawDetection>> _aux_points;
void Reset(uint32_t channel_point_count) {
std::memset(_header.data() + Index::SIZE, 0, sizeof(uint32_t) * GetChannelCount());
_max_channel_points = channel_point_count;
_aux_points.resize(GetChannelCount());
for (auto& aux : _aux_points) {
aux.clear();
aux.reserve(channel_point_count);
}
}
void WritePointAsync(uint32_t channel, LidarRawDetection &detection) {
DEBUG_ASSERT(GetChannelCount() > channel);
_aux_points[channel].emplace_back(detection);
}
void ResetSerPoints(std::vector<uint32_t> points_per_channel) {
virtual void ResetSerPoints(std::vector<uint32_t> points_per_channel) {
DEBUG_ASSERT(GetChannelCount() > points_per_channel.size());
std::memset(_header.data() + Index::SIZE, 0, sizeof(uint32_t) * GetChannelCount());
for (auto idxChannel = 0u; idxChannel < GetChannelCount(); ++idxChannel)
_header[Index::SIZE + idxChannel] = points_per_channel[idxChannel];
uint32_t total_points = std::accumulate(points_per_channel.begin(), points_per_channel.end(), 0);
uint32_t total_points = static_cast<uint32_t>(
std::accumulate(points_per_channel.begin(), points_per_channel.end(), 0));
_ser_points.clear();
_ser_points.reserve(total_points);
}
void WritePointSync(LidarRawDetection &detection) {
virtual void WritePointSync(LidarRawDetection &detection) {
_ser_points.emplace_back(detection);
}

181
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/RayCastLidar.cpp
View File

@ -13,6 +13,7 @@
#include <compiler/disable-ue4-macros.h>
#include "carla/geom/Math.h"
#include "carla/geom/Location.h"
#include <compiler/enable-ue4-macros.h>
#include "DrawDebugHelpers.h"
@ -25,17 +26,17 @@ FActorDefinition ARayCastLidar::GetSensorDefinition()
return UActorBlueprintFunctionLibrary::MakeLidarDefinition(TEXT("ray_cast"));
}
ARayCastLidar::ARayCastLidar(const FObjectInitializer& ObjectInitializer)
: Super(ObjectInitializer)
{
PrimaryActorTick.bCanEverTick = true;
ARayCastLidar::ARayCastLidar(const FObjectInitializer& ObjectInitializer)
: Super(ObjectInitializer) {
UE_LOG(LogCarla, Warning, TEXT("ARayCastLidar()!!!! "));
RandomEngine = CreateDefaultSubobject<URandomEngine>(TEXT("RandomEngine"));
}
void ARayCastLidar::Set(const FActorDescription &ActorDescription)
{
Super::Set(ActorDescription);
ASensor::Set(ActorDescription);
FLidarDescription LidarDescription;
UActorBlueprintFunctionLibrary::SetLidar(ActorDescription, LidarDescription);
Set(LidarDescription);
@ -46,22 +47,6 @@ void ARayCastLidar::Set(const FLidarDescription &LidarDescription)
Description = LidarDescription;
LidarData = FLidarData(Description.Channels);
CreateLasers();
}
void ARayCastLidar::CreateLasers()
{
const auto NumberOfLasers = Description.Channels;
check(NumberOfLasers > 0u);
const float DeltaAngle = NumberOfLasers == 1u ? 0.f :
(Description.UpperFovLimit - Description.LowerFovLimit) /
static_cast<float>(NumberOfLasers - 1);
LaserAngles.Empty(NumberOfLasers);
for(auto i = 0u; i < NumberOfLasers; ++i)
{
const float VerticalAngle =
Description.UpperFovLimit - static_cast<float>(i) * DeltaAngle;
LaserAngles.Emplace(VerticalAngle);
}
// Compute drop off model parameters
DropOffBeta = 1.0f - Description.DropOffAtZeroIntensity;
@ -71,72 +56,18 @@ void ARayCastLidar::CreateLasers()
void ARayCastLidar::Tick(const float DeltaTime)
{
Super::Tick(DeltaTime);
ASensor::Tick(DeltaTime);
ReadPoints(DeltaTime);
SimulateLidar(DeltaTime);
auto DataStream = GetDataStream(*this);
DataStream.Send(*this, LidarData, DataStream.PopBufferFromPool());
}
void ARayCastLidar::ReadPoints(const float DeltaTime)
float ARayCastLidar::ComputeIntensity(const FRawDetection& RawDetection) const
{
const uint32 ChannelCount = Description.Channels;
const uint32 PointsToScanWithOneLaser =
FMath::RoundHalfFromZero(
Description.PointsPerSecond * DeltaTime / float(ChannelCount));
if (PointsToScanWithOneLaser <= 0)
{
UE_LOG(
LogCarla,
Warning,
TEXT("%s: no points requested this frame, try increasing the number of points per second."),
*GetName());
return;
}
check(ChannelCount == LaserAngles.Num());
const float CurrentHorizontalAngle = carla::geom::Math::ToDegrees(
LidarData.GetHorizontalAngle());
const float AngleDistanceOfTick = Description.RotationFrequency * 360.0f * DeltaTime;
const float AngleDistanceOfLaserMeasure = AngleDistanceOfTick / PointsToScanWithOneLaser;
LidarData.Reset(PointsToScanWithOneLaser);
GetWorld()->GetPhysicsScene()->GetPxScene()->lockRead();
ParallelFor(ChannelCount, [&](int32 idxChannel) {
FCriticalSection Mutex;
ParallelFor(PointsToScanWithOneLaser, [&](int32 idxPtsOneLaser) {
FVector Point;
float Intensity;
const float Angle = CurrentHorizontalAngle + AngleDistanceOfLaserMeasure * idxPtsOneLaser;
if (ShootLaser(idxChannel, Angle, Point, Intensity)) {
Mutex.Lock();
FLidarRawDetection det {Point, Intensity, 2, 3};
LidarData.WritePointAsync(idxChannel, det);
Mutex.Unlock();
}
});
});
GetWorld()->GetPhysicsScene()->GetPxScene()->unlockRead();
LidarData.SaveDetections();
const float HorizontalAngle = carla::geom::Math::ToRadians(
std::fmod(CurrentHorizontalAngle + AngleDistanceOfTick, 360.0f));
LidarData.SetHorizontalAngle(HorizontalAngle);
}
float ARayCastLidar::ComputeIntensity(const FVector &LidarBodyLoc, const FHitResult& HitInfo) const
{
const FVector HitPoint = HitInfo.ImpactPoint - LidarBodyLoc;
constexpr float TO_METERS = 1e-2;
const float Distance = TO_METERS * HitPoint.Size();
const carla::geom::Location HitPoint = RawDetection.point;
const float Distance = HitPoint.Length();
const float AttenAtm = Description.AtmospAttenRate;
const float AbsAtm = exp(-AttenAtm * Distance);
@ -146,70 +77,30 @@ float ARayCastLidar::ComputeIntensity(const FVector &LidarBodyLoc, const FHitRes
return IntRec;
}
bool ARayCastLidar::ShootLaser(const uint32 Channel, const float HorizontalAngle, FVector &XYZ, float &Intensity) const
{
if(DropOffGenActive && RandomEngine->GetUniformFloat() < Description.DropOffGenRate)
return false;
const float VerticalAngle = LaserAngles[Channel];
FCollisionQueryParams TraceParams = FCollisionQueryParams(FName(TEXT("Laser_Trace")), true, this);
TraceParams.bTraceComplex = true;
TraceParams.bReturnPhysicalMaterial = false;
FHitResult HitInfo(ForceInit);
FTransform actorTransf = GetTransform();
FVector LidarBodyLoc = GetActorLocation();
FRotator LidarBodyRot = GetActorRotation();
FRotator LaserRot (VerticalAngle, HorizontalAngle, 0); // float InPitch, float InYaw, float InRoll
FRotator ResultRot = UKismetMathLibrary::ComposeRotators(
LaserRot,
LidarBodyRot
);
const auto Range = Description.Range;
const FVector ForwardVector = UKismetMathLibrary::GetForwardVector(ResultRot);
FVector EndTrace = Range * ForwardVector + LidarBodyLoc;
GetWorld()->LineTraceSingleByChannel(
HitInfo,
LidarBodyLoc,
EndTrace,
ECC_GameTraceChannel2,
TraceParams,
FCollisionResponseParams::DefaultResponseParam
);
if (HitInfo.bBlockingHit)
{
if (Description.ShowDebugPoints)
{
DrawDebugPoint(
GetWorld(),
HitInfo.ImpactPoint,
10, //size
FColor(255,0,255),
false, //persistent (never goes away)
0.1 //point leaves a trail on moving object
);
}
FVector hp = HitInfo.ImpactPoint;
if (Description.NoiseStdDev > 0.0f)
{
const FVector Noise = ForwardVector * RandomEngine->GetNormalDistribution(0.0f, Description.NoiseStdDev);
hp += Noise;
}
XYZ = actorTransf.Inverse().TransformPosition(hp);
Intensity = ComputeIntensity(LidarBodyLoc, HitInfo);
if(Intensity > Description.DropOffIntensityLimit)
return true;
bool ARayCastLidar::PreprocessRay(const float& VerticalAngle, float &HorizontalAngle) const {
if(DropOffGenActive && RandomEngine->GetUniformFloat() < Description.DropOffGenRate)
return false;
else
return RandomEngine->GetUniformFloat() < DropOffAlpha * Intensity + DropOffBeta;
} else {
return false;
return true;
}
void ARayCastLidar::ComputeAndSaveDetections(const FTransform& SensorTransform) {
std::vector<u_int32_t> PointsPerChannel(Description.Channels);
UE_LOG(LogCarla, Warning, TEXT("ARayCastLidar::ComputeAndSaveDetections()!!!! "));
for (auto idxChannel = 0u; idxChannel < Description.Channels; ++idxChannel)
PointsPerChannel[idxChannel] = RecordedHits[idxChannel].size();
LidarData.ResetSerPoints(PointsPerChannel);
for (auto idxChannel = 0u; idxChannel < Description.Channels; ++idxChannel) {
for (auto& hit : RecordedHits[idxChannel]) {
FRawDetection raw_detection;
ComputeRawDetection(hit, SensorTransform, raw_detection);
float intensity = ComputeIntensity(raw_detection);
FDetection detection(raw_detection.point, intensity);
LidarData.WritePointSync(detection);
}
}
}
}

39
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/RayCastLidar.h
View File

@ -6,10 +6,12 @@
#pragma once
#include "Carla/Sensor/Sensor.h"
#include "Carla/Actor/ActorDefinition.h"
#include "Carla/Sensor/LidarDescription.h"
#include "Carla/Sensor/Sensor.h"
#include "Carla/Sensor/RayCastRawLidar.h"
#include "Carla/Actor/ActorBlueprintFunctionLibrary.h"
#include <compiler/disable-ue4-macros.h>
#include <carla/sensor/data/LidarData.h>
@ -19,46 +21,29 @@
/// A ray-cast based Lidar sensor.
UCLASS()
class CARLA_API ARayCastLidar : public ASensor
class CARLA_API ARayCastLidar : public ARayCastRawLidar
{
GENERATED_BODY()
using FLidarData = carla::sensor::data::LidarData;
using FLidarRawDetection = carla::sensor::data::LidarRawDetection;
using FLidarDetection = carla::sensor::data::LidarDetection;
using FDetection = carla::sensor::data::LidarDetection;
public:
static FActorDefinition GetSensorDefinition();
ARayCastLidar(const FObjectInitializer &ObjectInitializer);
void Set(const FActorDescription &Description) override;
void Set(const FLidarDescription &LidarDescription);
protected:
virtual void Tick(float DeltaTime) override;
virtual void Set(const FActorDescription &Description) override;
virtual void Set(const FLidarDescription &LidarDescription) override;
private:
/// Creates a Laser for each channel.
void CreateLasers();
/// Updates LidarMeasurement with the points read in DeltaTime.
void ReadPoints(float DeltaTime);
/// Shoot a laser ray-trace, return whether the laser hit something.
bool ShootLaser(uint32 Channel, float HorizontalAngle, FVector &Point, float& Intensity) const;
/// Compute the received intensity of the point
float ComputeIntensity(const FVector &LidarBodyLoc, const FHitResult& HitInfo) const;
float ComputeIntensity(const FRawDetection& RawDetection) const;
UPROPERTY(EditAnywhere)
FLidarDescription Description;
bool PreprocessRay(const float& VerticalAngle, float &HorizontalAngle) const override;
TArray<float> LaserAngles;
void ComputeAndSaveDetections(const FTransform& SensorTransform) override;
virtual void Tick(float DeltaTime) override;
FLidarData LidarData;

28
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/RayCastRawLidar.cpp
View File

@ -29,8 +29,6 @@ ARayCastRawLidar::ARayCastRawLidar(const FObjectInitializer& ObjectInitializer)
: Super(ObjectInitializer)
{
PrimaryActorTick.bCanEverTick = true;
RandomEngine = CreateDefaultSubobject<URandomEngine>(TEXT("RandomEngine"));
}
void ARayCastRawLidar::Set(const FActorDescription &ActorDescription)
@ -44,7 +42,7 @@ void ARayCastRawLidar::Set(const FActorDescription &ActorDescription)
void ARayCastRawLidar::Set(const FLidarDescription &LidarDescription)
{
Description = LidarDescription;
LidarData = FLidarData(Description.Channels);
LidarRawData = FLidarRawData(Description.Channels);
CreateLasers();
}
@ -71,7 +69,7 @@ void ARayCastRawLidar::Tick(const float DeltaTime)
SimulateLidar(DeltaTime);
auto DataStream = GetDataStream(*this);
DataStream.Send(*this, LidarData, DataStream.PopBufferFromPool());
DataStream.Send(*this, LidarRawData, DataStream.PopBufferFromPool());
}
void ARayCastRawLidar::SimulateLidar(const float DeltaTime)
@ -94,12 +92,10 @@ void ARayCastRawLidar::SimulateLidar(const float DeltaTime)
check(ChannelCount == LaserAngles.Num());
const float CurrentHorizontalAngle = carla::geom::Math::ToDegrees(
LidarData.GetHorizontalAngle());
LidarRawData.GetHorizontalAngle());
const float AngleDistanceOfTick = Description.RotationFrequency * 360.0f * DeltaTime;
const float AngleDistanceOfLaserMeasure = AngleDistanceOfTick / PointsToScanWithOneLaser;
LidarData.Reset(PointsToScanWithOneLaser);
ResetRecordedHits(ChannelCount, PointsToScanWithOneLaser);
GetWorld()->GetPhysicsScene()->GetPxScene()->lockRead();
@ -127,7 +123,7 @@ void ARayCastRawLidar::SimulateLidar(const float DeltaTime)
const float HorizontalAngle = carla::geom::Math::ToRadians(
std::fmod(CurrentHorizontalAngle + AngleDistanceOfTick, 360.0f));
LidarData.SetHorizontalAngle(HorizontalAngle);
LidarRawData.SetHorizontalAngle(HorizontalAngle);
}
void ARayCastRawLidar::ResetRecordedHits(uint32_t Channels, uint32_t MaxPointsPerChannel) {
@ -146,21 +142,23 @@ void ARayCastRawLidar::SimulateLidar(const float DeltaTime)
void ARayCastRawLidar::ComputeAndSaveDetections(const FTransform& SensorTransform) {
std::vector<u_int32_t> PointsPerChannel(Description.Channels);
UE_LOG(LogCarla, Warning, TEXT("ARayCastRawLidar::ComputeAndSaveDetections()!!!! "));
for (auto idxChannel = 0u; idxChannel < Description.Channels; ++idxChannel)
PointsPerChannel[idxChannel] = RecordedHits[idxChannel].size();
LidarData.ResetSerPoints(PointsPerChannel);
LidarRawData.ResetSerPoints(PointsPerChannel);
for (auto idxChannel = 0u; idxChannel < Description.Channels; ++idxChannel) {
for (auto& hit : RecordedHits[idxChannel]) {
FDetection detection;
FRawDetection detection;
ComputeRawDetection(hit, SensorTransform, detection);
LidarData.WritePointSync(detection);
LidarRawData.WritePointSync(detection);
}
}
}
void ARayCastRawLidar::ComputeRawDetection(const FHitResult& HitInfo, const FTransform& SensorTransf, FDetection& Detection) const
void ARayCastRawLidar::ComputeRawDetection(const FHitResult& HitInfo, const FTransform& SensorTransf, FRawDetection& Detection) const
{
const FVector HitPoint = HitInfo.ImpactPoint;
Detection.point = SensorTransf.Inverse().TransformPosition(HitPoint);
@ -188,16 +186,16 @@ void ARayCastRawLidar::ComputeRawDetection(const FHitResult& HitInfo, const FTra
Detection.object_tag = static_cast<uint32_t>(*labels.CreateConstIterator());
}
else {
UE_LOG(LogCarla, Warning, TEXT("Info not valid!!!!"));
//UE_LOG(LogCarla, Warning, TEXT("Info not valid!!!!"));
}
}
else {
UE_LOG(LogCarla, Warning, TEXT("View is not valid %p!!!!"), view.GetActor());
//UE_LOG(LogCarla, Warning, TEXT("View is not valid %p!!!!"), view.GetActor());
}
}
else {
UE_LOG(LogCarla, Warning, TEXT("Actor not valid %p!!!!"), actor);
//UE_LOG(LogCarla, Warning, TEXT("Actor not valid %p!!!!"), actor);
}
}

26
Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/RayCastRawLidar.h
View File

@ -10,6 +10,7 @@
#include "Carla/Actor/ActorDefinition.h"
#include "Carla/Sensor/LidarDescription.h"
#include "Carla/Actor/ActorBlueprintFunctionLibrary.h"
#include <compiler/disable-ue4-macros.h>
#include <carla/sensor/data/LidarRawData.h>
@ -23,25 +24,22 @@ class CARLA_API ARayCastRawLidar : public ASensor
{
GENERATED_BODY()
using FLidarData = carla::sensor::data::LidarRawData;
using FLidarRawData = carla::sensor::data::LidarRawData;
protected:
using FDetection = carla::sensor::data::LidarRawDetection;
using FLidarRawData = carla::sensor::data::LidarRawData;
using FRawDetection = carla::sensor::data::LidarRawDetection;
public:
static FActorDefinition GetSensorDefinition();
ARayCastRawLidar(const FObjectInitializer &ObjectInitializer);
void Set(const FActorDescription &Description) override;
void Set(const FLidarDescription &LidarDescription);
virtual void Set(const FActorDescription &Description) override;
virtual void Set(const FLidarDescription &LidarDescription);
protected:
virtual void Tick(float DeltaTime) override;
private:
/// Creates a Laser for each channel.
void CreateLasers();
@ -52,14 +50,14 @@ private:
bool ShootLaser(const float VerticalAngle, float HorizontalAngle, FHitResult &RawData) const;
/// Method that allow to preprocess the ray before shoot it
bool PreprocessRay(const float& VerticalAngle, float &HorizontalAngle) const {
virtual bool PreprocessRay(const float& VerticalAngle, float &HorizontalAngle) const {
// This method allows to introduce noise or drop points if needed
// A true return value will make the proposed ray to be actually computed.
return true;
}
/// Compute all raw detection information
void ComputeRawDetection(const FHitResult &HitInfo, const FTransform &SensorTransf, FDetection &Detection) const;
void ComputeRawDetection(const FHitResult &HitInfo, const FTransform &SensorTransf, FRawDetection &Detection) const;
/// Saving the hits the raycast returns per channel
void WritePointAsync(uint32_t Channel, FHitResult &Detection);
@ -68,16 +66,16 @@ private:
void ResetRecordedHits(uint32_t Channels, uint32_t MaxPointsPerChannel);
/// Clear the recorded data structure
void ComputeAndSaveDetections(const FTransform &SensorTransform);
virtual void ComputeAndSaveDetections(const FTransform &SensorTransform);
UPROPERTY(EditAnywhere)
FLidarDescription Description;
TArray<float> LaserAngles;
FLidarData LidarData;
std::vector<std::vector<FHitResult>> RecordedHits;
private:
FLidarRawData LidarRawData;
};