Agrarsense
Camera.cpp
Go to the documentation of this file.
1// Copyright (c) 2023 FrostBit Software Lab at the Lapland University of Applied Sciences
2//
3// This work is licensed under the terms of the MIT license.
4// For a copy, see <https://opensource.org/licenses/MIT>.
5
6#include "Camera.h"
7
16
17#include "NiagaraComponent.h"
18#include "Components/PrimitiveComponent.h"
19#include "Materials/Material.h"
20#include "ImageUtils.h"
21#include "Async/Async.h"
22#include "HighResScreenshot.h"
23#include "ImageWriteTask.h"
24#include "ImageWriteQueue.h"
25#include "Misc/DateTime.h"
26#include "Math/Color.h"
27#include "Blueprint/UserWidget.h"
28#include "GameFramework/PlayerController.h"
29#include "GeoReferencingSystem.h"
30#include "TimerManager.h"
31#include "GameFramework/SpringArmComponent.h"
32#include "Async/ParallelFor.h"
33
34ACamera::ACamera(const FObjectInitializer& ObjectInitializer) : Super(ObjectInitializer)
35{
36 PrimaryActorTick.bCanEverTick = true;
37 PrimaryActorTick.TickGroup = TG_PostUpdateWork;
38
39 CaptureRenderTarget = CreateDefaultSubobject<UTextureRenderTarget2D>(FName(*FString::Printf(TEXT("CaptureRenderTarget"))));
40 CaptureRenderTarget->CompressionSettings = TextureCompressionSettings::TC_Default;
41 CaptureRenderTarget->SRGB = false;
42 //CaptureRenderTarget->bForceLinearGamma = true;
43 CaptureRenderTarget->bAutoGenerateMips = false;
44 CaptureRenderTarget->bGPUSharedFlag = true;
45 //CaptureRenderTarget->RenderTargetFormat = RTF_RGBA8;
46 CaptureRenderTarget->AddressX = TextureAddress::TA_Clamp;
47 CaptureRenderTarget->AddressY = TextureAddress::TA_Clamp;
48
49 CaptureComponent2D = CreateDefaultSubobject<USceneCaptureComponent2D>(FName(*FString::Printf(TEXT("SceneCaptureComponent2D"))));
50 CaptureComponent2D->SetupAttachment(RootComponent);
51 CaptureComponent2D->PrimitiveRenderMode = ESceneCapturePrimitiveRenderMode::PRM_RenderScenePrimitives;
52 CaptureComponent2D->bCaptureOnMovement = false;
53 CaptureComponent2D->bCaptureEveryFrame = false;
54 CaptureComponent2D->bAlwaysPersistRenderingState = true;
55}
56
58{
59 SetupCamera(newParameters);
60}
61
62void ACamera::AddPostProcessingMaterial(const FString& Path, float Weight)
63{
64 UMaterial* PostProcessingMat = Cast<UMaterial>(StaticLoadObject(UMaterial::StaticClass(), nullptr, *Path));
65 if (PostProcessingMat && CaptureComponent2D)
66 {
67 FPostProcessSettings& PostProcessSettings = CaptureComponent2D->PostProcessSettings;
68 PostProcessSettings.AddBlendable(PostProcessingMat, Weight);
69 }
70 else
71 {
72 const FString ErrorMessage = FString::Printf(TEXT("Failed to add post-processing material to sensor: %s"), *GetSensorName());
73 SimulatorLog::Log(ErrorMessage);
74 }
75}
76
78{
80 {
81 FPostProcessSettings& PostProcessSettings = CaptureComponent2D->PostProcessSettings;
82 PostProcessSettings.RemoveBlendable(Material);
83 }
84}
85
86void ACamera::Init(FCameraBaseParameters parameters, bool SimulateSensor)
87{
89 SetSimulateSensor(SimulateSensor);
90 SetupCamera(parameters);
91}
92
94{
95 if (IsValid(LogFile))
96 {
97 // File has already been created, return
98 return;
99 }
100
101 FLogFileSettings Settings;
104 Settings.QueueLength = MAX_int32; // Only write the log after destroying the sensor
105 Settings.KeepFileOpen = false;
106 Settings.Timestamp = false;
107 Settings.OverrideFilePath = true;
108 Settings.FilePath = FileSavePath;
109
110 LogFile = NewObject<ULogFile>(ULogFile::StaticClass());
111 if (IsValid(LogFile))
112 {
113 const FString FileName = "camera_metadata";
114 LogFile->Create(FileName, Settings);
115
116 // Write camera metadata first line to explain the values.
117 GeoReferencingSystem = AGeoReferencingSystem::GetGeoReferencingSystem(GetWorld());
119 {
120 WriteToLogFile("timestamp, image_name, X location, Y location, Z location, yaw rotation, pitch rotation, roll rotation, GPS latitude, GPS longitude, GPS altitude");
121 }
122 else
123 {
124 WriteToLogFile("timestamp, image_name, X location, Y location, Z location, yaw rotation, pitch rotation, roll rotation");
125 }
126 }
127}
128
130{
132 {
133#if WITH_EDITOR
134 UE_LOG(LogTemp, Warning, TEXT("Camera.cpp: CaptureComponent2D is nullptr!"));
135#endif
136 return;
137 }
138
139 CameraParameters = parameters;
140
142
144 {
146 }
147
148 const bool UsePostProcessingEffects = CameraParameters.PostProcessingEffects;
149 auto& PostProcessSettings = CaptureComponent2D->PostProcessSettings;
150
151 if (parameters.UsePhysicLensDistortionEffect)
152 {
153 if (!PhysicLensDistortion.IsValid())
154 {
155 const FString Path = "/Game/Agrarsense/Materials/PostProcessingMaterials/PhysicLensDistortion.PhysicLensDistortion";
156 PhysicLensDistortion = Cast<UMaterial>(StaticLoadObject(UMaterial::StaticClass(), nullptr, *Path));
157
158 if (PhysicLensDistortion.IsValid())
159 {
160 PostProcessSettings.AddBlendable(PhysicLensDistortion.Get(), 1.0f);
161#if WITH_EDITOR
162 UE_LOG(LogTemp, Warning, TEXT("Camera.cpp: Added physics lens distortion effect."));
163#endif
164 }
165 }
166 }
167 else if (PhysicLensDistortion.IsValid())
168 {
169 PostProcessSettings.RemoveBlendable(PhysicLensDistortion.Get());
170 PhysicLensDistortion.Reset();
171#if WITH_EDITOR
172 UE_LOG(LogTemp, Warning, TEXT("Camera.cpp: Removed physics lens distortion effect."));
173#endif
174 }
175
176 if (parameters.UseIceLensEffect)
177 {
178 if (!IceMaterialInstance.IsValid())
179 {
180 const FString Path = "/Game/Agrarsense/Materials/PostProcessingMaterials/CameraPostProcessEffects/m_ice_lens_effect";
181 UMaterial* LoadedIceMaterial = Cast<UMaterial>(StaticLoadObject(UMaterial::StaticClass(), nullptr, *Path));
182
183 // Create UMaterialInstanceDynamic from LoadedIceMaterial
184 IceMaterialInstance = UMaterialInstanceDynamic::Create(LoadedIceMaterial, nullptr);
185 if (IceMaterialInstance.IsValid())
186 {
187 // Add material to CaptureComponent2D PostProcessSettings
188 PostProcessSettings.AddBlendable(IceMaterialInstance.Get(), 1.0f);
189#if WITH_EDITOR
190 UE_LOG(LogTemp, Warning, TEXT("Camera.cpp: Added ice lens effect."));
191#endif
192 }
193 }
194
195 UMaterialInstanceDynamic* IceMatInstance = IceMaterialInstance.Get();
196 if (IceMatInstance)
197 {
198 // Set IceMaterialInstance scalar parameters values
199 IceMatInstance->SetScalarParameterValue(FName("Strength"), parameters.IceLensEffectStrength);
200 IceMatInstance->SetScalarParameterValue(FName("Angle"), parameters.IceLensEffectAngle);
201#if WITH_EDITOR
202 UE_LOG(LogTemp, Warning, TEXT("Camera.cpp: Changed ice lens effect parameters."));
203#endif
204 }
205 }
206 else if (IceMaterialInstance.IsValid())
207 {
208 UMaterialInstanceDynamic* IceMatInstance = IceMaterialInstance.Get();
209 if (IceMatInstance)
210 {
211 // Remove material from CaptureComponent2D PostProcessSettings
212 PostProcessSettings.RemoveBlendable(IceMatInstance);
213 IceMaterialInstance.Reset();
214
215#if WITH_EDITOR
216 UE_LOG(LogTemp, Warning, TEXT("Camera.cpp: Removed ice lens effect."));
217#endif
218 }
219 }
220
223 const float TargetGamma = CameraParameters.TargetGamma;
224 const bool Enable16BitFormat = CameraParameters.Enable16BitFormat;
225 bool ForceLinearGamma = !CameraParameters.PostProcessingEffects;
226
227 ESensorTypes SensorType = GetSensorType();
228 EPixelFormat PixelFormat = Enable16BitFormat ? PF_FloatRGB : PF_B8G8R8A8;
229
231 {
232 PixelFormat = PF_B8G8R8A8;
233 ForceLinearGamma = true;
234 }
235
236 CaptureRenderTarget->InitCustomFormat(ImageWidth, ImageHeight, PixelFormat, ForceLinearGamma);
237 //CaptureRenderTarget->InitCustomFormat(ImageWidth, ImageHeight, PF_B8G8R8A8, true);
238 CaptureRenderTarget->TargetGamma = TargetGamma;
239 CaptureRenderTarget->UpdateResourceImmediate(true);
240
241 // Set PostProcessSettings override (on or off)
242 PostProcessSettings.bOverride_AutoExposureMethod = UsePostProcessingEffects;
243 PostProcessSettings.bOverride_AutoExposureBias = UsePostProcessingEffects;
244 PostProcessSettings.bOverride_AutoExposureMinBrightness = UsePostProcessingEffects;
245 PostProcessSettings.bOverride_AutoExposureMaxBrightness = UsePostProcessingEffects;
246 PostProcessSettings.bOverride_AutoExposureSpeedUp = UsePostProcessingEffects;
247 PostProcessSettings.bOverride_AutoExposureSpeedDown = UsePostProcessingEffects;
248 PostProcessSettings.bOverride_HistogramLogMin = UsePostProcessingEffects;
249 PostProcessSettings.bOverride_HistogramLogMax = UsePostProcessingEffects;
250 PostProcessSettings.bOverride_CameraShutterSpeed = UsePostProcessingEffects;
251 PostProcessSettings.bOverride_CameraISO = UsePostProcessingEffects;
252 PostProcessSettings.bOverride_DepthOfFieldFstop = UsePostProcessingEffects;
253 PostProcessSettings.bOverride_DepthOfFieldMinFstop = UsePostProcessingEffects;
254 PostProcessSettings.bOverride_DepthOfFieldBladeCount = UsePostProcessingEffects;
255 PostProcessSettings.bOverride_FilmSlope = UsePostProcessingEffects;
256 PostProcessSettings.bOverride_FilmToe = UsePostProcessingEffects;
257 PostProcessSettings.bOverride_FilmShoulder = UsePostProcessingEffects;
258 PostProcessSettings.bOverride_FilmWhiteClip = UsePostProcessingEffects;
259 PostProcessSettings.bOverride_FilmBlackClip = UsePostProcessingEffects;
260 PostProcessSettings.bOverride_MotionBlurAmount = UsePostProcessingEffects;
261 PostProcessSettings.bOverride_MotionBlurMax = UsePostProcessingEffects;
262 PostProcessSettings.bOverride_MotionBlurPerObjectSize = UsePostProcessingEffects;
263 PostProcessSettings.bOverride_WhiteTemp = UsePostProcessingEffects;
264 PostProcessSettings.bOverride_WhiteTint = UsePostProcessingEffects;
265 PostProcessSettings.bOverride_ColorContrast = UsePostProcessingEffects;
266 PostProcessSettings.bOverride_SceneFringeIntensity = UsePostProcessingEffects;
267 PostProcessSettings.bOverride_ChromaticAberrationStartOffset = UsePostProcessingEffects;
268 PostProcessSettings.bOverride_AmbientOcclusionIntensity = UsePostProcessingEffects;
269 PostProcessSettings.bOverride_AmbientOcclusionRadius = UsePostProcessingEffects;
270 PostProcessSettings.bOverride_AmbientOcclusionStaticFraction = UsePostProcessingEffects;
271 PostProcessSettings.bOverride_AmbientOcclusionFadeDistance = UsePostProcessingEffects;
272 PostProcessSettings.bOverride_AmbientOcclusionPower = UsePostProcessingEffects;
273 PostProcessSettings.bOverride_AmbientOcclusionBias = UsePostProcessingEffects;
274 PostProcessSettings.bOverride_AmbientOcclusionQuality = UsePostProcessingEffects;
275 PostProcessSettings.bOverride_BloomMethod = UsePostProcessingEffects;
276 PostProcessSettings.bOverride_BloomIntensity = UsePostProcessingEffects;
277 PostProcessSettings.bOverride_BloomThreshold = UsePostProcessingEffects;
278 PostProcessSettings.bOverride_LensFlareIntensity = UsePostProcessingEffects;
279 PostProcessSettings.bOverride_DepthOfFieldFocalDistance = UsePostProcessingEffects;
280 PostProcessSettings.bOverride_DepthOfFieldDepthBlurAmount = UsePostProcessingEffects;
281 PostProcessSettings.bOverride_DepthOfFieldDepthBlurRadius = UsePostProcessingEffects;
282
283 // Set PostProcessSettings values
284 PostProcessSettings.CameraShutterSpeed = CameraParameters.ShutterSpeed;
285 PostProcessSettings.CameraISO = CameraParameters.ISO;
286 PostProcessSettings.DepthOfFieldFstop = CameraParameters.Aperture;
287 PostProcessSettings.DepthOfFieldFocalDistance = CameraParameters.FocalDistance;
288 PostProcessSettings.DepthOfFieldDepthBlurAmount = CameraParameters.DepthBlurAmount;
289 PostProcessSettings.DepthOfFieldDepthBlurRadius = CameraParameters.DepthBlurRadius;
290 PostProcessSettings.DepthOfFieldMinFstop = CameraParameters.DofMinFStop;
291 PostProcessSettings.DepthOfFieldBladeCount = CameraParameters.DofBladeCount;
292 PostProcessSettings.FilmSlope = CameraParameters.FilmSlope;
293 PostProcessSettings.FilmToe = CameraParameters.FilmToe;
294 PostProcessSettings.FilmShoulder = CameraParameters.FilmShoulder;
295 PostProcessSettings.FilmBlackClip = CameraParameters.FilmBlackClip;
296 PostProcessSettings.FilmWhiteClip = CameraParameters.FilmWhiteClip;
297 PostProcessSettings.AutoExposureMinBrightness = CameraParameters.ExposureMinBrightness;
298 PostProcessSettings.AutoExposureMaxBrightness = CameraParameters.ExposureMaxBrightness;
299 PostProcessSettings.AutoExposureSpeedUp = CameraParameters.ExposureSpeedUp;
300 PostProcessSettings.AutoExposureSpeedDown = CameraParameters.ExposureSpeedDown;
301 PostProcessSettings.MotionBlurAmount = CameraParameters.MotionBlurIntensity;
302 PostProcessSettings.MotionBlurMax = CameraParameters.MotionBlurMax;
303 PostProcessSettings.MotionBlurPerObjectSize = CameraParameters.MotionBlurMinObjSize;
304 PostProcessSettings.LensFlareIntensity = CameraParameters.LensFlareIntensity;
305 PostProcessSettings.BloomIntensity = CameraParameters.BloomIntensity;
306 PostProcessSettings.WhiteTemp = CameraParameters.WhiteTemp;
307 PostProcessSettings.WhiteTint = CameraParameters.WhiteTint;
308 PostProcessSettings.SceneFringeIntensity = CameraParameters.ChromAberrIntensity;
309 PostProcessSettings.ChromaticAberrationStartOffset = CameraParameters.ChromAberrOffset;
310
313
315 {
317 }
318 else
319 {
320 CaptureComponent2D->MaxViewDistanceOverride = -1.0f; // Infinite
321 }
322
324 {
325 // Enable HDR on CaptureSource, only allowed for RGB Camera for now.
326 CaptureComponent2D->CaptureSource = ESceneCaptureSource::SCS_FinalToneCurveHDR;
327 }
328 else
329 {
330 CaptureComponent2D->CaptureSource = ESceneCaptureSource::SCS_FinalColorLDR;
331 }
332
333 CaptureComponent2D->bUseRayTracingIfEnabled = true;
334 CaptureComponent2D->UpdateContent();
335 CaptureComponent2D->Activate();
336
338
339 const FString Encoding = "bgr8";
340
341 // Create ROS Image message
342 ImageMsg.Reset();
343 ImageMsg = MakeShared<ROSMessages::sensor_msgs::Image>();
344 ImageMsg->header.frame_id = "world";
345 ImageMsg->height = ImageWidth;
346 ImageMsg->width = ImageHeight;
347 ImageMsg->encoding = Encoding;
348 ImageMsg->is_bigendian = 0;
349 ImageMsg->step = ImageWidth * 4;
350
352
353 if (!UnrealWindow.IsValid())
354 {
355 // Create and setup UnrealWindow
356 const FString WindowName = GetSensorIdentifier() + " Window";
358
359 // Set on Window closed event
361 UnrealWindow->GetSWindow()->SetOnWindowClosed(OnWindowClosedDelegate);
362
363 // try to create Camera parameter Widget from path
364 FSoftClassPath WidgetClassPath(TEXT("/Game/Agrarsense/Widgets/Camera/WBP_CameraControlsMenu.WBP_CameraControlsMenu_C"));
365 UClass* WidgetClass = WidgetClassPath.TryLoadClass<UUserWidget>();
366
367 APlayerController* PlayerController = UGameplayStatics::GetPlayerController(GetWorld(), 0);
368
369 if (WidgetClass && PlayerController)
370 {
371 UCameraWidgetBase* WidgetInstance = CreateWidget<UCameraWidgetBase>(PlayerController, WidgetClass);
372 if (WidgetInstance)
373 {
374 bool ShowGuide = false;
375 WidgetInstance->Setup(this, ShowGuide);
376 this->AddWidgetToWindow(WidgetInstance);
377 }
378 }
379 }
380
381 bool Resized = false;
382 if (UnrealWindow.IsValid())
383 {
384 UnrealWindow->SetupComponent(CaptureRenderTarget);
385 if (UnrealWindow->GetWindowWidth() != ImageWidth || UnrealWindow->GetWindowHeight() != ImageHeight)
386 {
387 UnrealWindow->ResizeWindow(ImageWidth, ImageHeight);
388 Resized = true;
389 }
390 }
391
393
394 if (Resized)
395 {
397 }
398
400 {
402 }
403}
404
406{
407 Super::BeginPlay();
408
409 // Register to PreActorTick where USceneCaptureComponent2D will get rendered.
410 OnPreTickDelegate = FWorldDelegates::OnWorldPreActorTick.AddUObject(this, &ACamera::PreActorTick);
411
412 // Check if we should use ATickManager which ticks in parallel, or use Post actor tick
413 // In there we capture the screen pixels with FrameGrabber API,
414 // might do some extra processing, save image to disk and send data to ROS.
416 {
417 SetActorTickEnabled(false);
418 TickEntry = ATickManager::AddTick(this, BindTick(this, &ACamera::EndOfFrameParellel), ETickType::LateTickParallel);
419 }
420 else
421 {
422 SetActorTickEnabled(true);
423
424 if (UseReadPixels)
425 {
426 UE_LOG(LogTemp, Warning, TEXT("juu"));
427 OnPostTickDelegate = FWorldDelegates::OnWorldPostActorTick.AddUObject(this, &ACamera::PixelReadEndOfFrame);
428 }
429 else
430 {
431 OnPostTickDelegate = FWorldDelegates::OnWorldPostActorTick.AddUObject(this, &ACamera::EndOfFrame);
432 }
433 }
434
435 // Get all existing components set to be hidden for all Camera sensors (like Lidar point cloud visualization).
436 auto Components = ASensor::GetComponentsToHide();
437 for (int32 i = 0; i < Components.Num(); i++)
438 {
439 UPrimitiveComponent* Primitive = Components[i].Get();
440 if (Primitive)
441 {
442 HidePrimitiveComponent(Primitive);
443 }
444 }
445
446 OnPrimitiveAdded.AddUniqueDynamic(this, &ACamera::HidePrimitiveComponent);
447
448 GetWorld()->GetTimerManager().SetTimerForNextTick([this]()
449 {
450 if (IsValid(this))
451 {
452 DronePtr = Cast<APIDDrone>(GetAttachParentActor());
453 if (!DronePtr)
454 {
455 AllowGimbal = false;
456 }
457 }
458 });
459}
460
461void ACamera::EndPlay(const EEndPlayReason::Type EndPlayReason)
462{
463 Super::EndPlay(EndPlayReason);
464
465 FWorldDelegates::OnWorldPreActorTick.Remove(OnPreTickDelegate);
466
468 {
470 }
471 else
472 {
473 FWorldDelegates::OnWorldPostActorTick.Remove(OnPostTickDelegate);
474 }
475
476 if (UnrealWindow.IsValid())
477 {
478 OnWindowClosedDelegate.Unbind();
479 UnrealWindow->DestroyWindow();
480 UnrealWindow.Reset();
481 }
482
484
486 {
487 CaptureRenderTarget->ConditionalBeginDestroy();
488 CaptureRenderTarget = nullptr;
489 }
490
492 {
493 CaptureComponent2D->UnregisterComponent();
494 CaptureComponent2D->ConditionalBeginDestroy();
495 CaptureComponent2D = nullptr;
496 }
497
498 ImageMsg.Reset();
499 IceMaterialInstance.Reset();
500
501 if (BGR8Buffer)
502 {
503 delete[] BGR8Buffer;
504 BGR8Buffer = nullptr;
505 }
507}
508
509void ACamera::PreActorTick(UWorld* World, ELevelTick TickType, float DeltaSeconds)
510{
511 TRACE_CPUPROFILER_EVENT_SCOPE(ACamera::PreActorTick);
512
514
515 if (ShouldSimulate(DeltaSeconds) && CaptureComponent2D)
516 {
517 // Capture scene now.
518 // We could use CaptureSceneDeferred but it would not work if World (Spectator) Rendering is disabled.
519 CaptureComponent2D->CaptureScene();
520
521 // Inform FrameGrabber to capture this frame
522 if (FrameGrabber.IsValid() && !UseReadPixels)
523 {
524 FrameGrabber->CaptureThisFrame(FFramePayloadPtr());
525 }
526 }
527}
528
530{
531 // If this Camera sensor is attached to Drone,
532 // keep the camera steady kind of like a Gimbal.
534 {
535 if (SensorSetRotation == FRotator::ZeroRotator)
536 {
537 // Save garage sensor values
538 SensorSetRotation = GetTransform().Rotator();
539 }
540 FRotator GimbalRot = FRotator(SensorSetRotation.Pitch, GetActorRotation().Yaw, SensorSetRotation.Roll);
541 SetActorRotation(GimbalRot);
542 }
543}
544
545void ACamera::EndOfFrame(UWorld* World, ELevelTick TickType, float DeltaSeconds)
546{
548}
549
550void ACamera::PixelReadEndOfFrame(UWorld* World, ELevelTick TickType, float DeltaSeconds)
551{
553}
554
555void ACamera::EndOfFrameParellel(float DeltaTime)
556{
558}
559
560bool ACamera::ShouldSimulate(const float DeltaSeconds)
561{
562 if (!CanSimulateSensor())
563 {
564 ShouldSimulateCamera = false;
566 }
567
569 {
572 }
573
574 FrameRateTimer += DeltaSeconds;
576 {
577 FrameRateTimer = 0.0f;
580 }
581
582 ShouldSimulateCamera = false;
583
585}
586
588{
589 TRACE_CPUPROFILER_EVENT_SCOPE(ACamera::FrameGrabberCapture);
590
591 if (!ShouldSimulateCamera || !FrameGrabber.IsValid())
592 {
593 return;
594 }
595
597 {
598 return;
599 }
600
601 TArray<FCapturedFrameData> Frames = FrameGrabber->GetCapturedFrames();
602 if (Frames.Num())
603 {
604 FCapturedFrameData& LastFrame = Frames.Last();
605 TArray<FColor>& ImageBuffer = LastFrame.ColorBuffer;
606
607 AddProcessingToFrameBuffer(ImageBuffer);
608
610 {
612 {
615 }
616
617 AsyncTask(ENamedThreads::AnyBackgroundThreadNormalTask, [this, ImageBuffer]()
618 {
620 });
621 }
622
624 }
625}
626
628{
629 TRACE_CPUPROFILER_EVENT_SCOPE(ACamera::ReadPixelsCapture);
630
632 {
633 return;
634 }
635
637 {
638 return;
639 }
640
641 TArray<FColor> ImageBuffer;
642 ImageBuffer.Reserve(ImageWidth * ImageHeight);
643 FReadSurfaceDataFlags Flags(RCM_UNorm);
644 Flags.SetLinearToGamma(false); // IMPORTANT to get instance segmentation ID's correctly
645 auto* Resource = CaptureRenderTarget->GameThread_GetRenderTargetResource();
646 const bool succeeded = Resource->ReadPixels(ImageBuffer, Flags);
647
649 {
651 {
654 }
655
656 AsyncTask(ENamedThreads::AnyBackgroundThreadNormalTask, [this, ImageBuffer]()
657 {
659 });
660 }
661
663}
664
665void ACamera::AddProcessingToFrameBuffer(TArray<FColor>& buffer)
666{
667 return;
668}
669
670void ACamera::ApplyGammaCorrectionLUT(TArray<FColor>& Buffer)
671{
672 static uint8 GammaLUT[256];
673 static bool LUTInitialized = false;
674 if (!LUTInitialized)
675 {
676 LUTInitialized = true;
677 for (int32 i = 0; i < 256; ++i)
678 {
679 float Linear = FMath::Pow(i / 255.0f, 2.2f);
680 GammaLUT[i] = FMath::RoundToInt(Linear * 255.0f);
681 }
682 }
683
684 FColor* RESTRICT BufferPtr = Buffer.GetData();
685 const int32 Num = Buffer.Num();
686
687 const int32 ChunkSize = 512;
688 const int32 NumChunks = (Num + ChunkSize - 1) / ChunkSize;
689
690 ParallelFor(NumChunks, [BufferPtr, Num](int32 ChunkIndex)
691 {
692 const int32 Start = ChunkIndex * ChunkSize;
693 const int32 End = FMath::Min(Start + ChunkSize, Num);
694
695 for (int32 i = Start; i < End; ++i)
696 {
697 FColor& C = BufferPtr[i];
698 C.R = GammaLUT[C.R];
699 C.G = GammaLUT[C.G];
700 C.B = GammaLUT[C.B];
701 }
702 }, false);
703}
704
705void ACamera::HidePrimitiveComponent(UPrimitiveComponent* PrimitiveComponent)
706{
707 if (PrimitiveComponent && CaptureComponent2D)
708 {
709 CaptureComponent2D->HideComponent(PrimitiveComponent);
710 }
711}
712
714{
715 if (!UnrealWindow.IsValid())
716 {
717#if WITH_EDITOR
718 UE_LOG(LogTemp, Warning, TEXT("Camera.cpp: UnrealWindow is nullptr!"));
719#endif
720 return;
721 }
722
723 // Destroy old FrameGrabber
725
726 TSharedPtr<FSceneViewport> SceneViewport = UnrealWindow->GetSceneViewport();
727 if (SceneViewport.IsValid() && CaptureRenderTarget)
728 {
729 EPixelFormat PixelFormat = GetPixelFormatFromRenderTargetFormat(TextureFormat);
730 FIntPoint Size = SceneViewport->GetSize();
731
733 {
734 // Currently only DVS camera needs this texture to render DVS output into separate FUnrealWindow.
735 CaptureFrameTexture = UTexture2D::CreateTransient(Size.X, Size.Y, PixelFormat);
736 CaptureFrameTexture->UpdateResource();
737 }
738
739 FrameGrabber = MakeShareable(new FFrameGrabber(SceneViewport.ToSharedRef(), Size, PixelFormat));
740 FrameGrabber->StartCapturingFrames();
741 }
742}
743
745{
746 if (FrameGrabber.IsValid())
747 {
748 FrameGrabber->StopCapturingFrames();
749 FrameGrabber->Shutdown();
750 FrameGrabber.Reset();
751 }
752
754 {
755 CaptureFrameTexture->RemoveFromRoot();
756 CaptureFrameTexture->ConditionalBeginDestroy();
757 CaptureFrameTexture = nullptr;
758 }
759}
760
761void ACamera::SendImageDataToROS(const TArray<FColor>& FrameBuffer, int32 Width, int32 Height)
762{
763 TRACE_CPUPROFILER_EVENT_SCOPE(ACamera::SendImageDataToROS);
764
765 UTopic* Topic = GetROSTopic();
766 if (!SendDataToROS
767 || !Topic
768 || !IsROSConnected()
769 || FrameBuffer.IsEmpty()
770 || !ImageMsg.IsValid())
771 {
772 return;
773 }
774
775 // Cast TArray<FColor> into uint8[] data with all channels (RBGA)
776 // If you want to send RGBA data, uncomment the following lines
777 // and comment the BGR8Buffer conversion below
778 //ImageMsg->encoding = "bgra8";
779 //ImageMsg->step = Width * 4;
780 //ImageMsg->data = const_cast<uint8*>(reinterpret_cast<const uint8*>(FrameBuffer.GetData()));
781
782 // Convert TArray<FColor> to uint8[] data without alpha channel
783 // By removing alpha channel, we reduce ~25% of ROS bandwidth,
784 // but costs a bit more in processing time here
785 const int32 BufferSize = Width * Height * 3;
786 if (!BGR8Buffer || CurrentBufferSize != BufferSize)
787 {
788 // Resize buffer if needed
789 delete[] BGR8Buffer;
790 BGR8Buffer = new uint8[BufferSize];
791 CurrentBufferSize = BufferSize;
792 }
793
794 const FColor* RESTRICT Source = FrameBuffer.GetData();
795 uint8* RESTRICT Dest = BGR8Buffer;
796 const int32 PixelCount = FrameBuffer.Num();
797 const int32 ChunkSize = 512;
798 const int32 NumChunks = (PixelCount + ChunkSize - 1) / ChunkSize;
799
800 ParallelFor(NumChunks, [Source, Dest, ChunkSize, PixelCount](int32 ChunkIndex)
801 {
802 const int32 StartIndex = ChunkIndex * ChunkSize;
803 const int32 EndIndex = FMath::Min(StartIndex + ChunkSize, PixelCount);
804
805 for (int32 i = StartIndex; i < EndIndex; ++i)
806 {
807 const FColor& Pixel = Source[i];
808 const int32 j = i * 3;
809 Dest[j + 0] = Pixel.B;
810 Dest[j + 1] = Pixel.G;
811 Dest[j + 2] = Pixel.R;
812 }
813 }, false);
814
815 ImageMsg->data = BGR8Buffer;
816 ImageMsg->step = Width * 3;
817
818 ImageMsg->height = Height;
819 ImageMsg->width = Width;
820 ImageMsg->header.time = FROSTime::Now();
821
822 Topic->Publish(ImageMsg);
823}
824
825void ACamera::SaveImageToDisk(const TArray<FColor> FrameBuffer, int32 Width, int32 Height)
826{
827 TRACE_CPUPROFILER_EVENT_SCOPE(ACamera::SaveImageToDisk);
828
829 if (FrameBuffer.IsEmpty() || Width == 0 || Height == 0)
830 {
831 return;
832 }
833
834 int32 Size = Width * Height;
835 if (FrameBuffer.Num() != Size)
836 {
837 const FString Msg = "Camera sensor: Unable to save Image to disk. FrameBuffer size and resolution don't match!";
839 return;
840 }
841
842 const FString ImageName = FString::Printf(TEXT("image_%d.png"), ImageNumber);
843 const FString FullFileName = FString::Printf(TEXT("%s%s"), *FileSavePath, *ImageName);
844 ++ImageNumber;
845
846 SaveCameraMetaDataToDisk(ImageName);
847
848 FIntPoint DestSize(Width, Height);
849 TImagePixelData<FColor> PixelData(DestSize);
850 PixelData.Pixels = FrameBuffer;
851
852 // Create ImageTask
853 TUniquePtr<FImageWriteTask> ImageTask = MakeUnique<FImageWriteTask>();
854 ImageTask->PixelData = MakeUnique<TImagePixelData<FColor>>(PixelData);
855 ImageTask->Filename = FullFileName;
856 ImageTask->Format = EImageFormat::PNG;
857 ImageTask->CompressionQuality = (int32)EImageCompressionQuality::Default;
858 ImageTask->bOverwriteFile = true;
859 ImageTask->PixelPreProcessors.Add(TAsyncAlphaWrite<FColor>(255));
860
861 // Get Screenshot config and enqueue image save task
862 FHighResScreenshotConfig& HighResScreenshotConfig = GetHighResScreenshotConfig();
863 TFuture<bool> CompletionFuture = HighResScreenshotConfig.ImageWriteQueue->Enqueue(MoveTemp(ImageTask));
864}
865
866void ACamera::SaveCameraMetaDataToDisk(const FString& ImageName)
867{
868 if (!IsValid(LogFile))
869 {
870 // If the log file located in base class is not valid, return here.
871 return;
872 }
873
874 const FVector ActorPosition = GetActorLocation();
875 const FRotator ActorRotation = GetActorRotation();
876
877 const FString TimeStamp = CreateTimeStampString();
878 FString MetaData;
879
881 {
882 FGeographicCoordinates GeoCoordinates = UCoordinateConversionUtilities::UnrealToGeographicCoordinates(GeoReferencingSystem, ActorPosition);
883 MetaData = FString::Printf(TEXT("%s, %s, %.2f, %.2f, %.2f, %.2f, %.2f, %.2f, %.8f, %.8f, %.8f"),
884 *TimeStamp, *ImageName,
885 ActorPosition.X, ActorPosition.Y, ActorPosition.Z,
886 ActorRotation.Pitch, ActorRotation.Yaw, ActorRotation.Roll,
887 GeoCoordinates.Latitude, GeoCoordinates.Longitude, GeoCoordinates.Altitude);
888 }
889 else
890 {
891 MetaData = FString::Printf(TEXT("%s, %s, %.2f, %.2f, %.2f, %.2f, %.2f, %.2f"),
892 *TimeStamp, *ImageName,
893 ActorPosition.X, ActorPosition.Y, ActorPosition.Z,
894 ActorRotation.Pitch, ActorRotation.Yaw, ActorRotation.Roll);
895 }
896
897 // Write to the log file (this is written after sensor is destroyed)
898 WriteToLogFile(MetaData);
899}
900
901void ACamera::OnWindowClosed(const TSharedRef<SWindow>& Window)
902{
903#if WITH_EDITOR
904 UE_LOG(LogTemp, Warning, TEXT("Camera.cpp: Camera window closed. Destroying Camera sensor.."));
905#endif
906
907 // Broadcast event that this Camera sensor is about be destroyed when user has clicked Window Close button.
908 OnCameraWindowClosed.Broadcast(this);
909
910 // Destroy this camera sensor
911 this->Destroy();
912}
913
914void ACamera::AddWidgetToWindow(UWidget* WidgetToAdd)
915{
916 if (UnrealWindow.IsValid() && WidgetToAdd)
917 {
918 UnrealWindow->AddUWidgetToWindow(WidgetToAdd);
919 }
920}
921
922void ACamera::RemoveWidgetFromWindow(UWidget* WidgetToRemove)
923{
924 if (UnrealWindow.IsValid() && WidgetToRemove)
925 {
926 UnrealWindow->RemoveUWidgetFromWindow(WidgetToRemove);
927 }
928}
929
930void ACamera::SetShadowRendering(bool RenderShadows)
931{
933 {
934 auto& CameraShowFlags = CaptureComponent2D->ShowFlags;
935 CameraShowFlags.SetDynamicShadows(RenderShadows);
936 CameraShowFlags.SetContactShadows(RenderShadows);
937 CameraShowFlags.SetCapsuleShadows(RenderShadows);
938 }
939}
940
941void ACamera::SetTemporalAA(bool SetTemporal)
942{
944 {
945 auto& CameraShowFlags = CaptureComponent2D->ShowFlags;
946 CameraShowFlags.SetTemporalAA(SetTemporal);
947 }
948}
949
950void ACamera::ResizeCamera(int32 Width, int32 Height)
951{
952 if (Width == 0 || Height == 0)
953 {
954 return;
955 }
956
957 CameraParameters.Width = Width;
958 CameraParameters.Height = Height;
960}
961
962void ACamera::AddHiddenActor(AActor* Actor)
963{
964 if (Actor && CaptureComponent2D)
965 {
966 CaptureComponent2D->HiddenActors.AddUnique(Actor);
967 }
968}
969
971{
973 {
974 return;
975 }
976
977 auto& ShowFlags = CaptureComponent2D->ShowFlags;
978
979 ShowFlags.SetDeferredLighting(false);
980 ShowFlags.SetSkyLighting(false);
981 ShowFlags.SetAmbientOcclusion(false);
982 ShowFlags.SetAntiAliasing(false);
983 ShowFlags.SetVolumetricFog(false);;
984 ShowFlags.SetBloom(false);
985 ShowFlags.SetCameraImperfections(false);
986 ShowFlags.SetCameraInterpolation(false);
987 ShowFlags.SetColorGrading(false);
988 ShowFlags.SetDepthOfField(false);
989 ShowFlags.SetDiffuse(false);
990 ShowFlags.SetDirectionalLights(false);
991 ShowFlags.SetDirectLighting(false);
992 ShowFlags.SetDynamicShadows(false);
993 ShowFlags.SetEyeAdaptation(false);
994 ShowFlags.SetFog(false);
995 ShowFlags.SetGlobalIllumination(false);
996 ShowFlags.SetGrain(false);
997 ShowFlags.SetHLODColoration(false);
998 ShowFlags.SetHMDDistortion(false);
999 ShowFlags.SetLensFlares(false);
1000 ShowFlags.SetLightComplexity(false);
1001 ShowFlags.SetLightFunctions(false);
1002 ShowFlags.SetLightInfluences(false);
1003 ShowFlags.SetLighting(false);
1004 ShowFlags.SetLightMapDensity(false);
1005 ShowFlags.SetLightRadius(false);
1006 ShowFlags.SetLightShafts(false);
1007 ShowFlags.SetLODColoration(false);
1008 ShowFlags.SetMotionBlur(false);
1009 ShowFlags.SetOnScreenDebug(false);
1010 ShowFlags.SetParticles(false);
1011 ShowFlags.SetPointLights(false);
1012 ShowFlags.SetRefraction(false);
1013 ShowFlags.SetSceneColorFringe(false);
1014 ShowFlags.SetScreenSpaceAO(false);
1015 ShowFlags.SetScreenSpaceReflections(false);
1016 ShowFlags.SetSkyLighting(false);
1017 ShowFlags.SetSpotLights(false);
1018 ShowFlags.SetStationaryLightOverlap(false);
1019 ShowFlags.SetSubsurfaceScattering(false);
1020 ShowFlags.SetTonemapper(false);
1021 ShowFlags.SetVisualizeBuffer(false);
1022 ShowFlags.SetVisualizeDistanceFieldAO(false);
1023 ShowFlags.SetVisualizeDOF(false);
1024 ShowFlags.SetVisualizeHDR(false);
1025 ShowFlags.SetVisualizeLightCulling(false);
1026 ShowFlags.SetVisualizeMeshDistanceFields(false);
1027 ShowFlags.SetVisualizeMotionBlur(false);
1028 ShowFlags.SetVisualizeOutOfBoundsPixels(false);
1029 ShowFlags.SetVisualizeSenses(false);
1030 ShowFlags.SetVisualizeShadingModels(false);
1031 ShowFlags.SetVisualizeSSR(false);
1032 ShowFlags.SetVisualizeSSS(false);
1033}
ESensorTypes
Definition: SensorTypes.h:15
@ InstanceSegmentationCamera
static auto BindTick(ObjectType *Object, FunctionType Function)
Definition: TickManager.h:162
void UpdateCameraStabilization()
Definition: Camera.cpp:529
bool UseParallelLateTick
Definition: Camera.h:235
void AddPostProcessingMaterial(const FString &Path, float Weight=1.0f)
Definition: Camera.cpp:62
int32 ImageNumber
Definition: Camera.h:390
FTickEntry TickEntry
Definition: Camera.h:303
TSharedPtr< FFrameGrabber > FrameGrabber
Definition: Camera.h:349
void ChangeCameraParameters(FCameraBaseParameters newParameters)
Definition: Camera.cpp:57
virtual ESensorTypes GetSensorType() const override
Definition: Camera.h:74
bool UseReadPixels
Definition: Camera.h:311
FCameraBaseParameters CameraParameters
Definition: Camera.h:365
void ApplyGammaCorrectionLUT(TArray< FColor > &Buffer)
Definition: Camera.cpp:670
FOnWindowClosed OnWindowClosedDelegate
Definition: Camera.h:378
void AddHiddenActor(AActor *Actor)
Definition: Camera.cpp:962
virtual void Init(FCameraBaseParameters parameters, bool SimulateSensor=true)
Definition: Camera.cpp:86
virtual void EndPlay(const EEndPlayReason::Type EndPlayReason) override
Definition: Camera.cpp:461
APIDDrone * DronePtr
Definition: Camera.h:314
USceneCaptureComponent2D * CaptureComponent2D
Definition: Camera.h:292
void RemovePostProcessingMaterial(UMaterial *Material)
Definition: Camera.cpp:77
void SaveCameraMetaDataToDisk(const FString &ImageName)
Definition: Camera.cpp:866
AGeoReferencingSystem * GeoReferencingSystem
Definition: Camera.h:374
float FrameRateTimer
Definition: Camera.h:388
void SaveImageToDisk(const TArray< FColor > FrameBuffer, int32 Width, int32 Height)
Definition: Camera.cpp:825
void ReadPixelsCapture()
Definition: Camera.cpp:627
FDelegateHandle OnPostTickDelegate
Definition: Camera.h:242
bool ShouldSimulateCamera
Definition: Camera.h:385
void PixelReadEndOfFrame(UWorld *World, ELevelTick TickType, float DeltaSeconds)
Definition: Camera.cpp:550
void SetShadowRendering(bool RenderShadows)
Definition: Camera.cpp:930
void OnWindowClosed(const TSharedRef< SWindow > &Window)
Definition: Camera.cpp:901
void StartFrameGrabber()
Definition: Camera.cpp:713
void FrameGrabberCapture()
Definition: Camera.cpp:587
void SendImageDataToROS(const TArray< FColor > &FrameBuffer, int32 Width, int32 Height)
Definition: Camera.cpp:761
FDelegateHandle OnPreTickDelegate
Definition: Camera.h:237
UTextureRenderTarget2D * CaptureRenderTarget
Definition: Camera.h:289
void AddWidgetToWindow(UWidget *WidgetToAdd)
Definition: Camera.cpp:914
TSharedPtr< ROSMessages::sensor_msgs::Image > ImageMsg
Definition: Camera.h:309
int32 CurrentBufferSize
Definition: Camera.h:394
FRotator SensorSetRotation
Definition: Camera.h:315
bool SaveCurrentFrameToDiskRequested
Definition: Camera.h:383
void SetTemporalAA(bool SetTemporal)
Definition: Camera.cpp:941
void CreateLogFile() override
Definition: Camera.cpp:93
UTexture2D * CaptureFrameTexture
Definition: Camera.h:299
void SetupCamera(FCameraBaseParameters parameters)
Definition: Camera.cpp:129
TWeakObjectPtr< UMaterial > PhysicLensDistortion
Definition: Camera.h:307
void RemoveWidgetFromWindow(UWidget *WidgetToRemove)
Definition: Camera.cpp:922
FCameraDelegate_OnWindowResized OnCameraWindowResized
Definition: Camera.h:148
void HidePrimitiveComponent(UPrimitiveComponent *PrimitiveComponent)
Definition: Camera.cpp:705
TEnumAsByte< ETextureRenderTargetFormat > TextureFormat
Definition: Camera.h:134
void ReleaseFrameGrabber()
Definition: Camera.cpp:744
void EndOfFrameParellel(float DeltaTime)
Definition: Camera.cpp:555
virtual void AddProcessingToFrameBuffer(TArray< FColor > &buffer)
Definition: Camera.cpp:665
void ResizeCamera(int32 Width=1280, int32 Height=720)
Definition: Camera.cpp:950
uint8 * BGR8Buffer
Definition: Camera.h:392
TSharedPtr< FUnrealWindow > UnrealWindow
Definition: Camera.h:301
void EndOfFrame(UWorld *World, ELevelTick TickType, float DeltaSeconds)
Definition: Camera.cpp:545
float CameraFrameRate
Definition: Camera.h:387
ACamera(const FObjectInitializer &ObjectInitializer)
Definition: Camera.cpp:34
int32 ImageHeight
Definition: Camera.h:381
void PreActorTick(UWorld *World, ELevelTick TickType, float DeltaSeconds)
Definition: Camera.cpp:509
FCameraDelegate_OnWindowClosed OnCameraWindowClosed
Definition: Camera.h:141
void DisableShowFlags()
Definition: Camera.cpp:970
virtual void BeginPlay() override
Definition: Camera.cpp:405
TWeakObjectPtr< UMaterialInstanceDynamic > IceMaterialInstance
Definition: Camera.h:305
int32 ImageWidth
Definition: Camera.h:380
bool ShouldSimulate(const float DeltaSeconds)
Definition: Camera.cpp:560
bool AllowGimbal
Definition: Camera.h:316
bool CanSimulateSensor() const
Definition: Sensor.h:170
static FPrimitiveAdded OnPrimitiveAdded
Definition: Sensor.h:377
FString GetSensorIdentifier() const
Definition: Sensor.h:75
virtual void CreateDataSavePath()
Definition: Sensor.cpp:254
FString GetSensorName() const
Definition: Sensor.h:96
void SetSimulateSensor(bool SimulateSensor)
Definition: Sensor.h:160
UTopic * GetROSTopic() const
Definition: Sensor.h:150
ULogFile * LogFile
Definition: Sensor.h:367
bool SendDataToROS
Definition: Sensor.h:364
static TArray< TWeakObjectPtr< UPrimitiveComponent > > GetComponentsToHide()
Definition: Sensor.h:270
FString CreateTimeStampString() const
Definition: Sensor.cpp:330
FString FileSavePath
Definition: Sensor.h:372
void WriteToLogFile(const FString &Message)
Definition: Sensor.cpp:302
FORCEINLINE bool IsROSConnected() const
Definition: Sensor.h:201
virtual void CreateROSTopic()
Definition: Sensor.cpp:197
static void RemoveTick(FTickEntry TickEntry)
Definition: TickManager.cpp:80
static FTickEntry AddTick(UObject *Object, std::function< void(float)> Function, ETickType Type)
Definition: TickManager.cpp:51
bool IsVehicleInGarage() const
Definition: Vehicle.h:133
static void Log(const FString &Message, bool LogToTextFile=true, bool LogToROS=true)
static bool ShouldStartWindowMinimized()
virtual void Setup(ACamera *CameraRef, bool ShowOnStartup=true)
static FGeographicCoordinates UnrealToGeographicCoordinates(AGeoReferencingSystem *GeoReferencingSystem, const FVector &Position)
void Create(const FString &FileNameWithoutExtension, FLogFileSettings Settings)
Definition: LogFile.cpp:40
bool KeepFileOpen
Definition: LogFile.h:42
bool Timestamp
Definition: LogFile.h:39
FString FilePath
Definition: LogFile.h:54
FFileWriteOptions FileWriteOptions
Definition: LogFile.h:45
int32 QueueLength
Definition: LogFile.h:48
bool OverrideFilePath
Definition: LogFile.h:51
FFileCreationOptions FileCreationOptions
Definition: LogFile.h:36