Camera Information System
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

328 lines
10 KiB

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
}