CARLA
RoadMap.cpp
Go to the documentation of this file.
1 // Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
2 // de Barcelona (UAB).
3 //
4 // This work is licensed under the terms of the MIT license.
5 // For a copy, see <https://opensource.org/licenses/MIT>.
6 
7 #include "Carla.h"
8 #include "Carla/MapGen/RoadMap.h"
9 
11 
12 #include "FileHelper.h"
13 #include "HighResScreenshot.h"
14 
15 #if WITH_EDITOR
16 #include "DrawDebugHelpers.h"
17 #endif // WITH_EDITOR
18 
19 #include <type_traits>
20 
21 #define LOCTEXT_NAMESPACE "CarlaRoadMap"
22 
23 // =============================================================================
24 // -- Static local methods -----------------------------------------------------
25 // =============================================================================
26 
27 static uint32 ClampFloatToUInt(const float Value, int32 Min, int32 Max)
28 {
29  return FMath::Clamp(FMath::FloorToInt(Value), Min, Max);
30 }
31 
32 // Return the azimuth angle (in spherical coordinates) rotated by PI so it lies
33 // in the range [0, 2*PI].
34 static float GetRotatedAzimuthAngle(const FVector &Direction)
35 {
36  const FVector2D SphericalCoords = Direction.UnitCartesianToSpherical();
37  return SphericalCoords.Y + PI;
38 }
39 
40 // =============================================================================
41 // -- FRoadMapPixelData --------------------------------------------------------
42 // =============================================================================
43 
44 uint16 FRoadMapPixelData::Encode(bool IsRoad, bool HasDirection, const FVector &Direction)
45 {
46  const uint16 AngleAsUInt = MaximumEncodedAngle * GetRotatedAzimuthAngle(Direction) / (2.0f * PI);
47  check(!(AngleAsUInt & (1 << IsRoadRow)));
48  check(!(AngleAsUInt & (1 << HasDirectionRow)));
49  return (IsRoad << IsRoadRow) | (HasDirection << HasDirectionRow) | (AngleAsUInt);
50 }
51 
53 {
54  if (!IsRoad()) {
55  return FColor(0u, 0u, 0u, 255u);
56  } else if (!HasDirection()) {
57  return FColor(255u, 255u, 255u, 255u);
58  } else {
59  auto ToColor = [](float X){
60  return FMath::FloorToInt(256.0 * (X + PI) / (2.0f * PI)) % 256;
61  };
62  const float Azimuth = GetDirectionAzimuthalAngle();
63  return FColor(0u, 255u, ToColor(Azimuth), 255u);
64  }
65 }
66 
67 // =============================================================================
68 // -- URoadMap -----------------------------------------------------------------
69 // =============================================================================
70 
71 URoadMap::URoadMap(const FObjectInitializer& ObjectInitializer) :
72  Super(ObjectInitializer),
73  PixelsPerCentimeter(1.0f),
74  Width(1u),
75  Height(1u)
76 {
77  RoadMapData.Add(0u);
78  static_assert(
79  std::is_same<decltype(FRoadMapPixelData::Value), typename decltype(RoadMapData)::ElementType>::value,
80  "Declaration map of FRoadMapPixelData's value does not match current serialization type");
81 }
82 
84  const uint32 inWidth,
85  const uint32 inHeight,
86  const float inPixelsPerCentimeter,
87  const FTransform &inWorldToMap,
88  const FVector &inMapOffset)
89 {
90  RoadMapData.Init(0u, inWidth * inHeight);
91  Width = inWidth;
92  Height = inHeight;
93  PixelsPerCentimeter = inPixelsPerCentimeter;
94  WorldToMap = inWorldToMap;
95  MapOffset = inMapOffset;
96 }
97 
99  const uint32 PixelX,
100  const uint32 PixelY,
101  const ECityMapMeshTag Tag,
102  const FTransform &Transform,
103  const bool bInvertDirection)
104 {
105  bool bIsRoad = false;
106  bool bHasDirection = false;
107  FVector Direction(0.0f, 0.0f, 0.0f);
108 
109  auto Rotator = Transform.GetRotation().Rotator();
110 
111  switch (Tag) {
112  default:
113  // It's not road.
114  break;
115  case ECityMapMeshTag::RoadTwoLanes_LaneRight:
116  case ECityMapMeshTag::Road90DegTurn_Lane1:
117  case ECityMapMeshTag::RoadTIntersection_Lane1:
118  case ECityMapMeshTag::RoadTIntersection_Lane9:
119  case ECityMapMeshTag::RoadXIntersection_Lane1:
120  case ECityMapMeshTag::RoadXIntersection_Lane9:
121  bIsRoad = true;
122  bHasDirection = true;
123  Rotator.Yaw += 180.0f;
124  break;
125  case ECityMapMeshTag::RoadTwoLanes_LaneLeft:
126  case ECityMapMeshTag::Road90DegTurn_Lane0:
127  case ECityMapMeshTag::RoadTIntersection_Lane0:
128  case ECityMapMeshTag::RoadTIntersection_Lane2:
129  case ECityMapMeshTag::RoadTIntersection_Lane5:
130  case ECityMapMeshTag::RoadTIntersection_Lane8:
131  case ECityMapMeshTag::RoadXIntersection_Lane0:
132  case ECityMapMeshTag::RoadXIntersection_Lane8:
133  bIsRoad = true;
134  bHasDirection = true;
135  break;
136  case ECityMapMeshTag::Road90DegTurn_Lane9:
137  case ECityMapMeshTag::RoadTIntersection_Lane7:
138  case ECityMapMeshTag::RoadXIntersection_Lane7:
139  case ECityMapMeshTag::RoadXIntersection_Lane5:
140  bIsRoad = true;
141  bHasDirection = true;
142  Rotator.Yaw += 90.0f;
143  break;
144  case ECityMapMeshTag::Road90DegTurn_Lane7:
145  bIsRoad = true;
146  bHasDirection = true;
147  Rotator.Yaw += 90.0f; //+ 15.5f;
148  break;
149  case ECityMapMeshTag::Road90DegTurn_Lane5:
150  bIsRoad = true;
151  bHasDirection = true;
152  Rotator.Yaw += 90.0f + 35.0f;
153  break;
154  case ECityMapMeshTag::Road90DegTurn_Lane3:
155  bIsRoad = true;
156  bHasDirection = true;
157  Rotator.Yaw += 90.0f + 45.0f + 20.5f;
158  break;
159  case ECityMapMeshTag::Road90DegTurn_Lane8:
160  case ECityMapMeshTag::RoadTIntersection_Lane4:
161  case ECityMapMeshTag::RoadXIntersection_Lane2:
162  case ECityMapMeshTag::RoadXIntersection_Lane4:
163  bIsRoad = true;
164  bHasDirection = true;
165  Rotator.Yaw += 270.0f;
166  break;
167  case ECityMapMeshTag::Road90DegTurn_Lane6:
168  bIsRoad = true;
169  bHasDirection = true;
170  Rotator.Yaw += 270.0f + 50.0f;
171  break;
172  case ECityMapMeshTag::Road90DegTurn_Lane4:
173  bIsRoad = true;
174  bHasDirection = true;
175  Rotator.Yaw += 270.0f + 80.0f;
176  break;
177  case ECityMapMeshTag::Road90DegTurn_Lane2:
178  bIsRoad = true;
179  bHasDirection = true;
180  //Rotator.Yaw += 270.0f + 70.0f;
181  break;
182  case ECityMapMeshTag::RoadTIntersection_Lane3:
183  case ECityMapMeshTag::RoadTIntersection_Lane6:
184  case ECityMapMeshTag::RoadXIntersection_Lane3:
185  case ECityMapMeshTag::RoadXIntersection_Lane6:
186  bIsRoad = true;
187  bHasDirection = false;
188  break;
189  }
190  if (bHasDirection) {
191  FQuat Rotation(Rotator);
192  Direction = Rotation.GetForwardVector();
193  if (bInvertDirection) {
194  Direction *= -1.0f;
195  }
196  }
197  const auto Value = FRoadMapPixelData::Encode(bIsRoad, bHasDirection, Direction);
198  RoadMapData[GetIndex(PixelX, PixelY)] = Value;
199 }
200 
201 FVector URoadMap::GetWorldLocation(uint32 PixelX, uint32 PixelY) const
202 {
203  const FVector RelativePosition(
204  static_cast<float>(PixelX) / PixelsPerCentimeter,
205  static_cast<float>(PixelY) / PixelsPerCentimeter,
206  0.0f);
207  return WorldToMap.InverseTransformPosition(RelativePosition + MapOffset);
208 }
209 
210 FRoadMapPixelData URoadMap::GetDataAt(const FVector &WorldLocation) const
211 {
212  check(IsValid());
213  const FVector Location = WorldToMap.TransformPosition(WorldLocation) - MapOffset;
214  uint32 X = ClampFloatToUInt(PixelsPerCentimeter * Location.X, 0, Width - 1);
215  uint32 Y = ClampFloatToUInt(PixelsPerCentimeter * Location.Y, 0, Height - 1);
216  return GetDataAt(X, Y);
217 }
218 
220  const FTransform &BoxTransform,
221  const FVector &BoxExtent,
222  float ChecksPerCentimeter) const
223 {
224  auto DirectionOfMovement = BoxTransform.GetRotation().GetForwardVector();
225  DirectionOfMovement.Z = 0.0f; // Project to XY plane (won't be normalized anymore).
226  uint32 CheckCount = 0u;
227  FRoadMapIntersectionResult Result = {0.0f, 0.0f};
228  const float Step = 1.0f / ChecksPerCentimeter;
229  for (float X = -BoxExtent.X; X < BoxExtent.X; X += Step) {
230  for (float Y = -BoxExtent.Y; Y < BoxExtent.Y; Y += Step) {
231  ++CheckCount;
232  auto Location = BoxTransform.TransformPosition(FVector(X, Y, 0.0f));
233  const auto &Data = GetDataAt(Location);
234  if (!Data.IsRoad()) {
235  Result.OffRoad += 1.0f;
236  } else if (Data.HasDirection() &&
237  0.0f > FVector::DotProduct(Data.GetDirection(), DirectionOfMovement)) {
238  Result.OppositeLane += 1.0f;
239  }
240  }
241  }
242  if (CheckCount > 0u) {
243  Result.OffRoad /= static_cast<float>(CheckCount);
244  Result.OppositeLane /= static_cast<float>(CheckCount);
245  } else {
246  UE_LOG(LogCarla, Warning, TEXT("URoadMap::Intersect did zero checks"));
247  }
248  return Result;
249 }
250 
251 bool URoadMap::SaveAsPNG(const FString &Folder, const FString &MapName) const
252 {
253  if (!IsValid()) {
254  UE_LOG(LogCarla, Error, TEXT("Cannot save invalid road map to disk"));
255  return false;
256  }
257 
258  const FString ImagePath = FPaths::Combine(Folder, MapName + TEXT(".png"));
259  const FString MetadataPath = FPaths::Combine(Folder, MapName + TEXT(".txt"));
260 
261  const FIntPoint DestSize(Width, Height);
262  TUniquePtr<TImagePixelData<FColor>> PixelData = MakeUnique<TImagePixelData<FColor>>(DestSize);
263  PixelData->Pixels.Reserve(RoadMapData.Num());
264  for (auto Value : RoadMapData) {
265  PixelData->Pixels.Emplace(FRoadMapPixelData(Value).EncodeAsColor());
266  }
267  FPixelReader::SavePixelsToDisk(std::move(PixelData), ImagePath);
268 
269  // Save metadata.
270  FFormatNamedArguments Args;
271  Args.Add("MapName", FText::FromString(MapName));
272  Args.Add("Width", GetWidth());
273  Args.Add("Height", GetHeight());
274  Args.Add("CmPerPixel", 1.0f / PixelsPerCentimeter);
275  Args.Add("Transform", FText::FromString(WorldToMap.ToString()));
276  Args.Add("Offset", FText::FromString(MapOffset.ToString()));
277  const auto Contents = FText::Format(
278  LOCTEXT("RoadMapMetadata",
279  "Map name = {MapName}\n"
280  "Size = {Width}x{Height} pixels\n"
281  "Density = {CmPerPixel} cm/pixel\n"
282  "World-To-Map Transform (T|R|S) = ({Transform})\n"
283  "Map Offset = ({Offset})\n"),
284  Args);
285  if (!FFileHelper::SaveStringToFile(Contents.ToString(), *MetadataPath)) {
286  UE_LOG(LogCarla, Error, TEXT("Failed to save map metadata"));
287  }
288 
289  UE_LOG(LogCarla, Log, TEXT("Saved road map to \"%s\""), *ImagePath);
290  return true;
291 }
292 
293 #if WITH_EDITOR
294 
295 void URoadMap::Log() const
296 {
297  const float MapSizeInMB = // Only map data, not the class itself.
298  static_cast<float>(sizeof(decltype(RoadMapData)::ElementType) * RoadMapData.Num()) /
299  (1024.0f * 1024.0f);
300  UE_LOG(
301  LogCarla,
302  Log,
303  TEXT("Generated road map %dx%d (%.2fMB) with %.2f cm/pixel"),
304  GetWidth(),
305  GetHeight(),
306  MapSizeInMB,
307  1.0f / PixelsPerCentimeter);
308 
309  if (!IsValid()) {
310  UE_LOG(LogCarla, Error, TEXT("Error generating road map"));
311  return;
312  }
313 }
314 
315 void URoadMap::DrawDebugPixelsToLevel(UWorld *World, const bool bJustFlushDoNotDraw) const
316 {
317  const FVector ZOffset(0.0f, 0.0f, 50.0f);
318  FlushPersistentDebugLines(World);
319  if (!bJustFlushDoNotDraw) {
320  for (auto X = 0u; X < Width; ++X) {
321  for (auto Y = 0u; Y < Height; ++Y) {
322  auto Location = GetWorldLocation(X, Y) + ZOffset;
323  const auto &Data = GetDataAt(X, Y);
324  auto Color = Data.EncodeAsColor();
325  if (Data.HasDirection()) {
326  const FVector ArrowEnd = Location + 50.0f * Data.GetDirection();
327  DrawDebugDirectionalArrow(World, Location, ArrowEnd, 60.0f, Color, true);
328  } else {
329  DrawDebugPoint(World, Location, 6.0f, Color, true);
330  }
331  }
332  }
333  }
334 }
335 
336 #endif // WITH_EDITOR
337 
338 #undef LOCTEXT_NAMESPACE
TArray< uint16 > RoadMapData
Definition: RoadMap.h:192
bool IsValid() const
Definition: RoadMap.h:166
bool IsRoad() const
Whether this pixel lies in-road.
Definition: RoadMap.h:46
void Reset(uint32 Width, uint32 Height, float PixelsPerCentimeter, const FTransform &WorldToMap, const FVector &MapOffset)
Resets current map an initializes an empty map of the given size.
Definition: RoadMap.cpp:83
FVector GetWorldLocation(uint32 PixelX, uint32 PixelY) const
Return the world location of a given pixel.
Definition: RoadMap.cpp:201
void SetPixelAt(uint32 PixelX, uint32 PixelY, ECityMapMeshTag Tag, const FTransform &Transform, bool bInvertDirection=false)
Definition: RoadMap.cpp:98
sensor::data::Color Color
uint32 Height
Height of the map in pixels.
Definition: RoadMap.h:189
Road map intersection result. See URoadMap.
Definition: RoadMap.h:15
uint32 GetHeight() const
Definition: RoadMap.h:119
ECityMapMeshTag
Tag to identify the meshes used by the ProceduralMapGenerator.
URoadMap(const FObjectInitializer &ObjectInitializer)
Creates a valid empty map (every point is off-road).
Definition: RoadMap.cpp:71
FRoadMapPixelData GetDataAt(uint32 PixelX, uint32 PixelY) const
Retrieve the data stored at a given pixel.
Definition: RoadMap.h:128
uint32 GetWidth() const
Definition: RoadMap.h:114
Data stored in a road map pixel. See URoadMap.
Definition: RoadMap.h:29
uint16 Value
Definition: RoadMap.h:84
static constexpr int IsRoadRow
Definition: RoadMap.h:33
static uint16 Encode(bool IsRoad, bool HasDirection, const FVector &Direction)
Definition: RoadMap.cpp:44
geom::Location Location
Definition: rpc/Location.h:14
static float GetRotatedAzimuthAngle(const FVector &Direction)
Definition: RoadMap.cpp:34
static constexpr uint16 MaximumEncodedAngle
Definition: RoadMap.h:37
FRoadMapIntersectionResult Intersect(const FTransform &BoxTransform, const FVector &BoxExtent, float ChecksPerCentimeter) const
Intersect actor bounds with map.
Definition: RoadMap.cpp:219
static uint32 ClampFloatToUInt(const float Value, int32 Min, int32 Max)
Definition: RoadMap.cpp:27
static constexpr int HasDirectionRow
Definition: RoadMap.h:35
FTransform WorldToMap
World-to-map transform.
Definition: RoadMap.h:173
FColor EncodeAsColor() const
Definition: RoadMap.cpp:52
float GetDirectionAzimuthalAngle() const
Get the azimuth angle [-PI, PI] of the road direction (in spherical coordinates) at this pixel...
Definition: RoadMap.h:62
geom::Rotation Rotation
Definition: rpc/Transform.h:14
float OppositeLane
Percentage of the box invading opposite lane (wrong direction).
Definition: RoadMap.h:25
uint32 Width
Width of the map in pixels.
Definition: RoadMap.h:185
bool SaveAsPNG(const FString &Folder, const FString &MapName) const
Save the current map as PNG with the pixel data encoded as color.
Definition: RoadMap.cpp:251
int32 GetIndex(uint32 PixelX, uint32 PixelY) const
Definition: RoadMap.h:161
FVector MapOffset
Offset of the map in map coordinates.
Definition: RoadMap.h:177
static TFuture< bool > SavePixelsToDisk(UTextureRenderTarget2D &RenderTarget, const FString &FilePath)
Asynchronously save the pixels in RenderTarget to disk.
geom::Transform Transform
Definition: rpc/Transform.h:16
float PixelsPerCentimeter
Number of pixels per centimeter.
Definition: RoadMap.h:181
float OffRoad
Percentage of the box lying off-road.
Definition: RoadMap.h:21
bool HasDirection() const
Whether this pixel has a direction defined (e.g.
Definition: RoadMap.h:53