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 /// /// 当前相机计算参数 /// protected CameraCalcParams CameraCalcParams { get; set; } /// /// 相机当前位置的世界坐标转化为相机坐标矩阵 /// protected Matrix World2CameraMatrix { get; set; } /// /// {markLabelId, MarkLabelCalcParams} /// protected ConcurrentDictionary MarkLabelCalcParamsDict { get; set; } = new(); /// /// {markLabelId, MarkLabelCalcParams} /// protected ConcurrentDictionary MarkLabelCalcResultDict { get; set; } = new(); #endregion Attr public MarkSearcherBase(CameraCalcParams cameraCalcParams) { CameraCalcParams = cameraCalcParams; CalcSensor(); World2CameraMatrix = ConvertWorldToCamera(cameraCalcParams); } #region Calc /// /// 判断相机是否进行了转动,转动了则需要重新计算世界坐标到相机坐标的转换矩阵 /// /// /// protected bool IsCameraRotate(PtzInfo newInfo) { bool ret = CameraCalcParams.PtzInfo.Pan != newInfo.Pan || CameraCalcParams.PtzInfo.Tilt != newInfo.Tilt || CameraCalcParams.PtzInfo.Zoom != newInfo.Zoom; return ret; } /// /// 此方法计算在球机zoom值最小的情况下成像矩阵中的 f 本质为获取像元大小 /// 尝试方案1:通过计算的方式来获取 /// 尝试方案2:通过张正友相机标定的方法来生成成像矩阵中的 f /// protected void CalcSensor() { CameraCalcParams.FocusX /= CameraCalcParams.VideoWidth; CameraCalcParams.FocusY /= CameraCalcParams.VideoHeight; } /// /// 获取将世界坐标系中的点转化为相机坐标系中的点的转换矩阵 /// /// /// protected Matrix 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 fMatrix = new DenseMatrix(3, 3, new double[] { pointF.X, 0, 0, 0, pointF.Y, 0, 0, 0, 1, }); Matrix rotateTiltMatrix = new DenseMatrix(3, 3, new double[] { 1, 0, 0, 0, cosTilt, sinTilt, 0, -sinTilt, cosTilt, }); Matrix rotatePanMatrix = new DenseMatrix(3, 3, new double[] { cosPan, 0, sinPan, 0, 1, 0, -sinPan, 0, cosPan, }); Matrix resultMatrix = fMatrix.Multiply(rotateTiltMatrix).Multiply(rotatePanMatrix); return resultMatrix; } /// /// 获取将相机坐标系中的点转化为世界坐标系中的点的转换矩阵 /// /// /// protected Matrix 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 rotatePanMatrix = new DenseMatrix(3, 3, new double[] { cosPan, 0, -sinPan, 0, 1, 0, sinPan, 0, cosPan, }); Matrix rotateTiltMatrix = new DenseMatrix(3, 3, new double[] { 1, 0, 0, 0, cosTilt, -sinTilt, 0, sinTilt, cosTilt, }); Matrix fMatrix = new DenseMatrix(3, 3, new double[] { (1 / pointF.X), 0, 0, 0, (1 / pointF.Y), 0, 0, 0, 1, }); Matrix resultMatrix = rotatePanMatrix.Multiply(rotateTiltMatrix).Multiply(fMatrix); return resultMatrix; } /// /// 计算标签位置过程 /// /// 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 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 labelC2WMatrix = ConvertCameraToWorld(item); Matrix 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 lResult = labelC2WMatrix.Multiply(labelPointMatrix); Matrix 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 /// /// 将Pan值转化为角度 /// /// protected abstract double ConvertPanPosToAngle(double panPos); /// /// 将Tilt转化为角度 /// /// protected abstract double ConvertTiltPosToAngle(double tiltPos, double tiltMinPos = 0); /// /// 根据当前zoom值获取相机矩阵参数 /// /// /// 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; } /// /// 将计算公式存储到数据库,实现动态计算公式 /// https://github.com/houlongchao/HLC.Expression /// https://blog.csdn.net/cxb2011/article/details/100837168 /// https://github.com/zz1231118/Rabbit /// /// /// protected abstract double GetZoomTag(double zoomPos); #endregion Util #region Base Method /// /// 更新相机计算参数 /// /// public void UpdateCameraCalcParams(PtzInfo ptzInfo) { if (IsCameraRotate(ptzInfo)) { CameraCalcParams.PtzInfo = ptzInfo; World2CameraMatrix = ConvertWorldToCamera(CameraCalcParams); Search(); } } /// /// 添加标签计算参数 /// /// /// public bool AddMarkLabelCalcParams(MarkLabelCalcParams labelCalcParams) { long markLabelId = labelCalcParams.Id; return MarkLabelCalcParamsDict.TryAdd(markLabelId, labelCalcParams) && MarkLabelCalcResultDict.TryAdd(markLabelId, SearchMarkLabel(labelCalcParams)); } /// /// 删除标签计算参数 /// /// /// public bool DeleteMarkLabelCalcParams(long markLabelId) { return MarkLabelCalcParamsDict.TryRemove(markLabelId, out _) && MarkLabelCalcResultDict.TryRemove(markLabelId, out _); } /// /// 是否存在标签计算参数 /// /// /// public bool IsExistsMarkLabelCalcParams(long markLabelId) { return MarkLabelCalcParamsDict.ContainsKey(markLabelId); } public List GetMarkLabelCalcResultList() { return MarkLabelCalcResultDict.Values.ToList(); } #endregion Base Method #region Base Method Async /// /// 更新相机计算参数 /// /// public async Task UpdateCameraCalcParamsAsync(PtzInfo ptzInfo) { if (IsCameraRotate(ptzInfo)) { CameraCalcParams.PtzInfo = ptzInfo; World2CameraMatrix = ConvertWorldToCamera(CameraCalcParams); await SearchAsync(); } } public async Task> GetMarkLabelCalcResultListAsync() { return await Task.Run(GetMarkLabelCalcResultList); } #endregion Base Method Async }