Fix #24, codify road map pixel info as uint16

This commit is contained in:
nsubiron 2017-05-08 17:45:59 +02:00
parent 0fbd3446cd
commit 33590041a6
3 changed files with 273 additions and 152 deletions

View File

@ -237,7 +237,9 @@ void ACityMapGenerator::GenerateRoadMap()
const FTransform &ActorTransform = GetActorTransform();
RoadMap->RoadMap.Empty();
const FVector MapOffset(-Offset, -Offset, 0.0f);
RoadMap->Reset(SizeX, SizeY, 1.0f / CmPerPixel, ActorTransform.Inverse(), MapOffset);
for (uint32 PixelY = 0u; PixelY < SizeY; ++PixelY) {
for (uint32 PixelX = 0u; PixelX < SizeX; ++PixelX) {
const float X = static_cast<float>(PixelX) * CmPerPixel - Offset;
@ -245,8 +247,6 @@ void ACityMapGenerator::GenerateRoadMap()
const FVector Start = ActorTransform.TransformPosition(FVector(X, Y, 50.0f));
const FVector End = ActorTransform.TransformPosition(FVector(X, Y, -50.0f));
bool Success = false;
// Do the ray tracing.
FHitResult Hit;
if (LineTrace(World, Start, End, Hit)) {
@ -258,39 +258,21 @@ void ACityMapGenerator::GenerateRoadMap()
if (!InstancedStaticMeshComponent->GetInstanceTransform(Hit.Item, InstanceTransform, true)) {
UE_LOG(LogCarla, Error, TEXT("Failed to get instance's transform"));
} else {
RoadMap->AppendPixel(
RoadMap->SetPixelAt(
PixelX,
PixelY,
GetTag(*InstancedStaticMeshComponent->GetStaticMesh()),
InstanceTransform,
bLeftHandTraffic);
Success = true;
}
}
}
if (!Success) {
RoadMap->AppendEmptyPixel();
}
}
}
const FVector MapOffset(-Offset, -Offset, 0.0f);
RoadMap->Set(SizeX, SizeY, 1.0f / CmPerPixel, ActorTransform.Inverse(), MapOffset);
const float MapSizeInMB = // Only map data, not the class itself.
static_cast<float>(sizeof(FRoadMapPixelData) * RoadMap->RoadMap.Num()) /
(1024.0f * 1024.0f);
UE_LOG(
LogCarla,
Log,
TEXT("Generated road map %dx%d (%.2fMB) with %.2f cm/pixel"),
RoadMap->GetWidth(),
RoadMap->GetHeight(),
MapSizeInMB,
CmPerPixel);
if (!RoadMap->IsValid()) {
UE_LOG(LogCarla, Error, TEXT("Error generating road map"));
return;
}
#ifdef WITH_EDITOR
RoadMap->Log();
#endif // WITH_EDITOR
if (bSaveRoadMapToDisk) {
const FString MapName = World->GetMapName() + TEXT(".png");
@ -298,5 +280,7 @@ void ACityMapGenerator::GenerateRoadMap()
RoadMap->SaveAsPNG(FilePath);
}
#ifdef WITH_EDITOR
RoadMap->DrawDebugPixelsToLevel(GetWorld(), !bDrawDebugPixelsToLevel);
#endif // WITH_EDITOR
}

View File

