Adaptations in samples to work correctly with Orbbec Femto Bolt devic…

…es via OrbbecSDK-K4A-Wrapper.

K4AdotNet.Samples.Wpf.BackgroundRemover/FrameData.cs

@@ -1,5 +1,7 @@
-using K4AdotNet.Sensor;
+using K4AdotNet.Samples.Wpf.Common;
+using K4AdotNet.Sensor;
 using System;
+using System.Buffers;
 
 namespace K4AdotNet.Samples.Wpf.BackgroundRemover
 {
@@ -13,8 +15,8 @@ internal class FrameData : IDisposable
     {
         public FrameData(Image colorFrame, Image depthFrame)
         {
-            ColorFrame = colorFrame ?? throw new ArgumentNullException(nameof(colorFrame));
-            DepthFrame = depthFrame ?? throw new ArgumentNullException(nameof(depthFrame));
+            ColorFrame = DecodeColorImageIfNeeded(colorFrame) ?? throw new ArgumentNullException(nameof(colorFrame));
+            DepthFrame = depthFrame?.DuplicateReference() ?? throw new ArgumentNullException(nameof(depthFrame));
         }
 
         public void Dispose()
@@ -25,5 +27,30 @@ public void Dispose()
 
         public Image ColorFrame { get; }
         public Image DepthFrame { get; }
+
+        private static Image? DecodeColorImageIfNeeded(Image? colorFrame)
+        {
+            if (colorFrame == null)
+                return null;
+
+            if (colorFrame.Format == ImageFormat.ColorMjpg)
+            {
+                var bgraImageSize = ImageFormat.ColorBgra32.StrideBytes(colorFrame.WidthPixels) * colorFrame.HeightPixels;
+                var bgraBuffer = ArrayPool<byte>.Shared.Rent(bgraImageSize);
+                try
+                {
+                    ImageConverters.DecodeMjpegToBgra(colorFrame, bgraBuffer);
+                    var res = new Image(ImageFormat.ColorBgra32, colorFrame.WidthPixels, colorFrame.HeightPixels);
+                    res.FillFrom(bgraBuffer);
+                    return res;
+                }
+                finally
+                {
+                    ArrayPool<byte>.Shared.Return(bgraBuffer);
+                }
+            }
+
+            return colorFrame.DuplicateReference();
+        }
     }
 }

K4AdotNet.Samples.Wpf.BackgroundRemover/ProcessingViewModel.cs

@@ -104,7 +104,7 @@ private void ReadingLoop_CaptureReady(object? sender, CaptureReadyEventArgs e)
                 if (e.Capture != null && e.Capture.ColorImage != null && e.Capture.DepthImage != null)
                 {
                     transformation.DepthImageToColorCamera(e.Capture.DepthImage, depthImage);
-                    var frameData = new FrameData(e.Capture.ColorImage.DuplicateReference(), depthImage.DuplicateReference());
+                    var frameData = new FrameData(e.Capture.ColorImage, depthImage);
                     processor.Enqueue(frameData);
                 }
             }

K4AdotNet.Samples.Wpf.BackgroundRemover/Processor.cs

@@ -53,10 +53,8 @@ public bool UnknownDepthIsBackground
         }
         private volatile bool _unknownDepthIsBackground;
 
-
         public event EventHandler? ImageUpdated;
 
