Cis/Cis.Application/Core/Component/MarkSeacher/Seacher/MarkSearcherBase.cs

using EC.Helper.CameraSDK;
using MathNet.Numerics.LinearAlgebra;
using MathNet.Numerics.LinearAlgebra.Double;
using System.Collections.Concurrent;
using System.Drawing;

namespace Cis.Application.Core.Component.MarkSeacher;

public abstract class MarkSearcherBase
{
    #region Attr

    /// <summary>
    ///  当前相机计算参数
    /// </summary>
    protected CameraCalcParams CameraCalcParams { get; set; }

    /// <summary>
    /// 相机当前位置的世界坐标转化为相机坐标矩阵
    /// </summary>
    protected Matrix<double> World2CameraMatrix { get; set; }

    /// <summary>
    /// {markLabelId, MarkLabelCalcParams}
    /// </summary>
    protected ConcurrentDictionary<long, MarkLabelCalcParams> MarkLabelCalcParamsDict { get; set; } = new();

    /// <summary>
    /// {markLabelId, MarkLabelCalcParams}
    /// </summary>
    protected ConcurrentDictionary<long, MarkLabelCalcResult> MarkLabelCalcResultDict { get; set; } = new();

    #endregion Attr

    public MarkSearcherBase(CameraCalcParams cameraCalcParams)
    {
        CameraCalcParams = cameraCalcParams;
        CalcSensor();
        World2CameraMatrix = ConvertWorldToCamera(cameraCalcParams);
    }

    #region Calc

    /// <summary>
    ///  判断相机是否进行了转动，转动了则需要重新计算世界坐标到相机坐标的转换矩阵
    /// </summary>
    /// <param name="newInfo"></param>
    /// <returns></returns>
    protected bool IsCameraRotate(PtzInfo newInfo)
    {
        bool ret = CameraCalcParams.PtzInfo.Pan != newInfo.Pan ||
            CameraCalcParams.PtzInfo.Tilt != newInfo.Tilt ||
            CameraCalcParams.PtzInfo.Zoom != newInfo.Zoom;
        return ret;
    }

    /// <summary>
    ///  此方法计算在球机zoom值最小的情况下成像矩阵中的 f 本质为获取像元大小
    ///  尝试方案1：通过计算的方式来获取
    ///  尝试方案2：通过张正友相机标定的方法来生成成像矩阵中的 f
    /// </summary>
    protected void CalcSensor()
    {
        CameraCalcParams.FocusX /= CameraCalcParams.VideoWidth;
        CameraCalcParams.FocusY /= CameraCalcParams.VideoHeight;
    }

    /// <summary>
    ///  获取将世界坐标系中的点转化为相机坐标系中的点的转换矩阵
    /// </summary>
    /// <param name="cameraCalcParams"></param>
    /// <returns></returns>
    protected Matrix<double> ConvertWorldToCamera(CameraCalcParams cameraCalcParams)
    {
        double panAngle = ConvertPanPosToAngle(cameraCalcParams.PtzInfo.Pan);
        double tiltAngle = ConvertTiltPosToAngle(cameraCalcParams.PtzInfo.Tilt);
        PointF pointF = GetFOfMatrixByZoomPos(cameraCalcParams.PtzInfo.Zoom);
        double sinPan = Math.Sin(panAngle);
        double cosPan = Math.Cos(panAngle);
        double sinTilt = Math.Sin(tiltAngle);
        double cosTilt = Math.Cos(tiltAngle);

        Matrix<double> fMatrix = new DenseMatrix(3, 3, new double[]
        {
             pointF.X, 0, 0,
             0, pointF.Y, 0,
             0, 0, 1,
        });

        Matrix<double> rotateTiltMatrix = new DenseMatrix(3, 3, new double[]
        {
             1, 0, 0,
             0, cosTilt, sinTilt,
             0, -sinTilt, cosTilt,
        });

        Matrix<double> rotatePanMatrix = new DenseMatrix(3, 3, new double[]
        {
             cosPan, 0, sinPan,
             0, 1, 0,
             -sinPan, 0, cosPan,
        });

        Matrix<double> resultMatrix = fMatrix.Multiply(rotateTiltMatrix).Multiply(rotatePanMatrix);
        return resultMatrix;
    }

