Cis/Cis.Application/Core/Algo/MarkSearcherBase.cs

using MathNet.Numerics.LinearAlgebra;
using MathNet.Numerics.LinearAlgebra.Double;
using System.Collections.Concurrent;
using System.Drawing;

namespace Cis.Application.Core;

public abstract class MarkSearcherBase
{
	#region Attr

	/// <summary>
	///  当前相机参数信息
	/// </summary>
	protected CameraCalcInfo _cameraCalcInfo { get; set; }

	/// <summary>
	/// _markLabelInfoList 锁对象，写锁
	/// </summary>
	private static object mliListLock { get; } = new();

	/// <summary>
	/// 需要计算的 markLabelInfo 列表
	/// </summary>
	protected ConcurrentDictionary<long, MarkLabelCalcInfo> _markLabelInfoDict { get; set; } = new();

	//protected MatrixBuilder<double> MBuilder { get; set; } = Matrix<double>.Build;

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

	#endregion Attr

	public MarkSearcherBase(CameraCalcInfo cameraCalcInfo)
	{
		UpdateCameraCalcInfoRelated(cameraCalcInfo);
	}

	#region Calc

	/// <summary>
	///  计算标签位置过程
	/// </summary>
	/// <returns></returns>
	public List<MarkLabelCalcResult> Calc()
	{
		List<MarkLabelCalcResult> resultList = new();

		if (World2CameraMatrix == null || _markLabelInfoDict.IsEmpty)
			return resultList;

		foreach (MarkLabelCalcInfo item in _markLabelInfoDict.Values)
		{
			Matrix<double> labelC2WMatrix = ConvertCameraToWorld(item);
			Matrix<double> labelPointMatrix = new DenseMatrix(3, 1, new double[]
			{
				 (item.CanvasLeftRatio * item.VideoWidth) / _cameraCalcInfo.VideoWidth - 0.5 ,
				 (item.CanvasTopRatio * item.VideoHeight) / _cameraCalcInfo.VideoHeight - 0.5 ,
				 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);
			resultList.Add(labelCalcResult);
		}
		return resultList;
	}

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

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

	/// <summary>
	///  获取将世界坐标系中的点转化为相机坐标系中的点的转换矩阵
	/// </summary>
	/// <param name="cameraCalcInfo"></param>
	/// <returns></returns>
	protected Matrix<double> ConvertWorldToCamera(CameraCalcInfo cameraCalcInfo)
	{
		double panAngle = ConvertPanPosToAngle(cameraCalcInfo.PtzInfo.Pan);
		double tiltAngle = ConvertTiltPosToAngle(cameraCalcInfo.PtzInfo.Tilt);
		PointF pointF = GetFOfMatrixByZoomPos(cameraCalcInfo.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> midResult = fMatrix.Multiply(rotateTiltMatrix);

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

		var last = midResult.Multiply(rotatePanMatrix);
		return last;
	}

	/// <summary>
	///  获取将相机坐标系中的点转化为世界坐标系中的点的转换矩阵
	/// </summary>
	/// <param name="labelCalcInfo"></param>
	/// <returns></returns>
	protected Matrix<double> ConvertCameraToWorld(MarkLabelCalcInfo labelCalcInfo)
	{
		double panAngle = ConvertPanPosToAngle(labelCalcInfo.PtzInfo.Pan);
		double tiltAngle = ConvertTiltPosToAngle(labelCalcInfo.PtzInfo.Tilt);
		PointF pointF = GetFOfMatrixByZoomPos(labelCalcInfo.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> midResult = rotatePanMatrix.Multiply(rotateTiltMatrix);

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

		var last = midResult.Multiply(fMatrix);
		return last;
	}

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

	#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 abstract PointF GetFOfMatrixByZoomPos(double zoomPos);

	#endregion Util

	#region Operate Attr

	protected void UpdateCameraCalcInfoRelated(CameraCalcInfo cameraCalcInfo)
	{
		_cameraCalcInfo = cameraCalcInfo;
		CalcSensor();
		World2CameraMatrix = ConvertWorldToCamera(cameraCalcInfo);
	}

	public void UpdateCameraCalcInfo(PtzInfo ptzInfo)
	{
		if (IsCameraRotate(ptzInfo))
		{
			_cameraCalcInfo.PtzInfo = ptzInfo;
			World2CameraMatrix = ConvertWorldToCamera(_cameraCalcInfo);
		}
	}

	public bool AddMarkLabelCalcInfo(MarkLabelCalcInfo info)
	{
		lock (mliListLock)
		{
			if (_markLabelInfoDict.ContainsKey(info.Id))
				return false;
			return _markLabelInfoDict.TryAdd(info.Id, info);
		}
	}

	public bool DeleteMarkLabelCalcInfo(long id)
	{
		lock (mliListLock)
		{
			if (!_markLabelInfoDict.ContainsKey(id))
				return true;
			return _markLabelInfoDict.Remove(id);
		}
	}

	public bool ExistsMarkLabelCalcInfo(long id)
	{
		if (_markLabelInfoDict.ContainsKey(id))
			return true;
		return false;
	}

	#endregion Operate Attr
}