-
         public void Start()
         {
             lock (sync)

K4AdotNet.Samples.Wpf.BodyTracker/TrackerModel.cs

@@ -1,4 +1,5 @@
 using K4AdotNet.BodyTracking;
+using K4AdotNet.Samples.Wpf.Common;
 using K4AdotNet.Sensor;
 using System;
 using System.Windows;

K4AdotNet.Samples.Wpf.Common/BackgroundReadingLoop.cs

@@ -8,11 +8,25 @@ namespace K4AdotNet.Samples.Wpf
 {
     public abstract class BackgroundReadingLoop : IDisposable
     {
-        public static BackgroundReadingLoop CreateForPlayback(string filePath, bool disableColor, bool disableDepth, bool doNotPlayFasterThanOriginalFps)
-            => new PlaybackReadingLoop(filePath, disableColor, disableDepth, doNotPlayFasterThanOriginalFps);
-
-        public static BackgroundReadingLoop CreateForDevice(Device device, DepthMode depthMode, ColorResolution colorResolution, FrameRate frameRate)
-            => new DeviceReadingLoop(device, depthMode, colorResolution, frameRate);
+        public const ImageFormat DefaultColorFormat =
+#if ORBBECSDK_K4A_WRAPPER
+            ImageFormat.ColorMjpg;          // It looks like that OrbbecSDK-K4A-Wrapper does not support BGRA
+#else
+            ImageFormat.ColorBgra32;        // Allow Azure Kinect SDK to convert color images to BGRA internally
+#endif
+
+        public static BackgroundReadingLoop CreateForPlayback(
+            string filePath,
+            bool disableColor, bool disableDepth,
+            bool doNotPlayFasterThanOriginalFps,
+            ImageFormat colorFormat = DefaultColorFormat)
+            => new PlaybackReadingLoop(filePath, disableColor, disableDepth, doNotPlayFasterThanOriginalFps, colorFormat);
+
+        public static BackgroundReadingLoop CreateForDevice(
+            Device device,
+            DepthMode depthMode, ColorResolution colorResolution, FrameRate frameRate,
+            ImageFormat colorFormat = DefaultColorFormat)
+            => new DeviceReadingLoop(device, depthMode, colorResolution, frameRate, colorFormat);
 
         protected readonly Thread backgroundThread;
         protected volatile bool isRunning;
@@ -22,6 +36,8 @@ protected BackgroundReadingLoop()
 
         public abstract ColorResolution ColorResolution { get; }
 
+        public abstract ImageFormat ColorFormat { get; }
+
         public abstract DepthMode DepthMode { get; }
 
         public virtual void Dispose()
@@ -58,7 +74,11 @@ private sealed class PlaybackReadingLoop : BackgroundReadingLoop
             private readonly Playback playback;
             private readonly int frameRateHz;
 
-            public PlaybackReadingLoop(string filePath, bool disableColor, bool disableDepth, bool doNotPlayFasterThanOriginalFps)
+            public PlaybackReadingLoop(
+                string filePath,
+                bool disableColor, bool disableDepth,
+                bool doNotPlayFasterThanOriginalFps,
+                ImageFormat colorFormat)
             {
                 this.doNotPlayFasterThanOriginalFps = doNotPlayFasterThanOriginalFps;
 
@@ -70,9 +90,10 @@ public PlaybackReadingLoop(string filePath, bool disableColor, bool disableDepth
                     ? (config.DepthMode == DepthMode.PassiveIR || config.DepthMode == DepthMode.WideViewUnbinned)
                         ? DepthMode.PassiveIR : DepthMode.Off
                     : config.DepthMode;
+                ColorFormat = colorFormat;
 
                 if (ColorResolution != ColorResolution.Off)
-                    playback.SetColorConversion(ImageFormat.ColorBgra32);   // NB! This results in very-very expensive operation during data extraction!..
+                    playback.SetColorConversion(colorFormat);
             }
 
             public override void Dispose()
@@ -83,6 +104,8 @@ public override void Dispose()
 
             public override ColorResolution ColorResolution { get; }
 
+            public override ImageFormat ColorFormat { get; }
+
             public override DepthMode DepthMode { get; }
 
             public override void GetCalibration(out Calibration calibration)
@@ -142,12 +165,16 @@ private sealed class DeviceReadingLoop : BackgroundReadingLoop
         {
             private readonly Device device;
 
-            public DeviceReadingLoop(Device device, DepthMode depthMode, ColorResolution colorResolution, FrameRate frameRate)
+            public DeviceReadingLoop(
+                Device device,
+                DepthMode depthMode, ColorResolution colorResolution, FrameRate frameRate,
+                ImageFormat colorFormat)
             {
                 this.device = device;
                 DepthMode = depthMode;
                 ColorResolution = colorResolution;
                 FrameRate = frameRate;
+                ColorFormat = colorFormat;
             }
 
             public override void Dispose()
@@ -162,6 +189,8 @@ public override void Dispose()
 
             public FrameRate FrameRate { get; }
 
+            public override ImageFormat ColorFormat { get; }
+
             public override void GetCalibration(out Calibration calibration)
                 => device.GetCalibration(DepthMode, ColorResolution, out calibration);
 
@@ -175,7 +204,7 @@ protected override void BackgroundLoop()
                     device.StartCameras(new DeviceConfiguration
                     {
                         CameraFps = FrameRate,
-                        ColorFormat = ImageFormat.ColorBgra32,
+                        ColorFormat = ColorFormat,
                         ColorResolution = ColorResolution,
                         DepthMode = DepthMode,
                         WiredSyncMode = WiredSyncMode.Standalone,

K4AdotNet.Samples.Wpf.Common/ImageConverters.cs

@@ -0,0 +1,27 @@
+using K4AdotNet.Sensor;
+using SixLabors.ImageSharp.Formats;
+using SixLabors.ImageSharp.Formats.Jpeg;
+using SixLabors.ImageSharp.PixelFormats;
+using System.Diagnostics;
+using System.IO;
+
+namespace K4AdotNet.Samples.Wpf.Common
+{
+    /// <summary>Helper methods to work with images.</summary>
+    public static class ImageConverters
+    {
+        private static readonly DecoderOptions decoderOptions = new();
+
+        /// <summary>Decodes MJPEG image to the destination BGRA image buffer.</summary>
+        /// <param name="mjpegImage">Source image in MJPEG format to be decoded. Not <see langword="null"/>.</param>
+        /// <param name="dstBuffer">Destination buffer for a decoded image. Should be big enough for a result BGRA image (4 bytes per pixel). Not <see langword="null"/>.</param>
+        public static unsafe void DecodeMjpegToBgra(Image mjpegImage, byte[] dstBuffer)
+        {
+            Debug.Assert(mjpegImage.Format == ImageFormat.ColorMjpg);
+
+            using var stream = new UnmanagedMemoryStream((byte*)mjpegImage.Buffer.ToPointer(), mjpegImage.SizeBytes);
+            using var decodedImage = JpegDecoder.Instance.Decode<Bgra32>(decoderOptions, stream);
+            decodedImage.CopyPixelDataTo(dstBuffer);
+        }
+    }
+}