    /// <summary>
    ///  获取将相机坐标系中的点转化为世界坐标系中的点的转换矩阵
    /// </summary>
    /// <param name="labelCalcParams"></param>
    /// <returns></returns>
    protected Matrix<double> ConvertCameraToWorld(MarkLabelCalcParams labelCalcParams)
    {
        double panAngle = ConvertPanPosToAngle(labelCalcParams.PtzInfo.Pan);
        double tiltAngle = ConvertTiltPosToAngle(labelCalcParams.PtzInfo.Tilt);
        PointF pointF = GetFOfMatrixByZoomPos(labelCalcParams.PtzInfo.Zoom);
        double sinPan = Math.Sin(panAngle);
        double cosPan = Math.Cos(panAngle);
        double sinTilt = Math.Sin(tiltAngle);
        double cosTilt = Math.Cos(tiltAngle);

        Matrix<double> rotatePanMatrix = new DenseMatrix(3, 3, new double[]
        {
             cosPan, 0, -sinPan,
             0, 1, 0,
             sinPan, 0, cosPan,
        });

        Matrix<double> rotateTiltMatrix = new DenseMatrix(3, 3, new double[]
        {
             1, 0, 0,
             0, cosTilt, -sinTilt,
             0, sinTilt, cosTilt,
        });

        Matrix<double> fMatrix = new DenseMatrix(3, 3, new double[]
         {
             (1 / pointF.X), 0, 0,
             0, (1 / pointF.Y), 0,
             0, 0, 1,
        });

        Matrix<double> resultMatrix = rotatePanMatrix.Multiply(rotateTiltMatrix).Multiply(fMatrix);
        return resultMatrix;
    }

    /// <summary>
    ///  计算标签位置过程
    /// </summary>
    /// <returns></returns>
    protected void Search()
    {
        if (World2CameraMatrix == null || MarkLabelCalcParamsDict.IsEmpty)
            return;

        foreach (MarkLabelCalcParams item in MarkLabelCalcParamsDict.Values)
        {
            MarkLabelCalcResult result = SearchMarkLabel(item);
            if (!MarkLabelCalcParamsDict.ContainsKey(item.Id)) return;
            MarkLabelCalcResultDict[item.Id] = result;
        }
    }

    protected async Task SearchAsync()
    {
        if (World2CameraMatrix == null || MarkLabelCalcParamsDict.IsEmpty)
            return;

        List<Task> tasks = new();
        foreach (MarkLabelCalcParams item in MarkLabelCalcParamsDict.Values)
        {
            tasks.Add(Task.Run(() =>
            {
                MarkLabelCalcResult result = SearchMarkLabel(item);
                if (!MarkLabelCalcParamsDict.ContainsKey(item.Id)) return;
                MarkLabelCalcResultDict[item.Id] = result;
            }));
        }
        await Task.WhenAll(tasks);
    }

    protected MarkLabelCalcResult SearchMarkLabel(MarkLabelCalcParams item)
    {
        Matrix<double> labelC2WMatrix = ConvertCameraToWorld(item);
        Matrix<double> labelPointMatrix = new DenseMatrix(3, 1, new double[]
        {
                 (item.CanvasLeftRatio-0.5)*item.VideoWidth / CameraCalcParams.VideoWidth,
                 (item.CanvasTopRatio-0.5)*item.VideoHeight / CameraCalcParams.VideoHeight,
                 1
        });
        Matrix<double> lResult = labelC2WMatrix.Multiply(labelPointMatrix);
        Matrix<double> pResult = World2CameraMatrix.Multiply(lResult);

        double x = pResult[0, 0] / pResult[2, 0] + 0.5;
        double y = pResult[1, 0] / pResult[2, 0] + 0.5;
        MarkLabelCalcResult labelCalcResult;
        if (x > 0.99 || x < 0.01 || y > 0.99 || y < 0.01 || pResult[2, 0] < 0)
            labelCalcResult = MarkLabelCalcResult.New(item.Id, false);
        else
            labelCalcResult = MarkLabelCalcResult.New(item.Id, true, x, y);
        return labelCalcResult;
    }

