diff --git a/K4AdotNet.Samples.Wpf.BackgroundRemover/FrameData.cs b/K4AdotNet.Samples.Wpf.BackgroundRemover/FrameData.cs index e3efdb6..bb75c88 100644 --- a/K4AdotNet.Samples.Wpf.BackgroundRemover/FrameData.cs +++ b/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.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.Shared.Return(bgraBuffer); + } + } + + return colorFrame.DuplicateReference(); + } } } diff --git a/K4AdotNet.Samples.Wpf.BackgroundRemover/ProcessingViewModel.cs b/K4AdotNet.Samples.Wpf.BackgroundRemover/ProcessingViewModel.cs index db6519d..560f2cc 100644 --- a/K4AdotNet.Samples.Wpf.BackgroundRemover/ProcessingViewModel.cs +++ b/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); } } diff --git a/K4AdotNet.Samples.Wpf.BackgroundRemover/Processor.cs b/K4AdotNet.Samples.Wpf.BackgroundRemover/Processor.cs index 2757b66..490bbe7 100644 --- a/K4AdotNet.Samples.Wpf.BackgroundRemover/Processor.cs +++ b/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) diff --git a/K4AdotNet.Samples.Wpf.BodyTracker/TrackerModel.cs b/K4AdotNet.Samples.Wpf.BodyTracker/TrackerModel.cs index 0a8b3dd..6d64175 100644 --- a/K4AdotNet.Samples.Wpf.BodyTracker/TrackerModel.cs +++ b/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; diff --git a/K4AdotNet.Samples.Wpf.Common/BackgroundReadingLoop.cs b/K4AdotNet.Samples.Wpf.Common/BackgroundReadingLoop.cs index d7f6bfc..4ba9fe1 100644 --- a/K4AdotNet.Samples.Wpf.Common/BackgroundReadingLoop.cs +++ b/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, diff --git a/K4AdotNet.Samples.Wpf.Common/ImageConverters.cs b/K4AdotNet.Samples.Wpf.Common/ImageConverters.cs new file mode 100644 index 0000000..07792d1 --- /dev/null +++ b/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 +{ + /// Helper methods to work with images. + public static class ImageConverters + { + private static readonly DecoderOptions decoderOptions = new(); + + /// Decodes MJPEG image to the destination BGRA image buffer. + /// Source image in MJPEG format to be decoded. Not . + /// Destination buffer for a decoded image. Should be big enough for a result BGRA image (4 bytes per pixel). Not . + 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(decoderOptions, stream); + decodedImage.CopyPixelDataTo(dstBuffer); + } + } +} diff --git a/K4AdotNet.Samples.Wpf.BodyTracker/ImageVisualizer.cs b/K4AdotNet.Samples.Wpf.Common/ImageVisualizer.cs similarity index 73% rename from K4AdotNet.Samples.Wpf.BodyTracker/ImageVisualizer.cs rename to K4AdotNet.Samples.Wpf.Common/ImageVisualizer.cs index 92f723c..e1a14d6 100644 --- a/K4AdotNet.Samples.Wpf.BodyTracker/ImageVisualizer.cs +++ b/K4AdotNet.Samples.Wpf.Common/ImageVisualizer.cs @@ -7,9 +7,9 @@ using System.Windows.Threading; using K4AdotNet.Sensor; -namespace K4AdotNet.Samples.Wpf.BodyTracker +namespace K4AdotNet.Samples.Wpf.Common { - internal abstract class ImageVisualizer + public abstract class ImageVisualizer { public const int DefaultDpi = 96; @@ -19,6 +19,9 @@ public static ImageVisualizer CreateForColorBgra(Dispatcher dispatcher, int widt public static ImageVisualizer CreateForDepth(Dispatcher dispatcher, int widthPixels, int heightPixels, int dpi = DefaultDpi) => new DepthImageVisualizer(dispatcher, widthPixels, heightPixels, dpi); + public static ImageVisualizer CreateForIR(Dispatcher dispatcher, int widthPixels, int heightPixels, int dpi = DefaultDpi) + => new IRImageVisualizer(dispatcher, widthPixels, heightPixels, dpi); + protected ImageVisualizer(Dispatcher dispatcher, ImageFormat format, int widthPixels, int heightPixels, int strideBytes, int dpi) { if (dispatcher.Thread != Thread.CurrentThread) @@ -51,6 +54,13 @@ public byte NonBodyAlphaValue set => nonBodyAlphaValue = value; } + public int VisualizationParameter + { + get => visualizationParameter; + set => visualizationParameter = value; + } + private volatile int visualizationParameter; + /// /// Image with visualized frame. You can use this property in WPF controls/windows. /// @@ -61,18 +71,21 @@ public byte NonBodyAlphaValue /// /// Image received from Kinect Sensor SDK. Can be . /// - updated, - not updated (frame is not compatible, or old frame). - public bool Update(Image? image, Image? bodyIndexMap) + public bool Update(Image? image, Image? bodyIndexMap = null) { // Is compatible? - if (image == null - || image.WidthPixels != WidthPixels || image.HeightPixels != HeightPixels - || image.Format != Format) + if (image == null || image.WidthPixels != WidthPixels || image.HeightPixels != HeightPixels) { return false; } // 1st step: filling of inner buffer - FillInnerBuffer(image.Buffer, image.StrideBytes, image.SizeBytes); + if (image.Format == ImageFormat.ColorMjpg) + DecodeMjpegToInnerBuffer(image); // special patch for OrbbecSDK-K4A-Wrapper + else if (image.Format == Format) + FillInnerBuffer(image.Buffer, image.StrideBytes, image.SizeBytes); + else + return false; // not compatible format if (bodyIndexMap != null) FillInnerBodyIndexBuffer(bodyIndexMap.Buffer, bodyIndexMap.StrideBytes, bodyIndexMap.SizeBytes); @@ -86,6 +99,18 @@ public bool Update(Image? image, Image? bodyIndexMap) return true; } + private void DecodeMjpegToInnerBuffer(Image image) + { + // This method can be called from some background thread, + // thus use synchronization + lock (innerBuffer) + { + // Decode MJPEG to BGRA and copy result to innerBuffer + ImageConverters.DecodeMjpegToBgra(image, innerBuffer); + } + } + + private void FillInnerBuffer(IntPtr srcPtr, int srcStrideBytes, int srcSizeBytes) { // This method can be called from some background thread, @@ -151,6 +176,7 @@ private void FillWritableBitmap() var backBuffer = writeableBitmap.BackBuffer; var backBufferStride = writeableBitmap.BackBufferStride; var nonBodyAlphaValue = this.nonBodyAlphaValue; + var visParameter = visualizationParameter; // This method works in UI thread, and uses innerBuffer // that is filled in Update() method from some background thread @@ -161,7 +187,7 @@ private void FillWritableBitmap() for (var y = 0; y < HeightPixels; y++) FillWritableBitmapLine(y, backBuffer, backBufferStride, nonBodyAlphaValue); #else - Parallel.For(0, HeightPixels, y => FillWritableBitmapLine(y, backBuffer, backBufferStride, nonBodyAlphaValue)); + Parallel.For(0, HeightPixels, y => FillWritableBitmapLine(y, backBuffer, backBufferStride, nonBodyAlphaValue, visParameter)); #endif } @@ -174,14 +200,14 @@ private void FillWritableBitmap() } } - protected abstract void FillWritableBitmapLine(int y, IntPtr backBuffer, int backBufferStride, byte nonBodyAlphaValue); + protected abstract void FillWritableBitmapLine(int y, IntPtr backBuffer, int backBufferStride, byte nonBodyAlphaValue, int visParameter); protected readonly byte[] innerBuffer; protected readonly byte[] innerBodyIndexBuffer; protected readonly WriteableBitmap writeableBitmap; protected volatile byte nonBodyAlphaValue = byte.MaxValue; -#region BGRA + #region BGRA private sealed class ColorBgraImageVisualizer : ImageVisualizer { @@ -189,7 +215,7 @@ public ColorBgraImageVisualizer(Dispatcher dispatcher, int widthPixels, int heig : base(dispatcher, ImageFormat.ColorBgra32, widthPixels, heightPixels, ImageFormat.ColorBgra32.StrideBytes(widthPixels), dpi) { } - protected override unsafe void FillWritableBitmapLine(int y, IntPtr backBuffer, int backBufferStride, byte nonBodyAlphaValue) + protected override unsafe void FillWritableBitmapLine(int y, IntPtr backBuffer, int backBufferStride, byte nonBodyAlphaValue, int notUsed) { byte* dstPtr = (byte*)backBuffer + y * backBufferStride; fixed (void* innerBufferPtr = innerBuffer) @@ -214,17 +240,19 @@ protected override unsafe void FillWritableBitmapLine(int y, IntPtr backBuffer, } } -#endregion + #endregion -#region Depth + #region Depth private sealed class DepthImageVisualizer : ImageVisualizer { public DepthImageVisualizer(Dispatcher dispatcher, int widthPixels, int heightPixels, int dpi) : base(dispatcher, ImageFormat.Depth16, widthPixels, heightPixels, ImageFormat.Depth16.StrideBytes(widthPixels), dpi) - { } + { + VisualizationParameter = 10_000; + } - protected override unsafe void FillWritableBitmapLine(int y, IntPtr backBuffer, int backBufferStride, byte nonBodyAlphaValue) + protected override unsafe void FillWritableBitmapLine(int y, IntPtr backBuffer, int backBufferStride, byte nonBodyAlphaValue, int maxDistanceMm) { byte* dstPtr = (byte*)backBuffer + y * backBufferStride; fixed (void* innerBufferPtr = innerBuffer) @@ -235,12 +263,15 @@ protected override unsafe void FillWritableBitmapLine(int y, IntPtr backBuffer, for (var x = 0; x < WidthPixels; x++) { var v = (int)*(srcPtr++); + if (v > maxDistanceMm) + v = 0; + var bodyIndex = *(srcBodyIndexPtr++); - // Some random heuristic to colorize depth map slightly like height-based colorization of earth maps - // (from blue though green to red) if (bodyIndex == BodyTracking.BodyFrame.NotABodyIndexMapPixelValue) { + // Some random heuristic to colorize depth map slightly like height-based colorization of earth maps + // (from blue though green to red) v = v >> 3; *(dstPtr++) = (byte)(Math.Max(0, 220 - 3 * Math.Abs(150 - v) / 2)); *(dstPtr++) = (byte)(Math.Max(0, 220 - Math.Abs(350 - v))); @@ -259,6 +290,43 @@ protected override unsafe void FillWritableBitmapLine(int y, IntPtr backBuffer, } } -#endregion + #endregion + + #region IR + + private sealed class IRImageVisualizer : ImageVisualizer + { + public IRImageVisualizer(Dispatcher dispatcher, int widthPixels, int heightPixels, int dpi) + : base(dispatcher, ImageFormat.IR16, widthPixels, heightPixels, ImageFormat.IR16.StrideBytes(widthPixels), dpi) + { + VisualizationParameter = 1; + } + + protected override unsafe void FillWritableBitmapLine(int y, IntPtr backBuffer, int backBufferStride, byte nonBodyAlphaValue, int shiftBase) + { + shiftBase = Math.Max(0, Math.Min(4, shiftBase)); + byte* dstPtr = (byte*)backBuffer + y * backBufferStride; + fixed (void* innerBufferPtr = innerBuffer) + fixed (void* innerBodyIndexBufferPtr = innerBodyIndexBuffer) + { + ushort* srcPtr = (ushort*)innerBufferPtr + y * WidthPixels; + byte* srcBodyIndexPtr = (byte*)innerBodyIndexBufferPtr + y * WidthPixels; + for (var x = 0; x < WidthPixels; x++) + { + var v = *(srcPtr++); + var bodyIndex = *(srcBodyIndexPtr++); + + *(dstPtr++) = unchecked((byte)Math.Min(255, v >> (shiftBase + 4))); + *(dstPtr++) = unchecked((byte)Math.Min(255, v >> shiftBase)); + *(dstPtr++) = unchecked((byte)Math.Min(255, v >> (shiftBase + 3))); + *(dstPtr++) = bodyIndex == BodyTracking.BodyFrame.NotABodyIndexMapPixelValue + ? nonBodyAlphaValue + : byte.MaxValue; + } + } + } + } + + #endregion } } diff --git a/K4AdotNet.Samples.Wpf.Common/K4AdotNet.Samples.Wpf.Common.csproj b/K4AdotNet.Samples.Wpf.Common/K4AdotNet.Samples.Wpf.Common.csproj index e807d1f..9e14c01 100644 --- a/K4AdotNet.Samples.Wpf.Common/K4AdotNet.Samples.Wpf.Common.csproj +++ b/K4AdotNet.Samples.Wpf.Common/K4AdotNet.Samples.Wpf.Common.csproj @@ -11,8 +11,13 @@ K4AdotNet.Samples.Wpf.Common True + True + + + + diff --git a/K4AdotNet.Samples.Wpf.Viewer/ImageVisualizer.cs b/K4AdotNet.Samples.Wpf.Viewer/ImageVisualizer.cs deleted file mode 100644 index 9c1c388..0000000 --- a/K4AdotNet.Samples.Wpf.Viewer/ImageVisualizer.cs +++ /dev/null @@ -1,224 +0,0 @@ -using System; -using System.Runtime.InteropServices; -using System.Threading; -using System.Threading.Tasks; -using System.Windows.Media; -using System.Windows.Media.Imaging; -using System.Windows.Threading; -using K4AdotNet.Sensor; - -namespace K4AdotNet.Samples.Wpf.Viewer -{ - internal abstract class ImageVisualizer - { - public const int DefaultDpi = 96; - - public static ImageVisualizer CreateForColorBgra(Dispatcher dispatcher, int widthPixels, int heightPixels, int dpi = DefaultDpi) - => new ColorBgraImageVisualizer(dispatcher, widthPixels, heightPixels, dpi); - - public static ImageVisualizer CreateForDepth(Dispatcher dispatcher, int widthPixels, int heightPixels, int dpi = DefaultDpi) - => new DepthImageVisualizer(dispatcher, widthPixels, heightPixels, dpi); - - public static ImageVisualizer CreateForIR(Dispatcher dispatcher, int widthPixels, int heightPixels, int dpi = DefaultDpi) - => new IRImageVisualizer(dispatcher, widthPixels, heightPixels, dpi); - - protected ImageVisualizer(Dispatcher dispatcher, ImageFormat format, int widthPixels, int heightPixels, int strideBytes, int dpi) - { - if (dispatcher.Thread != Thread.CurrentThread) - { - throw new InvalidOperationException( - "Call this constructor from UI thread please, because it creates ImageSource object for UI"); - } - - Dispatcher = dispatcher; - Format = format; - WidthPixels = widthPixels; - HeightPixels = heightPixels; - StrideBytes = strideBytes; - - innerBuffer = new byte[strideBytes * heightPixels]; - writeableBitmap = new(widthPixels, heightPixels, dpi, dpi, PixelFormats.Bgra32, null); - } - - public Dispatcher Dispatcher { get; } - public ImageFormat Format { get; } - public int WidthPixels { get; } - public int HeightPixels { get; } - public int StrideBytes { get; } - - public int VisualizationParameter - { - get => visualizationParameter; - set => visualizationParameter = value; - } - private volatile int visualizationParameter; - - /// - /// Image with visualized frame. You can use this property in WPF controls/windows. - /// - public BitmapSource ImageSource => writeableBitmap; - - /// - /// Updates based on . - /// - /// Image received from Kinect Sensor SDK. Can be . - /// - updated, - not updated (frame is not compatible, or old frame). - public bool Update(Image? image) - { - // Is compatible? - if (image == null - || image.WidthPixels != WidthPixels || image.HeightPixels != HeightPixels - || image.Format != Format) - { - return false; - } - - // 1st step: filling of inner buffer - FillInnerBuffer(image.Buffer, image.StrideBytes, image.SizeBytes); - - // 2nd step: we can update WritableBitmap only from its owner thread (as a rule, UI thread) - Dispatcher.BeginInvoke(DispatcherPriority.Render, new Action(FillWritableBitmap)); - - // Updated - return true; - } - - private void FillInnerBuffer(IntPtr srcPtr, int srcStrideBytes, int srcSizeBytes) - { - // This method can be called from some background thread, - // thus use synchronization - lock (innerBuffer) - { - if (srcStrideBytes == StrideBytes && srcSizeBytes == innerBuffer.Length) - { - Marshal.Copy(srcPtr, innerBuffer, 0, innerBuffer.Length); - } - else - { - var lineLength = Math.Min(srcStrideBytes, StrideBytes); - var dstOffset = 0; - for (var y = 0; y < HeightPixels; y++) - { - Marshal.Copy(srcPtr, innerBuffer, dstOffset, lineLength); - srcPtr += srcSizeBytes; - dstOffset += StrideBytes; - } - } - } - } - - private void FillWritableBitmap() - { - writeableBitmap.Lock(); - try - { - var backBuffer = writeableBitmap.BackBuffer; - var backBufferStride = writeableBitmap.BackBufferStride; - var visParameter = visualizationParameter; - - // This method works in UI thread, and uses innerBuffer - // that is filled in Update() method from some background thread - lock (innerBuffer) - { - // We use parallelism here to speed up - Parallel.For(0, HeightPixels, y => FillWritableBitmapLine(y, backBuffer, backBufferStride, visParameter)); - } - - // Inform UI infrastructure that we have updated content of image - writeableBitmap.AddDirtyRect(new System.Windows.Int32Rect(0, 0, WidthPixels, HeightPixels)); - } - finally - { - writeableBitmap.Unlock(); - } - } - - protected abstract void FillWritableBitmapLine(int y, IntPtr backBuffer, int backBufferStride, int visParameter); - - protected readonly byte[] innerBuffer; - protected readonly WriteableBitmap writeableBitmap; - - #region BGRA - - private sealed class ColorBgraImageVisualizer : ImageVisualizer - { - public ColorBgraImageVisualizer(Dispatcher dispatcher, int widthPixels, int heightPixels, int dpi) - : base(dispatcher, ImageFormat.ColorBgra32, widthPixels, heightPixels, ImageFormat.ColorBgra32.StrideBytes(widthPixels), dpi) - { } - - protected override void FillWritableBitmapLine(int y, IntPtr backBuffer, int backBufferStride, int notUsed) - { - var lineLength = Math.Min(backBufferStride, StrideBytes); - Marshal.Copy(innerBuffer, y * StrideBytes, backBuffer + backBufferStride * y, lineLength); - } - } - - #endregion - - #region Depth - - private sealed class DepthImageVisualizer : ImageVisualizer - { - public DepthImageVisualizer(Dispatcher dispatcher, int widthPixels, int heightPixels, int dpi) - : base(dispatcher, ImageFormat.Depth16, widthPixels, heightPixels, ImageFormat.Depth16.StrideBytes(widthPixels), dpi) - { - VisualizationParameter = 10_000; // 10 m - } - - protected override unsafe void FillWritableBitmapLine(int y, IntPtr backBuffer, int backBufferStride, int maxDistanceMm) - { - byte* dstPtr = (byte*)backBuffer + y * backBufferStride; - fixed (void* innerBufferPtr = innerBuffer) - { - short* srcPtr = (short*)innerBufferPtr + y * WidthPixels; - for (var x = 0; x < WidthPixels; x++) - { - var v = (int)*(srcPtr++); - if (v > maxDistanceMm) - v = 0; - v = v >> 3; - // Some random heuristic to colorize depth map slightly like height-based colorization of earth maps - // (from blue though green to red) - *(dstPtr++) = (byte)(Math.Max(0, 220 - 3 * Math.Abs(150 - v) / 2)); - *(dstPtr++) = (byte)(Math.Max(0, 220 - Math.Abs(350 - v))); - *(dstPtr++) = (byte)(Math.Max(0, 220 - Math.Abs(550 - v))); - *(dstPtr++) = byte.MaxValue; - } - } - } - } - - #endregion - - #region IR - - private sealed class IRImageVisualizer : ImageVisualizer - { - public IRImageVisualizer(Dispatcher dispatcher, int widthPixels, int heightPixels, int dpi) - : base(dispatcher, ImageFormat.IR16, widthPixels, heightPixels, ImageFormat.IR16.StrideBytes(widthPixels), dpi) - { - VisualizationParameter = 1; - } - - protected override unsafe void FillWritableBitmapLine(int y, IntPtr backBuffer, int backBufferStride, int shiftBase) - { - shiftBase = Math.Max(0, Math.Min(4, shiftBase)); - byte* dstPtr = (byte*)backBuffer + y * backBufferStride; - fixed (void* innerBufferPtr = innerBuffer) - { - ushort* srcPtr = (ushort*)innerBufferPtr + y * WidthPixels; - for (var x = 0; x < WidthPixels; x++) - { - var v = *(srcPtr++); - *(dstPtr++) = unchecked((byte)Math.Min(255, v >> (shiftBase + 4))); - *(dstPtr++) = unchecked((byte)Math.Min(255, v >> shiftBase)); - *(dstPtr++) = unchecked((byte)Math.Min(255, v >> (shiftBase + 3))); - *(dstPtr++) = byte.MaxValue; - } - } - } - } - - #endregion - } -} diff --git a/K4AdotNet.Samples.Wpf.Viewer/ViewerModel.cs b/K4AdotNet.Samples.Wpf.Viewer/ViewerModel.cs index 32d169f..d74e972 100644 --- a/K4AdotNet.Samples.Wpf.Viewer/ViewerModel.cs +++ b/K4AdotNet.Samples.Wpf.Viewer/ViewerModel.cs @@ -1,4 +1,5 @@ -using K4AdotNet.Sensor; +using K4AdotNet.Samples.Wpf.Common; +using K4AdotNet.Sensor; using System; using System.Windows; using System.Windows.Media.Imaging; @@ -196,7 +197,10 @@ public double DepthMaxVisibleDistance { if (DepthMaxVisibleDistance != value && depthImageVisualizer != null) { - depthImageVisualizer.VisualizationParameter = (int)(value * 1000); + depthImageVisualizer.VisualizationParameter + = (int)(value * 1000); + if (depthOverColorImageVisualizer != null) + depthOverColorImageVisualizer.VisualizationParameter = depthImageVisualizer.VisualizationParameter; RaisePropertyChanged(nameof(DepthMaxVisibleDistance)); } } diff --git a/K4AdotNet/Sensor/Image.cs b/K4AdotNet/Sensor/Image.cs index a3030e1..56e59ed 100644 --- a/K4AdotNet/Sensor/Image.cs +++ b/K4AdotNet/Sensor/Image.cs @@ -517,9 +517,9 @@ public void FillFrom(byte[] src) { if (src is null) throw new ArgumentNullException(nameof(src)); - if (src.Length != SizeBytes) + if (src.Length < SizeBytes) throw new ArgumentOutOfRangeException(nameof(src) + "." + nameof(src.Length)); - Marshal.Copy(src, 0, Buffer, src.Length); + Marshal.Copy(src, 0, Buffer, SizeBytes); } /// Fills data in image buffer from specified managed array. @@ -535,9 +535,10 @@ public void FillFrom(short[] src) var elementSize = sizeof(short); if (size % elementSize != 0) throw new InvalidOperationException($"Size of image {size} is not divisible by element size {elementSize}."); - if (src.Length * elementSize != size) + size /= elementSize; + if (src.Length < size) throw new ArgumentOutOfRangeException(nameof(src) + "." + nameof(src.Length)); - Marshal.Copy(src, 0, Buffer, src.Length); + Marshal.Copy(src, 0, Buffer, size); } /// Fills data in image buffer from specified managed array. @@ -553,9 +554,10 @@ public void FillFrom(float[] src) var elementSize = sizeof(float); if (size % elementSize != 0) throw new InvalidOperationException($"Size of image {size} is not divisible by element size {elementSize}."); - if (src.Length * elementSize != size) + size /= elementSize; + if (src.Length < size) throw new ArgumentOutOfRangeException(nameof(src) + "." + nameof(src.Length)); - Marshal.Copy(src, 0, Buffer, src.Length); + Marshal.Copy(src, 0, Buffer, size); } /// Fills data in image buffer from specified managed array. @@ -571,9 +573,10 @@ public void FillFrom(int[] src) var elementSize = sizeof(int); if (size % elementSize != 0) throw new InvalidOperationException($"Size of image {size} is not divisible by element size {elementSize}."); - if (src.Length * elementSize != size) + size /= elementSize; + if (src.Length < size) throw new ArgumentOutOfRangeException(nameof(src) + "." + nameof(src.Length)); - Marshal.Copy(src, 0, Buffer, src.Length); + Marshal.Copy(src, 0, Buffer, size); } /// Extracts handle from . @@ -637,7 +640,7 @@ public override string ToString() #endregion -#region Memory management + #region Memory management // This field is required to keep callback delegate in memory private static readonly NativeApi.MemoryDestroyCallback unmanagedBufferReleaseCallback @@ -700,6 +703,6 @@ private static void ReleasePinnedMemory(IntPtr _, IntPtr context) #endif -#endregion + #endregion } }