feat: image caprure with averaging

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<OpenCvSharp.Mat?> CaptureImage(CancellationToken cancellationToken)
+    public async Task<OpenCvSharp.Mat?> 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<int, byte[]>();
         var planesData = new List<byte[]>();
+        var fdsData = new Dictionary<int, byte[]>();
+
         try
         {
-            effectiveCancellationToken.ThrowIfCancellationRequested();
+            while (planesData.Count() < framesCount)
-
-            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)
             {
-                if (e.Request.Handle != request.Handle)
+                effectiveCancellationToken.ThrowIfCancellationRequested();
-                    return;
+                _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;
+                var oldPlanesDataCount = planesData.Count();
-                _logger.LogDebug("Request buffers status: {}", bufferStatus);
+
-                if (bufferStatus == FrameMetadata.StatusEnum.FrameError || bufferStatus == FrameMetadata.StatusEnum.FrameStartup)
+                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<Request>();
+
+                foreach (var buffer in buffers)
                 {
-                    request.Reuse(Request.ReuseFlagEnum.ReuseBuffers);
+                    var request = _camera.CreateRequest(checked((ulong)DateTimeOffset.UtcNow.ToUnixTimeMilliseconds()));
-                    result = _camera.QueueRequest(request);
+                    var result = request.AddBuffer(_imageCaptureStream, buffer);
-                    if (result >= 0)
+                    if (result < 0)
-                        return;
+                        throw new Exception("Error while request create");
+                    requests.Add(request);
                 }
-                cts.SetResult();
+                _logger.LogDebug("Request(s) created");
-            }
 
-            _camera.RequestCompleted += RequestCompleted;
+                effectiveCancellationToken.ThrowIfCancellationRequested();
-            result = _camera.QueueRequest(request);
+
-            if (result < 0)
+                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);
+                foreach (var key in fdsData.Keys)
-            _logger.LogDebug("Request complited: \n\tResult: {} \n\tStatus: {}\n\tCookie: {}\n\tSequence: {}\n\tHasPendingBuffers: {}", result, request.Status, request.Cookie, request.Sequence, request.HasPendingBuffers);
+#pragma warning disable CS8625 // Cannot convert null literal to non-nullable reference type.
-            _logger.LogDebug("Request buffer: \n\tStatus: {} \n\tCookie: {} \n\tSequence: {} \n\tTimestamp: {}", buffer.Metadata.Status, buffer.Cookie, buffer.Metadata.Sequence, buffer.Metadata.Timestamp);
+                    fdsData[key] = null;
-            _logger.LogDebug("Request buffer planes: ");
+#pragma warning restore CS8625 // Cannot convert null literal to non-nullable reference type.
-            foreach (var (i, plane) in buffer.Metadata.Planes.Enumerate())
+                fdsData.Clear();
-                _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)
+            _logger.LogDebug("Frame(s) collected");
-            {
+            if (File.Exists("/proc/self/status"))
-                var fd = plane.Fd.Get();
+                _logger.LogDebug("Current RAM Usage: {}", File.ReadAllLines("/proc/self/status").FirstOrDefault(l => l.StartsWith("RssAnon:"))?.Substring("RssAnon:".Length)?.Trim() ?? "UNKNOWN");
-                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);
-            }
 
             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;
     }

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)
+        if (planes.Count < 1)
-            throw new ArgumentException($"Single plane expected, got {planes.Count}");
+            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];
+        var unpackedAvg = new ushort[pixelCount];
-        PispDecompressor.DecompressPisp(source, width, height, stride, unpacked);
+        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);