From af828a407d4d8dddf54fa6f4a7deebf6aeeb6efe Mon Sep 17 00:00:00 2001 From: Alek-ban Date: Fri, 16 Jan 2026 09:44:00 +0300 Subject: [PATCH] feat: image caprure with averaging --- GSS2.Core/Hardware/CameraService.cs | 187 +++++++++++------- .../BGGR_PISP_COMP1_RAW_FrameDecoder.cs | 22 ++- 2 files changed, 132 insertions(+), 77 deletions(-) diff --git a/GSS2.Core/Hardware/CameraService.cs b/GSS2.Core/Hardware/CameraService.cs index f5d876c..7ced96f 100644 --- a/GSS2.Core/Hardware/CameraService.cs +++ b/GSS2.Core/Hardware/CameraService.cs @@ -34,6 +34,7 @@ public class CameraService : IDisposable ViewFinderWidth = 1920, ViewFinderHeight = 1080, ViewFinderFps = 30, + ImageCaptureFrameBufferCount = 2, ImageCaptureWidth = 4056, ImageCaptureHeight = 3080, }; @@ -129,10 +130,10 @@ public class CameraService : IDisposable CameraSelect(effectiveCancellationToken); CameraAcquire(effectiveCancellationToken); CameraConfigure(effectiveCancellationToken); - // ViewFinderFrameBuffersAllocate(effectiveCancellationToken); + ViewFinderFrameBuffersAllocate(effectiveCancellationToken); ImageCaptureFrameBuffersAllocate(effectiveCancellationToken); CameraStart(effectiveCancellationToken); - // ViewFinderFrameBuffersInitialize(effectiveCancellationToken); + ViewFinderFrameBuffersInitialize(effectiveCancellationToken); ImageCaptureFrameBuffersInitialize(effectiveCancellationToken); ImageCaptureFrameBuffersMap(effectiveCancellationToken); } @@ -221,7 +222,7 @@ public class CameraService : IDisposable checked((uint)_configuration.ImageCaptureWidth), checked((uint)_configuration.ImageCaptureHeight) ); - + if (_cameraConfiguration.Validate() == CameraConfiguration.Status.Invalid) { _logger.LogCritical("Error while camera configuration validation"); @@ -457,8 +458,10 @@ public class CameraService : IDisposable _state |= State.ImageCaptureFrameBuffersMapped; } - public async Task CaptureImage(CancellationToken cancellationToken) + public async Task CaptureImage(CancellationToken cancellationToken, int framesCount = 1) { + ArgumentOutOfRangeException.ThrowIfLessThan(framesCount, 1, nameof(framesCount)); + if (_camera is null) throw new InvalidOperationException("Camera not initialized"); if (_imageCaptureStream is null) @@ -474,87 +477,125 @@ public class CameraService : IDisposable .CreateLinkedTokenSource(_disposeCts.Token, cancellationToken) .Token; + _logger.LogDebug("Capturing {} frame(s)", framesCount); + OpenCvSharp.Mat? image = null; - var fdsData = new Dictionary(); var planesData = new List(); + var fdsData = new Dictionary(); + try { - effectiveCancellationToken.ThrowIfCancellationRequested(); - - var request = _camera.CreateRequest(checked((ulong)DateTimeOffset.UtcNow.ToUnixTimeMilliseconds())); - var buffers = _imageCaptureFrameBufferAllocator.Buffers(_imageCaptureStream); - var buffer = buffers.First(); - var result = request.AddBuffer(_imageCaptureStream, buffer); - if (result < 0) - throw new Exception("Error while request create"); - - effectiveCancellationToken.ThrowIfCancellationRequested(); - - var cts = new TaskCompletionSource(); - void RequestCompleted(object? sender, Camera.RequestCompletedEventArgs e) + while (planesData.Count() < framesCount) { - if (e.Request.Handle != request.Handle) - return; + effectiveCancellationToken.ThrowIfCancellationRequested(); + _logger.LogDebug("Requested {} frame(s), now collected {}", framesCount, planesData.Count()); + if (File.Exists("/proc/self/status")) + _logger.LogDebug("Current RAM Usage: {}", File.ReadAllLines("/proc/self/status").FirstOrDefault(l => l.StartsWith("RssAnon:"))?.Substring("RssAnon:".Length)?.Trim() ?? "UNKNOWN"); - var bufferStatus = request.FindBuffer(_imageCaptureStream).Metadata.Status; - _logger.LogDebug("Request buffers status: {}", bufferStatus); - if (bufferStatus == FrameMetadata.StatusEnum.FrameError || bufferStatus == FrameMetadata.StatusEnum.FrameStartup) + var oldPlanesDataCount = planesData.Count(); + + var buffers = _imageCaptureFrameBufferAllocator.Buffers(_imageCaptureStream); + if (buffers.Count() > framesCount - planesData.Count()) + buffers = buffers.Take(framesCount - planesData.Count()); + _logger.LogDebug("Taken {} buffer(s)", buffers.Count()); + + var requests = new List(); + + foreach (var buffer in buffers) { - request.Reuse(Request.ReuseFlagEnum.ReuseBuffers); - result = _camera.QueueRequest(request); - if (result >= 0) - return; + var request = _camera.CreateRequest(checked((ulong)DateTimeOffset.UtcNow.ToUnixTimeMilliseconds())); + var result = request.AddBuffer(_imageCaptureStream, buffer); + if (result < 0) + throw new Exception("Error while request create"); + requests.Add(request); } - cts.SetResult(); - } + _logger.LogDebug("Request(s) created"); - _camera.RequestCompleted += RequestCompleted; - result = _camera.QueueRequest(request); - if (result < 0) - { + effectiveCancellationToken.ThrowIfCancellationRequested(); + + var cts = new TaskCompletionSource(); + void RequestCompleted(object? sender, Camera.RequestCompletedEventArgs e) + { + if (!requests.Any(r => r.Handle == e.Request.Handle)) + return; + + var buffer = e.Request.FindBuffer(_imageCaptureStream); + if (buffer.Metadata.Status == FrameMetadata.StatusEnum.FrameError || + buffer.Metadata.Status == FrameMetadata.StatusEnum.FrameStartup) + { + _logger.LogDebug("Request buffers status: {}", buffer.Metadata.Status); + e.Request.Reuse(Request.ReuseFlagEnum.ReuseBuffers); + var result = _camera.QueueRequest(e.Request); + if (result >= 0) + return; + } + + _logger.LogDebug("Request complited: \n\tStatus: {}\n\tCookie: {}\n\tSequence: {}\n\tHasPendingBuffers: {}", e.Request.Status, e.Request.Cookie, e.Request.Sequence, e.Request.HasPendingBuffers); + _logger.LogDebug("Request buffer: \n\tStatus: {} \n\tCookie: {} \n\tSequence: {} \n\tTimestamp: {}", buffer.Metadata.Status, buffer.Cookie, buffer.Metadata.Sequence, buffer.Metadata.Timestamp); + _logger.LogDebug("Request buffer planes: "); + foreach (var (i, plane) in buffer.Metadata.Planes.Enumerate()) + _logger.LogDebug("Plane[{}]: bytes used: 0x{:X}", i, plane.BytesUsed); + foreach (var (i, plane) in buffer.Planes.Enumerate()) + _logger.LogDebug("Plane[{}]: \n\tFd: 0x{:X} \n\tOffset: 0x{:X} \n\tLength: 0x{:X} \n\tKInvalidOffset: 0x{:X}", i, plane.Fd.Get(), plane.Offset, plane.Length, plane.KInvalidOffset); + + effectiveCancellationToken.ThrowIfCancellationRequested(); + + foreach (var plane in buffer.Planes) + { + var fd = plane.Fd.Get(); + if (fdsData.ContainsKey(fd)) + continue; + var mappedBuffer = _imageCaptureMappedBuffers.Find(b => b.Fd == fd); + if (mappedBuffer is null) + continue; + var fdData = new byte[mappedBuffer.Length]; + Marshal.Copy(mappedBuffer.Address, fdData, 0, checked((int)mappedBuffer.Length)); + fdsData.Add(fd, fdData); + } + + foreach (var plane in buffer.Planes) + { + var fd = plane.Fd.Get(); + if (!fdsData.ContainsKey(fd)) + continue; + var fdData = fdsData[fd]; + var planeData = fdData + .Skip(checked((int)plane.Offset)) + .Take(checked((int)plane.Length)) + .ToArray(); + planesData.Add(planeData); + } + + if (planesData.Count() - oldPlanesDataCount == requests.Count()) + cts.SetResult(); + } + + _camera.RequestCompleted += RequestCompleted; + foreach (var request in requests) + { + var result = _camera.QueueRequest(request); + if (result < 0) + { + _camera.RequestCompleted -= RequestCompleted; + throw new Exception("Error while request queue"); + } + } + _logger.LogDebug("Request(s) queued"); + await cts.Task.WaitAsync(effectiveCancellationToken); _camera.RequestCompleted -= RequestCompleted; - throw new Exception("Error while request queue"); - } - await cts.Task.WaitAsync(effectiveCancellationToken); - _camera.RequestCompleted -= RequestCompleted; - buffer = request.FindBuffer(_imageCaptureStream); - _logger.LogDebug("Request complited: \n\tResult: {} \n\tStatus: {}\n\tCookie: {}\n\tSequence: {}\n\tHasPendingBuffers: {}", result, request.Status, request.Cookie, request.Sequence, request.HasPendingBuffers); - _logger.LogDebug("Request buffer: \n\tStatus: {} \n\tCookie: {} \n\tSequence: {} \n\tTimestamp: {}", buffer.Metadata.Status, buffer.Cookie, buffer.Metadata.Sequence, buffer.Metadata.Timestamp); - _logger.LogDebug("Request buffer planes: "); - foreach (var (i, plane) in buffer.Metadata.Planes.Enumerate()) - _logger.LogDebug("Plane[{}]: bytes used: 0x{:X}", i, plane.BytesUsed); - foreach (var (i, plane) in buffer.Planes.Enumerate()) - _logger.LogDebug("Plane[{}]: \n\tFd: 0x{:X} \n\tOffset: 0x{:X} \n\tLength: 0x{:X} \n\tKInvalidOffset: 0x{:X}", i, plane.Fd.Get(), plane.Offset, plane.Length, plane.KInvalidOffset); - - effectiveCancellationToken.ThrowIfCancellationRequested(); - - foreach (var plane in buffer.Planes) - { - var fd = plane.Fd.Get(); - if (fdsData.ContainsKey(fd)) - continue; - var mappedBuffer = _imageCaptureMappedBuffers.Find(b => b.Fd == fd); - if (mappedBuffer is null) - continue; - var fdData = new byte[mappedBuffer.Length]; - Marshal.Copy(mappedBuffer.Address, fdData, 0, checked((int)mappedBuffer.Length)); - fdsData.Add(fd, fdData); + foreach (var key in fdsData.Keys) +#pragma warning disable CS8625 // Cannot convert null literal to non-nullable reference type. + fdsData[key] = null; +#pragma warning restore CS8625 // Cannot convert null literal to non-nullable reference type. + fdsData.Clear(); } - foreach (var plane in buffer.Planes) - { - var fd = plane.Fd.Get(); - if (!fdsData.ContainsKey(fd)) - continue; - var fdData = fdsData[fd]; - var planeData = fdData - .Skip(checked((int)plane.Offset)) - .Take(checked((int)plane.Length)) - .ToArray(); - planesData.Add(planeData); - } + _logger.LogDebug("Frame(s) collected"); + if (File.Exists("/proc/self/status")) + _logger.LogDebug("Current RAM Usage: {}", File.ReadAllLines("/proc/self/status").FirstOrDefault(l => l.StartsWith("RssAnon:"))?.Substring("RssAnon:".Length)?.Trim() ?? "UNKNOWN"); + effectiveCancellationToken.ThrowIfCancellationRequested(); @@ -573,6 +614,7 @@ public class CameraService : IDisposable var decoder = FrameDecoderHelper.FindDecoder(fourcc, modifier, primariesEnum, transferFunctionEnum, ycbcrEncodingEnum, rangeEnum); if (decoder is null) throw new Exception($"Cannot find decoder for fourcc=0x{fourcc:X}({new string(BitConverter.GetBytes(fourcc).Select(b => (char)b).ToArray())}) modifier=0x{modifier:X} primariesEnum={primariesEnum} transferFunctionEnum={transferFunctionEnum} ycbcrEncodingEnum={ycbcrEncodingEnum} rangeEnum={rangeEnum}"); + _logger.LogDebug("Found decoder {}", decoder); effectiveCancellationToken.ThrowIfCancellationRequested(); @@ -585,6 +627,7 @@ public class CameraService : IDisposable _logger.LogError(ex, "Error while image decode"); throw; } + _logger.LogDebug("Image decoded successfully"); } catch (Exception e) { @@ -596,10 +639,12 @@ public class CameraService : IDisposable #pragma warning disable CS8625 // Cannot convert null literal to non-nullable reference type. fdsData[key] = null; #pragma warning restore CS8625 // Cannot convert null literal to non-nullable reference type. + fdsData.Clear(); for (int i = 0; i < planesData.Count(); i++) #pragma warning disable CS8625 // Cannot convert null literal to non-nullable reference type. planesData[i] = null; #pragma warning restore CS8625 // Cannot convert null literal to non-nullable reference type. + planesData.Clear(); } return image; } diff --git a/GSS2.FrameDecoders/BGGR_PISP_COMP1_RAW_FrameDecoder.cs b/GSS2.FrameDecoders/BGGR_PISP_COMP1_RAW_FrameDecoder.cs index 363c19c..1d957d5 100644 --- a/GSS2.FrameDecoders/BGGR_PISP_COMP1_RAW_FrameDecoder.cs +++ b/GSS2.FrameDecoders/BGGR_PISP_COMP1_RAW_FrameDecoder.cs @@ -135,15 +135,25 @@ public class BGGR_PISP_COMP1_RAW_FrameDecoder : IFrameDecoder // BGGR_PISP_COMP1 // 1) Распаковка исходных данных в которых каждые 8 байт представляют 8 пикселей (это не значит что 1 байт представляет 1 пиксель!) // 2) Демозаика полученого Bayer изображения в RGB (BGR) - if (planes.Count != 1) - throw new ArgumentException($"Single plane expected, got {planes.Count}"); + if (planes.Count < 1) + throw new ArgumentException($"At least 1 plane expected, got {planes.Count}"); + + var pixelCount = width * height; - var source = planes[0]; + var unpackedSum = new uint[pixelCount]; + foreach (var source in planes) + { + var unpacked = new ushort[pixelCount]; + PispDecompressor.DecompressPisp(source, width, height, stride, unpacked); + for (int i = 0; i < pixelCount; i++) + unpackedSum[i] += unpacked[i]; + } - var unpacked = new ushort[width * height]; - PispDecompressor.DecompressPisp(source, width, height, stride, unpacked); + var unpackedAvg = new ushort[pixelCount]; + for (int i = 0; i < pixelCount; i++) + unpackedAvg[i] = (ushort)(unpackedSum[i] / planes.Count()); - using var bayer = Mat.FromPixelData(height, width, MatType.CV_16UC1, unpacked); + using var bayer = Mat.FromPixelData(height, width, MatType.CV_16UC1, unpackedAvg); var bgr = new Mat(); Cv2.CvtColor(bayer, bgr, ColorConversionCodes.BayerRG2BGR);