@ -6,19 +6,142 @@
#include "DrawDebugHelpers.h"
#include "HighResScreenshot.h"
#include <type_traits>
/// ============================================================================
/// -- Static local methods ----------------------------------------------------
/// ============================================================================
static uint32 ClampFloatToUInt(const float Value, int32 Min, int32 Max)
{
return FMath::Clamp(FMath::FloorToInt(Value), Min, Max);
}
// Creates a valid empty map (every point is off-road).
// Return the azimuth angle (in spherical coordinates) rotated by PI so it lies
// in the range [0, 2*PI].
static float GetRotatedAzimuthAngle(const FVector &Direction)
{
const FVector2D SphericalCoords = Direction.UnitCartesianToSpherical();
return SphericalCoords.Y + PI;
}
/// ============================================================================
/// -- FRoadMapPixelData -------------------------------------------------------
/// ============================================================================
uint16 FRoadMapPixelData::Encode(bool IsRoad, bool HasDirection, const FVector &Direction)
{
const uint16 AngleAsUInt = MaximumEncodedAngle * GetRotatedAzimuthAngle(Direction) / (2.0f * PI);
check(!(AngleAsUInt & (1 << IsRoadRow)));
check(!(AngleAsUInt & (1 << HasDirectionRow)));
return (IsRoad << IsRoadRow) | (HasDirection << HasDirectionRow) | (AngleAsUInt);
}
FColor FRoadMapPixelData::EncodeAsColor() const
{
if (!IsRoad()) {
return FColor(0u, 0u, 0u, 255u);
} else if (!HasDirection()) {
return FColor(255u, 255u, 255u, 255u);
} else {
auto ToColor = [](float X){
return FMath::FloorToInt(256.0 * (X + PI) / (2.0f * PI)) % 256;
};
const float Azimuth = GetDirectionAzimuthalAngle();
return FColor(0u, 255u, ToColor(Azimuth), 255u);
}
}
/// ============================================================================
/// -- URoadMap ----------------------------------------------------------------
/// ============================================================================
URoadMap::URoadMap(const FObjectInitializer& ObjectInitializer) :
Super(ObjectInitializer),
PixelsPerCentimeter(1.0f),
Width(1u),
Height(1u)
{
AppendEmptyPixel();
RoadMapData.Add(0u);
static_assert(
std::is_same<decltype(FRoadMapPixelData::Value), typename decltype(RoadMapData)::ElementType>::value,
"Declaration map of FRoadMapPixelData's value does not match current serialization type");
}
void URoadMap::Reset(
const uint32 inWidth,
const uint32 inHeight,
const float inPixelsPerCentimeter,
const FTransform &inWorldToMap,
const FVector &inMapOffset)
{
RoadMapData.Init(0u, inWidth * inHeight);
Width = inWidth;
Height = inHeight;
PixelsPerCentimeter = inPixelsPerCentimeter;
WorldToMap = inWorldToMap;
MapOffset = inMapOffset;
}
void URoadMap::SetPixelAt(
const uint32 PixelX,
const uint32 PixelY,
const ECityMapMeshTag Tag,
const FTransform &Transform,
const bool bInvertDirection)
{
bool bIsRoad = false;
bool bHasDirection = false;
FVector Direction(0.0f, 0.0f, 0.0f);
auto Rotator = Transform.GetRotation().Rotator();
switch (Tag) {
default:
// Is not road.
break;
case ECityMapMeshTag::RoadTwoLanes_LaneRight:
case ECityMapMeshTag::Road90DegTurn_Lane0:
bIsRoad = true;
bHasDirection = true;
break;
case ECityMapMeshTag::RoadTwoLanes_LaneLeft:
case ECityMapMeshTag::Road90DegTurn_Lane1:
bIsRoad = true;
bHasDirection = true;
Rotator.Yaw += 180.0f;
break;
case ECityMapMeshTag::Road90DegTurn_Lane2:
bIsRoad = true;
bHasDirection = true;
Rotator.Yaw += 90.0f;
break;
case ECityMapMeshTag::Road90DegTurn_Lane3:
bIsRoad = true;
bHasDirection = true;
Rotator.Yaw += 270.0f;
break;
case ECityMapMeshTag::RoadTIntersection_Lane0:
case ECityMapMeshTag::RoadTIntersection_Lane1:
case ECityMapMeshTag::RoadTIntersection_Lane2:
case ECityMapMeshTag::RoadTIntersection_Lane3:
case ECityMapMeshTag::RoadXIntersection_Lane0:
case ECityMapMeshTag::RoadXIntersection_Lane1:
case ECityMapMeshTag::RoadXIntersection_Lane2:
case ECityMapMeshTag::RoadXIntersection_Lane3:
bIsRoad = true;
bHasDirection = false;
break;
}
if (bHasDirection) {
FQuat Rotation(Rotator);
Direction = Rotation.GetForwardVector();
if (bInvertDirection) {
Direction *= -1.0f;
}
}
const auto Value = FRoadMapPixelData::Encode(bIsRoad, bHasDirection, Direction);
RoadMapData[GetIndex(PixelX, PixelY)] = Value;
}
FVector URoadMap::GetWorldLocation(uint32 PixelX, uint32 PixelY) const
@ -30,7 +153,7 @@ FVector URoadMap::GetWorldLocation(uint32 PixelX, uint32 PixelY) const
return WorldToMap.InverseTransformPosition(RelativePosition + MapOffset);
}
const FRoadMapPixelData &URoadMap::GetDataAt(const FVector &WorldLocation) const
FRoadMapPixelData URoadMap::GetDataAt(const FVector &WorldLocation) const
{
check(IsValid());
const FVector Location = WorldToMap.TransformPosition(WorldLocation) - MapOffset;
@ -44,7 +167,8 @@ FRoadMapIntersectionResult URoadMap::Intersect(
const FVector &BoxExtent,
float ChecksPerCentimeter) const
{
const auto &DirectionOfMovement = BoxTransform.GetRotation().GetForwardVector();
auto DirectionOfMovement = BoxTransform.GetRotation().GetForwardVector();
DirectionOfMovement.Z = 0.0f; // Project to XY plane (won't be normalized anymore).
uint32 CheckCount = 0u;
FRoadMapIntersectionResult Result = {0.0f, 0.0f};
const float Step = 1.0f / ChecksPerCentimeter;
@ -52,11 +176,11 @@ FRoadMapIntersectionResult URoadMap::Intersect(
for (float Y = -BoxExtent.Y; Y < BoxExtent.Y; Y += Step) {
++CheckCount;
auto Location = BoxTransform.TransformPosition(FVector(X, Y, 0.0f));
auto &Data = GetDataAt(Location);
if (Data.bIsOffRoad) {
const auto &Data = GetDataAt(Location);
if (!Data.IsRoad()) {
Result.OffRoad += 1.0f;
} else if (Data.bHasDirection &&
0.0f < FVector::DotProduct(Data.Direction, DirectionOfMovement)) {
} else if (Data.HasDirection() &&
0.0f < FVector::DotProduct(Data.GetDirection(), DirectionOfMovement)) {
Result.OppositeLane += 1.0f;
}
}
@ -70,21 +194,6 @@ FRoadMapIntersectionResult URoadMap::Intersect(
return Result;
}
static FColor Encode(const FRoadMapPixelData &Data)
{
if (Data.bIsOffRoad) {
return FColor(0u, 0u, 0u, 255u);
} else if (!Data.bHasDirection) {
return FColor(255u, 255u, 255u, 255u);
} else {
// Assumes normalized direction.
auto ToColor = [](float X){
return FMath::FloorToInt(255.0 * (X + 1.0f) / 2.0f);
};
return FColor(ToColor(Data.Direction.X), ToColor(Data.Direction.Y), ToColor(Data.Direction.Z));
}
}
bool URoadMap::SaveAsPNG(const FString &Path) const
{
if (!IsValid()) {
@ -93,18 +202,42 @@ bool URoadMap::SaveAsPNG(const FString &Path) const
}
TArray<FColor> BitMap;
for (auto &Data : RoadMap) {
BitMap.Emplace(Encode(Data));
for (auto Value : RoadMapData) {
BitMap.Emplace(FRoadMapPixelData(Value).EncodeAsColor());
}
FIntPoint DestSize(Width, Height);
FString ResultPath;
FHighResScreenshotConfig& HighResScreenshotConfig = GetHighResScreenshotConfig();
FHighResScreenshotConfig &HighResScreenshotConfig = GetHighResScreenshotConfig();
HighResScreenshotConfig.SetHDRCapture(false);
HighResScreenshotConfig.SaveImage(Path, BitMap, DestSize, &ResultPath);
UE_LOG(LogCarla, Log, TEXT("Saved road map to \"%s\""), *ResultPath);
return true;
}
#ifdef WITH_EDITOR
void URoadMap::Log() const
{
const float MapSizeInMB = // Only map data, not the class itself.
static_cast<float>(sizeof(decltype(RoadMapData)::ElementType) * RoadMapData.Num()) /
(1024.0f * 1024.0f);
UE_LOG(
LogCarla,
Log,
TEXT("Generated road map %dx%d (%.2fMB) with %.2f cm/pixel"),
GetWidth(),
GetHeight(),
MapSizeInMB,
1.0f / PixelsPerCentimeter);
if (!IsValid()) {
UE_LOG(LogCarla, Error, TEXT("Error generating road map"));
return;
}
}
void URoadMap::DrawDebugPixelsToLevel(UWorld *World, const bool bJustFlushDoNotDraw) const
{
FlushPersistentDebugLines(World);
@ -112,61 +245,11 @@ void URoadMap::DrawDebugPixelsToLevel(UWorld *World, const bool bJustFlushDoNotD
for (auto X = 0u; X < Width; ++X) {
for (auto Y = 0u; Y < Height; ++Y) {
auto Location = GetWorldLocation(X, Y);
auto Color = Encode(GetDataAt(X, Y));
auto Color = GetDataAt(X, Y).EncodeAsColor();
DrawDebugPoint(World, Location, 20.0f, Color, true);
}
}
}
}
void URoadMap::AppendPixel(
ECityMapMeshTag Tag,
const FTransform &Transform,
const bool bInvertDirection)
{
AppendEmptyPixel();
auto &Data = RoadMap.Last();
Data.bIsOffRoad = false;
auto Rotator = Transform.GetRotation().Rotator();
switch (Tag) {
case ECityMapMeshTag::RoadTwoLanes_LaneRight:
case ECityMapMeshTag::Road90DegTurn_Lane0:
Data.bHasDirection = true;
break;
case ECityMapMeshTag::RoadTwoLanes_LaneLeft:
case ECityMapMeshTag::Road90DegTurn_Lane1:
Rotator.Yaw += 180.0f;
Data.bHasDirection = true;
break;
case ECityMapMeshTag::Road90DegTurn_Lane2:
Rotator.Yaw += 90.0f;
Data.bHasDirection = true;
break;
case ECityMapMeshTag::Road90DegTurn_Lane3:
Rotator.Yaw += 270.0f;
Data.bHasDirection = true;
break;
}
if (Data.bHasDirection) {
FQuat Rotation(Rotator);
Data.Direction = Rotation.GetForwardVector();
if (bInvertDirection) {
Data.Direction *= -1.0f;
}
}
}
void URoadMap::Set(
const uint32 inWidth,
const uint32 inHeight,
const float inPinxelsPerCentimeter,
const FTransform &inWorldToMap,
const FVector &inMapOffset)
{
Width = inWidth;
Height = inHeight;
PixelsPerCentimeter = inPinxelsPerCentimeter;
WorldToMap = inWorldToMap;
MapOffset = inMapOffset;
}
#endif // WITH_EDITOR

View File

@ -5,33 +5,78 @@
#include "UObject/NoExportTypes.h"
#include "RoadMap.generated.h"
/// Road map intersection result. See URoadMap.
USTRUCT()
struct FRoadMapIntersectionResult
{
GENERATED_BODY()
/** Percentage of the box lying off-road */
/// Percentage of the box lying off-road.
UPROPERTY(VisibleAnywhere, BlueprintReadOnly)
float OffRoad;
/** Percentage of the box invading opposite lane (wrong direction) */
/// Percentage of the box invading opposite lane (wrong direction).
UPROPERTY(VisibleAnywhere, BlueprintReadOnly)
float OppositeLane;
};
USTRUCT()
/// Data stored in a road map pixel. See URoadMap.
struct FRoadMapPixelData
{
GENERATED_BODY()
friend class URoadMap;
UPROPERTY(VisibleAnywhere, BlueprintReadOnly)
bool bIsOffRoad = true;
constexpr static int IsRoadRow = 15;
UPROPERTY(VisibleAnywhere, BlueprintReadOnly)
bool bHasDirection = false;
constexpr static int HasDirectionRow = 14;
UPROPERTY(VisibleAnywhere, BlueprintReadOnly)
FVector Direction;
constexpr static uint16 MaximumEncodedAngle = (1 << 14) - 1;
constexpr static uint16 AngleMask = (0xFFFF >> 2);
public:
explicit FRoadMapPixelData(uint16 inValue) : Value(inValue) {}
/// Whether this pixel lies in-road.
bool IsRoad() const
{
return (Value & (1 << IsRoadRow));
}
/// Whether this pixel has a direction defined (e.g. road intersections are
/// not off-road but neither have defined direction).
bool HasDirection() const
{
return (Value & (1 << HasDirectionRow));
}
/// Get the azimuth angle [-PI, PI] of the road direction (in spherical
/// coordinates) at this pixel.
///
/// Undefined if !HasDirection().
float GetDirectionAzimuthalAngle() const
{
const float Angle = AngleMask & Value;
// Internally the angle is rotated by PI.
return (Angle * 2.0f * PI / MaximumEncodedAngle) - PI;
}
/// Get the road direction at this pixel.
///
/// Undefined if !HasDirection().
FVector GetDirection() const
{
const FVector2D SphericalCoords(HALF_PI, GetDirectionAzimuthalAngle());
return SphericalCoords.SphericalToUnitCartesian();
}
FColor EncodeAsColor() const;
private:
static uint16 Encode(bool IsRoad, bool HasDirection, const FVector &Direction);
uint16 Value;
};
/// Road map of the level. Contains information in 2D of which areas are road
@ -43,13 +88,23 @@ class CARLA_API URoadMap : public UObject
public:
/// Creates a valid empty map (every point is off-road).
URoadMap(const FObjectInitializer& ObjectInitializer);
UFUNCTION(BlueprintCallable)
bool IsValid() const
{
return ((RoadMap.Num() > 0) && (RoadMap.Num() == Height * Width));
}
/// Resets current map an initializes an empty map of the given size.
void Reset(
uint32 Width,
uint32 Height,
float PixelsPerCentimeter,
const FTransform &WorldToMap,
const FVector &MapOffset);
void SetPixelAt(
uint32 PixelX,
uint32 PixelY,
ECityMapMeshTag Tag,
const FTransform &Transform,
bool bInvertDirection = false);
uint32 GetWidth() const
{
@ -61,74 +116,73 @@ public:
return Height;
}
/// Return the world location of a given pixel.
FVector GetWorldLocation(uint32 PixelX, uint32 PixelY) const;
const FRoadMapPixelData &GetDataAt(uint32 PixelX, uint32 PixelY) const
/// Retrieve the data stored at a given pixel.
FRoadMapPixelData GetDataAt(uint32 PixelX, uint32 PixelY) const
{
check(IsValid());
return RoadMap[PixelX + Width * PixelY];
return FRoadMapPixelData(RoadMapData[GetIndex(PixelX, PixelY)]);
}
/** Clamps value if lies outside map limits */
UFUNCTION(BlueprintCallable)
const FRoadMapPixelData &GetDataAt(const FVector &WorldLocation) const;
/// Clamps value if lies outside map limits.
FRoadMapPixelData GetDataAt(const FVector &WorldLocation) const;
/** Intersect actor bounds with map.
*
* Bounds box is projected to the map and checked against it for possible
* intersections with off-road areas and opposite lanes.
*/
UFUNCTION(BlueprintCallable)
/// Intersect actor bounds with map.
///
/// Bounds box is projected to the map and checked against it for possible
/// intersections with off-road areas and opposite lanes.
FRoadMapIntersectionResult Intersect(
const FTransform &BoxTransform,
const FVector &BoxExtent,
float ChecksPerCentimeter) const;
UFUNCTION(BlueprintCallable)
/// Save the current map as PNG with the pixel data encoded as color.
bool SaveAsPNG(const FString &Path) const;
/** Draw every pixel of the image as debug point */
UFUNCTION(BlueprintCallable)
#ifdef WITH_EDITOR
/// Log status of the map to the console.
void Log() const;
/// Draw every pixel of the image as debug point.
void DrawDebugPixelsToLevel(UWorld *World, bool bJustFlushDoNotDraw = false) const;
#endif // WITH_EDITOR
private:
friend class ACityMapGenerator;
void AppendEmptyPixel()
int32 GetIndex(uint32 PixelX, uint32 PixelY) const
{
RoadMap.AddDefaulted(1);
return PixelX + Width * PixelY;
}
void AppendPixel(
ECityMapMeshTag Tag,
const FTransform &Transform,
bool bInvertDirection);
void Set(
uint32 Width,
uint32 Height,
float PixelsPerCentimeter,
const FTransform &WorldToMap,
const FVector &MapOffset);
bool IsValid() const
{
return ((RoadMapData.Num() > 0) && (RoadMapData.Num() == Height * Width));
}
/// World-to-map transform.
UPROPERTY(VisibleAnywhere)
FTransform WorldToMap;
/// Offset of the map in map coordinates.
UPROPERTY(VisibleAnywhere)
FVector MapOffset;
/// Number of pixels per centimeter.
UPROPERTY(VisibleAnywhere)
float PixelsPerCentimeter;
/** Width of the map in pixels */
/// Width of the map in pixels.
UPROPERTY(VisibleAnywhere)
uint32 Width;
/** Height of the map in pixels */
/// Height of the map in pixels.
UPROPERTY(VisibleAnywhere)
uint32 Height;
UPROPERTY()
TArray<FRoadMapPixelData> RoadMap;
TArray<uint16> RoadMapData;
};