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.
 
 
 
 

260 lines
6.9 KiB

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
}