Tanghaowei
127 lines
3.8 KiB
C#
127 lines
3.8 KiB
C#
using System.Drawing;
|
|
using Emgu.CV;
|
|
using Emgu.CV.Util;
|
|
using Emgu.CV.Structure;
|
|
using XP.Calibration.Core;
|
|
using XP.Calibration.Models;
|
|
|
|
namespace XP.Calibration;
|
|
|
|
/// <summary>
|
|
/// 中心校准: 椭圆拟合 + 透视修正 | Center calibration via ellipse fitting + perspective correction
|
|
/// </summary>
|
|
public class CenterCalibration
|
|
{
|
|
/// <summary>
|
|
/// 从投影序列执行中心校准
|
|
/// </summary>
|
|
/// <param name="projections">投影帧列表 (CV_32F)</param>
|
|
/// <param name="geo">几何参数</param>
|
|
/// <returns>校准结果</returns>
|
|
public CenterCalibrationResult Calibrate(List<Mat> projections, GeoParams geo)
|
|
{
|
|
// 检测各帧球心
|
|
var centers = new List<PointF>();
|
|
for (int i = 0; i < projections.Count; i++)
|
|
{
|
|
if (BallDetector.DetectCenter(projections[i], out var c))
|
|
centers.Add(c);
|
|
}
|
|
|
|
if (centers.Count <= 10)
|
|
throw new InvalidOperationException(
|
|
$"有效检测帧数不足: {centers.Count}, 至少需要 10 帧");
|
|
|
|
// 椭圆拟合
|
|
var ellipse = FitCentersEllipse(centers);
|
|
|
|
// 放大倍率
|
|
double M = geo.DSD / geo.DSO;
|
|
|
|
// 从椭圆参数反算倾斜角和 R
|
|
double ratio = ellipse.ShortAxis / ellipse.LongAxis;
|
|
ratio = Math.Clamp(ratio, 0.0, 1.0);
|
|
double alphaRad = Math.Acos(ratio);
|
|
double alphaDeg = alphaRad * 180.0 / Math.PI;
|
|
|
|
// 长轴 = 2 * R * M / pixelSize → R = longAxis * pixelSize / (2 * M)
|
|
double R = ellipse.LongAxis * geo.PixelSize / (2.0 * M);
|
|
|
|
// 透视修正
|
|
double deltaPx = R * R * Math.Sin(2.0 * alphaRad)
|
|
/ (2.0 * geo.DSO * geo.DSO)
|
|
* geo.DSD / geo.PixelSize;
|
|
|
|
// 长轴方向角
|
|
var rawEllipse = FitEllipseRaw(centers);
|
|
double angleDeg = rawEllipse.Angle;
|
|
float w = rawEllipse.Size.Width;
|
|
float h = rawEllipse.Size.Height;
|
|
if (h > w) angleDeg += 90.0f;
|
|
|
|
double thetaDeg = 90.0 - angleDeg;
|
|
double thetaRad = thetaDeg * Math.PI / 180.0;
|
|
|
|
double deltaU = deltaPx * Math.Cos(thetaRad);
|
|
double deltaV = deltaPx * (-Math.Sin(thetaRad));
|
|
|
|
double u0 = ellipse.Center.X - deltaU;
|
|
double v0 = ellipse.Center.Y - deltaV;
|
|
|
|
return new CenterCalibrationResult
|
|
{
|
|
Ellipse = ellipse,
|
|
AlphaDeg = alphaDeg,
|
|
R_mm = R,
|
|
DeltaPx = deltaPx,
|
|
FocalU = u0,
|
|
FocalV = v0,
|
|
DetectedCenters = centers
|
|
};
|
|
}
|
|
|
|
/// <summary>
|
|
/// 从 RAW 文件执行中心校准 (便捷方法)
|
|
/// </summary>
|
|
public CenterCalibrationResult CalibrateFromRaw(
|
|
string rawPath, int width, int height, int count, GeoParams geo)
|
|
{
|
|
var projections = RawReader.ReadFloat32(rawPath, width, height, count);
|
|
try
|
|
{
|
|
return Calibrate(projections, geo);
|
|
}
|
|
finally
|
|
{
|
|
foreach (var p in projections) p.Dispose();
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 对检测到的球心序列做椭圆拟合, 返回长短轴和角度
|
|
/// </summary>
|
|
private static EllipseResult FitCentersEllipse(List<PointF> pts)
|
|
{
|
|
var rotRect = FitEllipseRaw(pts);
|
|
float a = rotRect.Size.Width;
|
|
float b = rotRect.Size.Height;
|
|
|
|
return new EllipseResult
|
|
{
|
|
Center = rotRect.Center,
|
|
LongAxis = Math.Max(a, b),
|
|
ShortAxis = Math.Min(a, b),
|
|
Angle = rotRect.Angle
|
|
};
|
|
}
|
|
|
|
/// <summary>
|
|
/// 调用 OpenCV fitEllipse 返回原始 RotatedRect
|
|
/// </summary>
|
|
private static RotatedRect FitEllipseRaw(List<PointF> pts)
|
|
{
|
|
using var vp = new VectorOfPointF(pts.ToArray());
|
|
return CvInvoke.FitEllipse(vp);
|
|
}
|
|
}
|