feat: image caprure with averaging

2026-01-16 09:44:00 +03:00
parent ced7e294eb
commit af828a407d
2 changed files with 132 additions and 77 deletions
@@ -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();
-
-            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<Request>();
+
+                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;
    }
@@ -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);