forked from amkovkov/GranuSightSoftware2
331 lines
12 KiB
C#
331 lines
12 KiB
C#
using System.CommandLine;
|
|
|
|
using GSS2.Core;
|
|
using GSS2.Core.Hardware;
|
|
using GSS2.Core.Logging;
|
|
|
|
using Microsoft.Extensions.Logging.Console;
|
|
|
|
using OpenCvSharp;
|
|
|
|
using ConsoleFormatter = GSS2.Core.Logging.ConsoleFormatter;
|
|
|
|
namespace GSS2.Test;
|
|
|
|
public partial class Program
|
|
{
|
|
private class CameraTuningWorker : BackgroundService
|
|
{
|
|
private readonly ILogger<CameraTuningWorker> _logger;
|
|
private readonly IHostApplicationLifetime _applicationLifetime;
|
|
private readonly CameraService _cameraService;
|
|
private readonly IlluminatorService _illuminatorService;
|
|
private readonly FileInfo? _outputFile;
|
|
private double _gainR;
|
|
private double _gainG;
|
|
private double _gainB;
|
|
private readonly double _threshold;
|
|
private readonly uint _steps;
|
|
private static readonly Scalar[] KnownSegments =
|
|
{
|
|
new Scalar(0,255,255), // Cyan
|
|
new Scalar(0,0,255), // Blue
|
|
new Scalar(255,0,255), // Magenta
|
|
new Scalar(128,128,128), // Gray (skip)
|
|
new Scalar(255,0,0), // Red
|
|
new Scalar(255,128,0), // Orange
|
|
new Scalar(255,255,0), // Yellow
|
|
new Scalar(0,255,0) // Green
|
|
};
|
|
private const double PatternOuterRadius = 180.0;
|
|
private const double PatternCenterRadius = 60.0;
|
|
|
|
public CameraTuningWorker(ILogger<CameraTuningWorker> logger, IHostApplicationLifetime applicationLifetime, CameraService cameraService, IlluminatorService illuminatorService, FileInfo? outputFile, double gainR, double gainG, double gainB, double threshold, uint steps)
|
|
{
|
|
_applicationLifetime = applicationLifetime;
|
|
_logger = logger;
|
|
_cameraService = cameraService;
|
|
_illuminatorService = illuminatorService;
|
|
_outputFile = outputFile;
|
|
_gainR = gainR;
|
|
_gainG = gainG;
|
|
_gainB = gainB;
|
|
_threshold = threshold;
|
|
_steps = steps;
|
|
_logger.LogInformation("Initialized");
|
|
_logger.LogInformation("Output File: {}", _outputFile);
|
|
_logger.LogInformation("Gain R: {}", _gainR);
|
|
_logger.LogInformation("Gain G: {}", _gainG);
|
|
_logger.LogInformation("Gain B: {}", _gainB);
|
|
_logger.LogInformation("Threshold: {}", _threshold);
|
|
_logger.LogInformation("Steps: {}", _steps);
|
|
}
|
|
|
|
protected override async Task ExecuteAsync(CancellationToken stoppingToken)
|
|
{
|
|
_logger.LogInformation("Running camera fine tuning");
|
|
|
|
Mat? image;
|
|
|
|
int i = 0;
|
|
bool optimalGainsFound = false;
|
|
|
|
_illuminatorService.SetIntensity(1, 0, 0);
|
|
_illuminatorService.TurnOn();
|
|
|
|
while (i < _steps)
|
|
{
|
|
_cameraService.Configuration.DecodeGainR = _gainR;
|
|
_cameraService.Configuration.DecodeGainG = _gainG;
|
|
_cameraService.Configuration.DecodeGainB = _gainB;
|
|
|
|
try
|
|
{
|
|
image = await _cameraService.CaptureImage(stoppingToken, 2);
|
|
|
|
if (image is null)
|
|
{
|
|
_logger.LogError("Cannot get image");
|
|
break;
|
|
}
|
|
|
|
if (image.Type() != MatType.CV_8UC3 &&
|
|
image.Type() != MatType.CV_8UC4 &&
|
|
image.Type() != MatType.CV_16UC3 &&
|
|
image.Type() != MatType.CV_16UC4)
|
|
{
|
|
_logger.LogCritical("Unsupported image type: {}", image.Type().ToString());
|
|
break;
|
|
}
|
|
|
|
if (image.Type() == MatType.CV_16UC3)
|
|
image.ConvertTo(image, MatType.CV_8UC3, 1 / 256.0);
|
|
if (image.Type() == MatType.CV_16UC4)
|
|
image.ConvertTo(image, MatType.CV_8UC4, 1 / 256.0);
|
|
|
|
var channels = image.Split();
|
|
Cv2.Merge([channels[0], channels[1], channels[2]], image);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Exception occurred while image capturing");
|
|
break;
|
|
}
|
|
|
|
try
|
|
{
|
|
if (_outputFile is not null)
|
|
image.ImWrite(_outputFile.FullName);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Exception occurred while image saving");
|
|
}
|
|
|
|
try
|
|
{
|
|
var gains = ComputeWhiteBalanceGains(image);
|
|
if (gains is null)
|
|
{
|
|
_logger.LogError("Cannot compute gains");
|
|
break;
|
|
}
|
|
|
|
var (r, g, b) = gains.Value;
|
|
if (Math.Abs(_gainR - r) < _threshold &&
|
|
Math.Abs(_gainG - g) < _threshold &&
|
|
Math.Abs(_gainB - b) < _threshold)
|
|
{
|
|
_logger.LogInformation("Optimal color gains: r={}, g={}, b:={}", r, g, b);
|
|
optimalGainsFound = true;
|
|
break;
|
|
}
|
|
_gainR = (_gainR + r) / 2;
|
|
_gainG = (_gainG + g) / 2;
|
|
_gainB = (_gainB + b) / 2;
|
|
_logger.LogInformation("Step {}/{} color gains: r={}, g={}, b:={}", i + 1, _steps, _gainR, _gainG, _gainB);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Exception occurred while gains computing");
|
|
break;
|
|
}
|
|
i++;
|
|
}
|
|
_illuminatorService.TurnOff();
|
|
|
|
if (!optimalGainsFound)
|
|
_logger.LogWarning("Optimal gains not found");
|
|
|
|
_applicationLifetime.StopApplication();
|
|
}
|
|
|
|
private (double rGain, double gGain, double bGain)? ComputeWhiteBalanceGains(Mat image)
|
|
{
|
|
var circle = FindCircle(image);
|
|
if (circle == null)
|
|
return null;
|
|
|
|
var (rx, ry, rr) = circle.Value;
|
|
|
|
double scale = rr / PatternOuterRadius;
|
|
double innerPx = PatternCenterRadius * scale;
|
|
int midR = (int)((innerPx + rr) / 2.0);
|
|
|
|
double phi0 = DetectOrientation(image, rx, ry, rr);
|
|
if (double.IsNaN(phi0))
|
|
phi0 = Math.PI / 2.0;
|
|
|
|
var samples = new List<Vec3d>();
|
|
var refs = new List<Vec3d>();
|
|
|
|
int N = KnownSegments.Length;
|
|
|
|
for (int i = 0; i < N; i++)
|
|
{
|
|
if (i == 3) continue; // skip gray
|
|
|
|
double theta = phi0 - (2 * Math.PI / N) * i;
|
|
|
|
int cx = (int)(rx + midR * Math.Cos(theta));
|
|
int cy = (int)(ry - midR * Math.Sin(theta));
|
|
|
|
var roi = new Rect(
|
|
Math.Max(cx - 8, 0),
|
|
Math.Max(cy - 8, 0),
|
|
Math.Min(16, image.Width - Math.Max(cx - 8, 0)),
|
|
Math.Min(16, image.Height - Math.Max(cy - 8, 0)));
|
|
|
|
if (roi.Width < 8 || roi.Height < 8)
|
|
continue;
|
|
|
|
using var patch = new Mat(image, roi);
|
|
var mean = Cv2.Mean(patch);
|
|
|
|
// BGR → RGB
|
|
samples.Add(new Vec3d(mean.Val2, mean.Val1, mean.Val0));
|
|
|
|
var refColor = KnownSegments[i];
|
|
refs.Add(new Vec3d(refColor.Val2, refColor.Val1, refColor.Val0));
|
|
}
|
|
|
|
if (samples.Count < 3)
|
|
return null;
|
|
|
|
// Формируем матрицы для least squares
|
|
var S = new Mat(samples.Count, 3, MatType.CV_64F);
|
|
var R = new Mat(samples.Count, 3, MatType.CV_64F);
|
|
|
|
for (int i = 0; i < samples.Count; i++)
|
|
{
|
|
S.Set(i, 0, samples[i][0]);
|
|
S.Set(i, 1, samples[i][1]);
|
|
S.Set(i, 2, samples[i][2]);
|
|
|
|
R.Set(i, 0, refs[i][0]);
|
|
R.Set(i, 1, refs[i][1]);
|
|
R.Set(i, 2, refs[i][2]);
|
|
}
|
|
|
|
var M = new Mat();
|
|
Cv2.Solve(S, R, M, DecompTypes.Normal);
|
|
|
|
double rGain = Math.Max(M.At<double>(0, 0), 1.0);
|
|
double gGain = 1.0;
|
|
double bGain = Math.Max(M.At<double>(2, 2), 1.0);
|
|
|
|
return (rGain, gGain, bGain);
|
|
}
|
|
private (int x, int y, int r)? FindCircle(Mat image)
|
|
{
|
|
using var gray = new Mat();
|
|
Cv2.CvtColor(image, gray, ColorConversionCodes.BGR2GRAY);
|
|
Cv2.GaussianBlur(gray, gray, new Size(9, 9), 2);
|
|
|
|
var circles = Cv2.HoughCircles(
|
|
gray,
|
|
HoughModes.Gradient,
|
|
dp: 1.2,
|
|
minDist: gray.Rows / 3,
|
|
param1: 100,
|
|
param2: 30,
|
|
minRadius: (int)(gray.Rows * 0.3),
|
|
maxRadius: (int)(gray.Rows * 0.6));
|
|
|
|
if (circles.Length == 0)
|
|
return null;
|
|
|
|
var c = circles[0];
|
|
return ((int)c.Center.X, (int)c.Center.Y, (int)c.Radius);
|
|
}
|
|
private double DetectOrientation(Mat image, int rx, int ry, int rr)
|
|
{
|
|
double scale = rr / PatternOuterRadius;
|
|
int innerR = (int)(PatternCenterRadius * scale);
|
|
|
|
using var gray = new Mat();
|
|
Cv2.CvtColor(image, gray, ColorConversionCodes.BGR2GRAY);
|
|
|
|
using var mask = Mat.Zeros(image.Size(), MatType.CV_8U).ToMat();
|
|
Cv2.Circle(mask, new Point(rx, ry), innerR, Scalar.White, -1);
|
|
|
|
var whitePoints = new List<Point>();
|
|
|
|
for (int y = 0; y < gray.Rows; y++)
|
|
{
|
|
for (int x = 0; x < gray.Cols; x++)
|
|
{
|
|
if (mask.At<byte>(y, x) == 255 &&
|
|
gray.At<byte>(y, x) > 128)
|
|
{
|
|
whitePoints.Add(new Point(x, y));
|
|
}
|
|
}
|
|
}
|
|
|
|
if (whitePoints.Count < 20)
|
|
return double.NaN;
|
|
|
|
double cx = whitePoints.Average(p => p.X);
|
|
double cy = whitePoints.Average(p => p.Y);
|
|
|
|
return Math.Atan2(ry - cy, cx - rx);
|
|
}
|
|
}
|
|
private static void CameraTuning(ParseResult parseResult)
|
|
{
|
|
var outputFile = parseResult.GetValue(_cameraTuningOutputFile);
|
|
var gainR = parseResult.GetRequiredValue(_cameraTuningGainR);
|
|
var gainG = parseResult.GetRequiredValue(_cameraTuningGainG);
|
|
var gainB = parseResult.GetRequiredValue(_cameraTuningGainB);
|
|
var threshold = parseResult.GetRequiredValue(_cameraTuningThreshold);
|
|
var steps = parseResult.GetRequiredValue(_cameraTuningSteps);
|
|
|
|
var builder = Host.CreateApplicationBuilder();
|
|
|
|
builder.Logging.ClearProviders();
|
|
builder.Logging.AddConsole(options => options.FormatterName = nameof(ConsoleFormatter));
|
|
builder.Logging.AddConsoleFormatter<ConsoleFormatter, ConsoleFormatterOptions>();
|
|
builder.Logging.AddProvider(new FileLoggerProvider("full.log", true));
|
|
builder.Logging.AddProvider(new FileLoggerProvider("last_run.log", false));
|
|
builder.Services.AddSingleton<LibCameraLogSink>();
|
|
|
|
builder.Services.AddIlluminatorService("Hardware:Illuminator");
|
|
builder.Services.AddCameraService("Hardware:Camera", true);
|
|
|
|
builder.Services.AddHostedService<CameraTuningWorker>(services =>
|
|
{
|
|
var logger = services.GetRequiredService<ILogger<CameraTuningWorker>>();
|
|
var applicationLifetime = services.GetRequiredService<IHostApplicationLifetime>();
|
|
var cameraService = services.GetRequiredService<CameraService>();
|
|
var illuminatorService = services.GetRequiredService<IlluminatorService>();
|
|
return new CameraTuningWorker(logger, applicationLifetime, cameraService, illuminatorService, outputFile, gainB, gainG, gainR, threshold, steps);
|
|
});
|
|
|
|
var host = builder.Build();
|
|
host.Services.GetRequiredService<LibCameraLogSink>();
|
|
|
|
host.Run();
|
|
}
|
|
}
|