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