增加校准通用代码

This commit is contained in:
Tanghaowei
2026-04-23 19:38:44 +08:00
committed by Tanghaowei
parent 40b229f5aa
commit b47c261bc3
12 changed files with 1329 additions and 0 deletions
+158
View File
@@ -0,0 +1,158 @@
using System.Drawing;
using Emgu.CV;
using Emgu.CV.CvEnum;
using Emgu.CV.Structure;
using Emgu.CV.Util;
namespace XP.Calibration.Core;
/// <summary>
/// 球心检测器 | Ball center detector from projection images
/// </summary>
public static class BallDetector
{
/// <summary>
/// 是否启用亚像素质心 | Enable sub-pixel centroid
/// </summary>
public static bool EnableSubPixel { get; set; } = true;
/// <summary>
/// 从单帧投影中检测球心 (自适应阈值 + 亚像素质心法)
/// </summary>
public static bool DetectCenter(Mat srcFloat, out PointF center)
{
center = new PointF(-1, -1);
// 归一化到 0-255
using var img8 = new Mat();
CvInvoke.Normalize(srcFloat, img8, 0, 255, NormType.MinMax);
img8.ConvertTo(img8, DepthType.Cv8U);
CvInvoke.GaussianBlur(img8, img8, new Size(3, 3), 0.8);
// 自适应阈值
using var bin = new Mat();
CvInvoke.AdaptiveThreshold(img8, bin, 255,
AdaptiveThresholdType.MeanC, ThresholdType.Binary, 31, -5);
// 查找轮廓
using var contours = new VectorOfVectorOfPoint();
CvInvoke.FindContours(bin, contours, null,
RetrType.External, ChainApproxMethod.ChainApproxNone);
if (contours.Size == 0)
return false;
// 找最大轮廓
int maxIdx = 0;
double maxArea = 0;
for (int i = 0; i < contours.Size; i++)
{
double area = CvInvoke.ContourArea(contours[i]);
if (area > maxArea)
{
maxArea = area;
maxIdx = i;
}
}
// 创建掩膜
using var mask = new Mat(srcFloat.Size, DepthType.Cv8U, 1);
mask.SetTo(new MCvScalar(0));
CvInvoke.DrawContours(mask, contours, maxIdx, new MCvScalar(255), -1);
if (EnableSubPixel)
{
center = SubPixelCentroid(srcFloat, mask);
}
else
{
var moments = CvInvoke.Moments(contours[maxIdx]);
center = new PointF(
(float)(moments.M10 / moments.M00),
(float)(moments.M01 / moments.M00));
}
return center.X >= 0 && center.Y >= 0;
}
/// <summary>
/// 从单帧投影中检测球心 (Canny 边缘 + 椭圆拟合法)
/// </summary>
public static bool DetectCenterByEllipse(Mat srcFloat, out PointF center)
{
center = new PointF(-1, -1);
using var img8 = new Mat();
CvInvoke.Normalize(srcFloat, img8, 0, 255, NormType.MinMax);
img8.ConvertTo(img8, DepthType.Cv8U);
CvInvoke.GaussianBlur(img8, img8, new Size(3, 3), 0.8);
using var edges = new Mat();
CvInvoke.Canny(img8, edges, 30, 80);
using var contours = new VectorOfVectorOfPoint();
CvInvoke.FindContours(edges, contours, null,
RetrType.External, ChainApproxMethod.ChainApproxNone);
if (contours.Size == 0)
return false;
int maxIdx = 0;
double maxArea = 0;
for (int i = 0; i < contours.Size; i++)
{
double area = CvInvoke.ContourArea(contours[i]);
if (area > maxArea)
{
maxArea = area;
maxIdx = i;
}
}
if (contours[maxIdx].Size < 5)
return false;
using var points = new VectorOfPointF(
Array.ConvertAll(contours[maxIdx].ToArray(), p => new PointF(p.X, p.Y)));
var ellipse = CvInvoke.FitEllipse(points);
center = ellipse.Center;
return true;
}
/// <summary>
/// 亚像素质心计算 | Sub-pixel centroid using intensity weighting
/// </summary>
private static PointF SubPixelCentroid(Mat imgFloat, Mat mask)
{
double sumI = 0, sumX = 0, sumY = 0;
var imgData = imgFloat.GetData() as float[,];
var maskData = mask.GetData() as byte[,];
if (imgData == null || maskData == null)
return new PointF(-1, -1);
int rows = imgFloat.Rows;
int cols = imgFloat.Cols;
for (int y = 0; y < rows; y++)
{
for (int x = 0; x < cols; x++)
{
if (maskData[y, x] != 0)
{
double intensity = imgData[y, x];
sumI += intensity;
sumX += x * intensity;
sumY += y * intensity;
}
}
}
if (sumI == 0)
return new PointF(-1, -1);
return new PointF((float)(sumX / sumI), (float)(sumY / sumI));
}
}
+161
View File
@@ -0,0 +1,161 @@
using System.Drawing;
using Emgu.CV;
using Emgu.CV.CvEnum;
using Emgu.CV.Structure;
using Emgu.CV.Util;
namespace XP.Calibration.Core;
/// <summary>
/// 双球心检测结果
/// </summary>
public record DualBallResult
{
/// <summary>第一个球心 (面积较大的)</summary>
public PointF Center1 { get; init; }
/// <summary>第二个球心 (面积较小的)</summary>
public PointF Center2 { get; init; }
/// <summary>两球心距离 (像素)</summary>
public double DistancePx { get; init; }
/// <summary>两球心距离 (mm), 需提供 pixelSize 才有效</summary>
public double DistanceMm { get; init; }
/// <summary>第一个球的轮廓面积</summary>
public double Area1 { get; init; }
/// <summary>第二个球的轮廓面积</summary>
public double Area2 { get; init; }
}
/// <summary>
/// 双球心检测器: 从单帧图像中检测两个球心并计算距离
/// </summary>
public static class DualBallDetector
{
/// <summary>
/// 检测图像中两个最大球体的中心及距离
/// </summary>
/// <param name="srcFloat">输入图像 (CV_32F)</param>
/// <param name="pixelSize">像素物理尺寸 (mm), 0 表示不计算物理距离</param>
/// <param name="minArea">最小轮廓面积阈值, 过滤噪声</param>
/// <param name="enableSubPixel">是否启用亚像素质心</param>
public static DualBallResult? Detect(
Mat srcFloat,
double pixelSize = 0,
double minArea = 50,
bool enableSubPixel = true)
{
// 归一化 + 预处理
using var img8 = new Mat();
CvInvoke.Normalize(srcFloat, img8, 0, 255, NormType.MinMax);
img8.ConvertTo(img8, DepthType.Cv8U);
CvInvoke.GaussianBlur(img8, img8, new Size(3, 3), 0.8);
// 自适应阈值
using var bin = new Mat();
CvInvoke.AdaptiveThreshold(img8, bin, 255,
AdaptiveThresholdType.MeanC, ThresholdType.Binary, 31, -5);
// 查找轮廓
using var contours = new VectorOfVectorOfPoint();
CvInvoke.FindContours(bin, contours, null,
RetrType.External, ChainApproxMethod.ChainApproxNone);
// 按面积排序, 取最大的两个
var candidates = new List<(int Index, double Area)>();
for (int i = 0; i < contours.Size; i++)
{
double area = CvInvoke.ContourArea(contours[i]);
if (area >= minArea)
candidates.Add((i, area));
}
if (candidates.Count < 2)
return null;
candidates.Sort((a, b) => b.Area.CompareTo(a.Area));
var top2 = candidates.Take(2).ToList();
// 计算两个球心
var centers = new PointF[2];
var areas = new double[2];
for (int k = 0; k < 2; k++)
{
int idx = top2[k].Index;
areas[k] = top2[k].Area;
if (enableSubPixel)
{
using var mask = new Mat(srcFloat.Size, DepthType.Cv8U, 1);
mask.SetTo(new MCvScalar(0));
CvInvoke.DrawContours(mask, contours, idx, new MCvScalar(255), -1);
centers[k] = SubPixelCentroid(srcFloat, mask);
}
else
{
var moments = CvInvoke.Moments(contours[idx]);
centers[k] = new PointF(
(float)(moments.M10 / moments.M00),
(float)(moments.M01 / moments.M00));
}
}
// 计算距离
double dx = centers[0].X - centers[1].X;
double dy = centers[0].Y - centers[1].Y;
double distPx = Math.Sqrt(dx * dx + dy * dy);
double distMm = pixelSize > 0 ? distPx * pixelSize : 0;
return new DualBallResult
{
Center1 = centers[0],
Center2 = centers[1],
DistancePx = distPx,
DistanceMm = distMm,
Area1 = areas[0],
Area2 = areas[1]
};
}
/// <summary>
/// 从 8bit 图像检测 (便捷重载, 支持 CV_8U 输入)
/// </summary>
public static DualBallResult? DetectFrom8U(
Mat src8U,
double pixelSize = 0,
double minArea = 50,
bool enableSubPixel = true)
{
using var floatMat = new Mat();
src8U.ConvertTo(floatMat, DepthType.Cv32F);
return Detect(floatMat, pixelSize, minArea, enableSubPixel);
}
private static PointF SubPixelCentroid(Mat imgFloat, Mat mask)
{
double sumI = 0, sumX = 0, sumY = 0;
var imgData = imgFloat.GetData() as float[,];
var maskData = mask.GetData() as byte[,];
if (imgData == null || maskData == null)
return new PointF(-1, -1);
for (int y = 0; y < imgFloat.Rows; y++)
{
for (int x = 0; x < imgFloat.Cols; x++)
{
if (maskData[y, x] != 0)
{
double intensity = imgData[y, x];
sumI += intensity;
sumX += x * intensity;
sumY += y * intensity;
}
}
}
if (sumI == 0)
return new PointF(-1, -1);
return new PointF((float)(sumX / sumI), (float)(sumY / sumI));
}
}
+139
View File
@@ -0,0 +1,139 @@
using XP.Calibration.Models;
namespace XP.Calibration.Core;
/// <summary>
/// TIGRE 投影模型 (角点法) | TIGRE projection model (corner-based, Siddon)
/// </summary>
public static class Projection
{
private struct Point3D
{
public double X, Y, Z;
public Point3D(double x, double y, double z) { X = x; Y = y; Z = z; }
}
/// <summary>
/// 计算物体原点在探测器上的投影像素坐标
/// </summary>
/// <param name="scanAngle">扫描角 (rad)</param>
/// <param name="ay">旋转轴倾斜角 (rad)</param>
/// <param name="detX">探测器 dYaw/Rx (rad)</param>
/// <param name="detY">探测器 dPitch/Ry (rad)</param>
/// <param name="detZ">探测器 dRoll/Rz (rad)</param>
/// <param name="offOrigX">物体 X 偏移 = -R (mm)</param>
/// <param name="offDetecU">探测器 U 偏移 (mm)</param>
/// <param name="offDetecV">探测器 V 偏移 (mm)</param>
/// <param name="gp">几何参数</param>
/// <param name="uPx">输出: U 像素坐标</param>
/// <param name="vPx">输出: V 像素坐标</param>
public static void ProjectPoint(
double scanAngle, double ay,
double detX, double detY, double detZ,
double offOrigX, double offDetecU, double offDetecV,
GeoParams gp,
out double uPx, out double vPx)
{
double ODD = gp.DSD - gp.DSO;
var S = new Point3D(gp.DSO, 0, 0);
// 探测器角点初始化
double py = gp.PixelSize * (0 - (double)gp.NDetecU / 2 + 0.5);
double pz = gp.PixelSize * ((double)gp.NDetecV / 2 - 0.5 - 0);
double puy = gp.PixelSize * (1 - (double)gp.NDetecU / 2 + 0.5);
double pvz = gp.PixelSize * ((double)gp.NDetecV / 2 - 0.5 - 1);
var P = new Point3D(0, py, pz);
var Pu0 = new Point3D(0, puy, pz);
var Pv0 = new Point3D(0, py, pvz);
// Step 1: rollPitchYaw (探测器自身旋转)
RollPitchYaw(detZ, detY, detX, ref P);
RollPitchYaw(detZ, detY, detX, ref Pu0);
RollPitchYaw(detZ, detY, detX, ref Pv0);
// 平移回探测器位置
P.X -= ODD; Pu0.X -= ODD; Pv0.X -= ODD;
// Step 2: offDetecU/V 偏移
P.Y += offDetecU; P.Z += offDetecV;
Pu0.Y += offDetecU; Pu0.Z += offDetecV;
Pv0.Y += offDetecU; Pv0.Z += offDetecV;
// Step 3: eulerZYZ 扫描旋转
EulerZYZ(scanAngle, ay, 0, ref P);
EulerZYZ(scanAngle, ay, 0, ref Pu0);
EulerZYZ(scanAngle, ay, 0, ref Pv0);
EulerZYZ(scanAngle, ay, 0, ref S);
// Step 4: offOrigin 偏移
P.X -= offOrigX; Pu0.X -= offOrigX; Pv0.X -= offOrigX;
S.X -= offOrigX;
// deltaU, deltaV
var deltaU = new Point3D(Pu0.X - P.X, Pu0.Y - P.Y, Pu0.Z - P.Z);
var deltaV = new Point3D(Pv0.X - P.X, Pv0.Y - P.Y, Pv0.Z - P.Z);
// 射线: S → 原点
var ray = new Point3D(-S.X, -S.Y, -S.Z);
// 探测器平面法线
var normal = new Point3D(
deltaU.Y * deltaV.Z - deltaU.Z * deltaV.Y,
deltaU.Z * deltaV.X - deltaU.X * deltaV.Z,
deltaU.X * deltaV.Y - deltaU.Y * deltaV.X);
// 射线与探测器平面交点
double pfsDotN = (P.X - S.X) * normal.X + (P.Y - S.Y) * normal.Y + (P.Z - S.Z) * normal.Z;
double rayDotN = ray.X * normal.X + ray.Y * normal.Y + ray.Z * normal.Z;
if (Math.Abs(rayDotN) < 1e-15)
{
uPx = vPx = -1;
return;
}
double t = pfsDotN / rayDotN;
var hit = new Point3D(S.X + t * ray.X, S.Y + t * ray.Y, S.Z + t * ray.Z);
var hitP = new Point3D(hit.X - P.X, hit.Y - P.Y, hit.Z - P.Z);
// 投影到像素坐标
double dU2 = deltaU.X * deltaU.X + deltaU.Y * deltaU.Y + deltaU.Z * deltaU.Z;
double dV2 = deltaV.X * deltaV.X + deltaV.Y * deltaV.Y + deltaV.Z * deltaV.Z;
double pixelU = (hitP.X * deltaU.X + hitP.Y * deltaU.Y + hitP.Z * deltaU.Z) / dU2;
double pixelV = (hitP.X * deltaV.X + hitP.Y * deltaV.Y + hitP.Z * deltaV.Z) / dV2;
uPx = pixelU;
vPx = (gp.NDetecV - 1) - pixelV; // TIGRE v 翻转
}
/// <summary>
/// rollPitchYaw: Rz(dRoll) * Ry(dPitch) * Rx(dYaw)
/// </summary>
private static void RollPitchYaw(double dRoll, double dPitch, double dYaw, ref Point3D p)
{
double cr = Math.Cos(dRoll), sr = Math.Sin(dRoll);
double cp = Math.Cos(dPitch), sp = Math.Sin(dPitch);
double cy = Math.Cos(dYaw), sy = Math.Sin(dYaw);
double x = p.X, y = p.Y, z = p.Z;
p.X = cr * cp * x + (cr * sp * sy - sr * cy) * y + (cr * sp * cy + sr * sy) * z;
p.Y = sr * cp * x + (sr * sp * sy + cr * cy) * y + (sr * sp * cy - cr * sy) * z;
p.Z = -sp * x + cp * sy * y + cp * cy * z;
}
/// <summary>
/// eulerZYZ: Rz(alpha) * Ry(theta) * Rz(psi)
/// </summary>
private static void EulerZYZ(double alpha, double theta, double psi, ref Point3D p)
{
double ca = Math.Cos(alpha), sa = Math.Sin(alpha);
double ct = Math.Cos(theta), st = Math.Sin(theta);
double cp = Math.Cos(psi), sp = Math.Sin(psi);
double x = p.X, y = p.Y, z = p.Z;
p.X = (ca * ct * cp - sa * sp) * x + (-ca * ct * sp - sa * cp) * y + ca * st * z;
p.Y = (sa * ct * cp + ca * sp) * x + (-sa * ct * sp + ca * cp) * y + sa * st * z;
p.Z = -st * cp * x + st * sp * y + ct * z;
}
}
+92
View File
@@ -0,0 +1,92 @@
using System.Drawing;
using Emgu.CV;
using Emgu.CV.CvEnum;
namespace XP.Calibration.Core;
/// <summary>
/// RAW 投影数据读取器 | RAW projection data reader
/// </summary>
public static class RawReader
{
/// <summary>
/// 从 RAW 文件读取 float32 投影序列
/// </summary>
/// <param name="path">文件路径</param>
/// <param name="width">图像宽度</param>
/// <param name="height">图像高度</param>
/// <param name="count">投影帧数</param>
/// <returns>Mat 列表 (CV_32F)</returns>
public static List<Mat> ReadFloat32(string path, int width, int height, int count)
{
var projections = new List<Mat>(count);
int frameBytes = width * height * sizeof(float);
using var stream = new FileStream(path, FileMode.Open, FileAccess.Read);
byte[] buffer = new byte[frameBytes];
for (int i = 0; i < count; i++)
{
int bytesRead = stream.Read(buffer, 0, frameBytes);
if (bytesRead < frameBytes)
throw new EndOfStreamException(
$"帧 {i} 数据不足: 期望 {frameBytes} 字节, 实际 {bytesRead} 字节");
var mat = new Mat(height, width, DepthType.Cv32F, 1);
System.Runtime.InteropServices.Marshal.Copy(buffer, 0, mat.DataPointer, frameBytes);
projections.Add(mat);
}
return projections;
}
/// <summary>
/// 读取单张 TIFF 图像, 返回 CV_32F 灰度 Mat
/// </summary>
/// <param name="path">TIFF 文件路径</param>
/// <returns>Mat (CV_32F, 单通道)</returns>
public static Mat ReadTiff(string path)
{
if (!File.Exists(path))
throw new FileNotFoundException($"TIFF 文件不存在: {path}");
// 以原始深度读取 (支持 8/16/32bit TIFF)
var src = CvInvoke.Imread(path, ImreadModes.AnyDepth | ImreadModes.Grayscale);
if (src.IsEmpty)
throw new InvalidOperationException($"无法读取 TIFF: {path}");
// 统一转为 CV_32F
if (src.Depth != DepthType.Cv32F)
{
var dst = new Mat();
src.ConvertTo(dst, DepthType.Cv32F);
src.Dispose();
return dst;
}
return src;
}
/// <summary>
/// 批量读取多张 TIFF 图像 (按文件名排序)
/// </summary>
/// <param name="directory">TIFF 文件所在目录</param>
/// <param name="pattern">搜索模式, 默认 "*.tif"</param>
/// <returns>Mat 列表 (CV_32F)</returns>
public static List<Mat> ReadTiffSequence(string directory, string pattern = "*.tif")
{
if (!Directory.Exists(directory))
throw new DirectoryNotFoundException($"目录不存在: {directory}");
var files = Directory.GetFiles(directory, pattern)
.Concat(Directory.GetFiles(directory, pattern + "f")) // 同时匹配 .tif 和 .tiff
.Distinct()
.OrderBy(f => f)
.ToList();
if (files.Count == 0)
throw new FileNotFoundException($"目录中未找到 TIFF 文件: {directory}");
return files.Select(ReadTiff).ToList();
}
}