    #endregion Calc

    #region Util

    /// <summary>
    ///  将Pan值转化为角度
    /// </summary>
    /// <returns></returns>
    protected abstract double ConvertPanPosToAngle(double panPos);

    /// <summary>
    ///  将Tilt转化为角度
    /// </summary>
    /// <returns></returns>
    protected abstract double ConvertTiltPosToAngle(double tiltPos, double tiltMinPos = 0);

    /// <summary>
    ///  根据当前zoom值获取相机矩阵参数
    /// </summary>
    /// <param name="zoomPos"></param>
    /// <returns></returns>
    protected PointF GetFOfMatrixByZoomPos(double zoomPos)
    {
        CameraCalcParams calcParams = CameraCalcParams;
        double zoomTag = GetZoomTag(zoomPos);
        PointF pointF = new()
        {
            X = (float)(calcParams.FocusX * zoomTag),
            Y = (float)(calcParams.FocusY * zoomTag)
        };
        return pointF;
    }

    /// <summary>
    /// 将计算公式存储到数据库，实现动态计算公式
    /// https://github.com/houlongchao/HLC.Expression
    /// https://blog.csdn.net/cxb2011/article/details/100837168
    /// https://github.com/zz1231118/Rabbit
    /// </summary>
    /// <param name="zoomPos"></param>
    /// <returns></returns>
    protected abstract double GetZoomTag(double zoomPos);

    #endregion Util

    #region Base Method

    /// <summary>
    /// 更新相机计算参数
    /// </summary>
    /// <param name="ptzInfo"></param>
    public void UpdateCameraCalcParams(PtzInfo ptzInfo)
    {
        if (IsCameraRotate(ptzInfo))
        {
            CameraCalcParams.PtzInfo = ptzInfo;
            World2CameraMatrix = ConvertWorldToCamera(CameraCalcParams);
            Search();
        }
    }

    /// <summary>
    /// 添加标签计算参数
    /// </summary>
    /// <param name="labelCalcParams"></param>
    /// <returns></returns>
    public bool AddMarkLabelCalcParams(MarkLabelCalcParams labelCalcParams)
    {
        long markLabelId = labelCalcParams.Id;
        return MarkLabelCalcParamsDict.TryAdd(markLabelId, labelCalcParams)
            && MarkLabelCalcResultDict.TryAdd(markLabelId, SearchMarkLabel(labelCalcParams));
    }

    /// <summary>
    /// 删除标签计算参数
    /// </summary>
    /// <param name="markLabelId"></param>
    /// <returns></returns>
    public bool DeleteMarkLabelCalcParams(long markLabelId)
    {
        return MarkLabelCalcParamsDict.TryRemove(markLabelId, out _)
            && MarkLabelCalcResultDict.TryRemove(markLabelId, out _);
    }

    /// <summary>
    /// 是否存在标签计算参数
    /// </summary>
    /// <param name="markLabelId"></param>
    /// <returns></returns>
    public bool IsExistsMarkLabelCalcParams(long markLabelId)
    {
        return MarkLabelCalcParamsDict.ContainsKey(markLabelId);
    }

    public List<MarkLabelCalcResult> GetMarkLabelCalcResultList()
    {
        return MarkLabelCalcResultDict.Values.ToList();
    }

    #endregion Base Method

    #region Base Method Async

    /// <summary>
    /// 更新相机计算参数
    /// </summary>
    /// <param name="ptzInfo"></param>
    public async Task UpdateCameraCalcParamsAsync(PtzInfo ptzInfo)
    {
        if (IsCameraRotate(ptzInfo))
        {
            CameraCalcParams.PtzInfo = ptzInfo;
            World2CameraMatrix = ConvertWorldToCamera(CameraCalcParams);
            await SearchAsync();
        }
    }

    public async Task<List<MarkLabelCalcResult>> GetMarkLabelCalcResultListAsync()
    {
        return await Task.Run(GetMarkLabelCalcResultList);
    }

    #endregion Base Method Async
}