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.
207 lines
5.9 KiB
207 lines
5.9 KiB
using MathNet.Numerics.LinearAlgebra;
|
|
using System.Drawing;
|
|
|
|
namespace Cis.Application.Core;
|
|
|
|
public abstract class MarkSearcherBase
|
|
{
|
|
#region Attr
|
|
|
|
/// <summary>
|
|
/// 当前相机参数信息
|
|
/// </summary>
|
|
protected CameraCalcInfo CameraCalcInfo { get; set; }
|
|
|
|
protected List<MarkLabelCalcInfo> MarkLabelInfoList { 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)
|
|
{
|
|
InitCameraCalcInfoRelated(cameraCalcInfo);
|
|
}
|
|
|
|
protected void InitCameraCalcInfoRelated(CameraCalcInfo cameraCalcInfo)
|
|
{
|
|
CameraCalcInfo = cameraCalcInfo;
|
|
CalcSensor();
|
|
World2CameraMatrix = ConvertWorldToCamera(cameraCalcInfo);
|
|
}
|
|
|
|
#region Calc
|
|
|
|
/// <summary>
|
|
/// 计算标签位置过程
|
|
/// </summary>
|
|
/// <returns></returns>
|
|
public void Calc(CameraCalcInfo cameraCalcInfo)
|
|
{
|
|
//判断
|
|
if (IsCameraRotate(cameraCalcInfo))
|
|
InitCameraCalcInfoRelated(cameraCalcInfo);
|
|
|
|
if (World2CameraMatrix == null || MarkLabelInfoList.Count == 0)
|
|
return;
|
|
|
|
for (int index = 0; index < MarkLabelInfoList.Count; index++)
|
|
{
|
|
MarkLabelCalcInfo label = MarkLabelInfoList[index];
|
|
Matrix<double> labelC2WMatrix = ConvertCameraToWorld(label);
|
|
Matrix<double> labelPointMatrix = MBuilder.DenseOfArray(new double[,]
|
|
{
|
|
{ label.CanvasWidth / CameraCalcInfo.ImageWidth - 0.5 },
|
|
{ label.CanvasHeight / CameraCalcInfo.ImageHeight - 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 = new();
|
|
if (x > 0.99 || x < 0.01 || y > 0.99 || y < 0.01 || pResult[2, 0] < 0)
|
|
{
|
|
labelCalcResult.InFlag = false;
|
|
}
|
|
else
|
|
{
|
|
labelCalcResult.InFlag = true;
|
|
labelCalcResult.CanvasLeft = x * CameraCalcInfo.ImageWidth;
|
|
labelCalcResult.CanvasTop = y * CameraCalcInfo.ImageHeight;
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 判断相机是否进行了转动,转动了则需要重新计算世界坐标到相机坐标的转换矩阵
|
|
/// </summary>
|
|
/// <param name="newInfo"></param>
|
|
/// <returns></returns>
|
|
protected bool IsCameraRotate(CameraCalcInfo newInfo)
|
|
{
|
|
return CameraCalcInfo == null ||
|
|
CameraCalcInfo.PtzInfo.Pan != newInfo.PtzInfo.Pan ||
|
|
CameraCalcInfo.PtzInfo.Tilt != newInfo.PtzInfo.Tilt ||
|
|
CameraCalcInfo.PtzInfo.Zoom != newInfo.PtzInfo.Zoom;
|
|
}
|
|
|
|
/// <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 = MBuilder.DenseOfArray(new double[,]
|
|
{
|
|
{ pointF.X, 0, 0 },
|
|
{ 0, pointF.Y, 0 },
|
|
{ 0, 0, 1 },
|
|
});
|
|
|
|
Matrix<double> rotateTiltMatrix = MBuilder.DenseOfArray(new double[,]
|
|
{
|
|
{ 1, 0, 0 },
|
|
{ 0, cosTilt, sinTilt },
|
|
{ 0, -sinTilt, cosTilt },
|
|
});
|
|
|
|
Matrix<double> midResult = fMatrix.Multiply(rotateTiltMatrix);
|
|
|
|
Matrix<double> rotatePanMatrix = MBuilder.DenseOfArray(new double[,]
|
|
{
|
|
{ cosPan, 0, sinPan },
|
|
{ 0, 1, 0 },
|
|
{ -sinPan, 0, cosPan },
|
|
});
|
|
|
|
return midResult.Multiply(rotatePanMatrix);
|
|
}
|
|
|
|
/// <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 = MBuilder.DenseOfArray(new double[,] {
|
|
{ cosPan, 0, -sinPan },
|
|
{ 0, 1, 0 },
|
|
{ sinPan, 0, cosPan }
|
|
});
|
|
|
|
Matrix<double> rotateTiltMatrix = MBuilder.DenseOfArray(new double[,] {
|
|
{ 1, 0, 0 },
|
|
{ 0, cosTilt, -sinTilt },
|
|
{ 0, sinTilt, cosTilt }
|
|
});
|
|
|
|
Matrix<double> midResult = rotatePanMatrix.Multiply(rotateTiltMatrix);
|
|
|
|
Matrix<double> fMatrix = MBuilder.DenseOfArray(new double[,] {
|
|
{ (1 / pointF.X), 0, 0 },
|
|
{ 0, (1 / pointF.Y), 0 },
|
|
{ 0, 0, 1 }
|
|
});
|
|
|
|
return midResult.Multiply(fMatrix);
|
|
}
|
|
|
|
/// <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
|
|
}
|