TURBO-569：更新工程结构；将导航相机标定和校准功能迁移到XP.Camera类
This commit is contained in:
李伟
@@ -0,0 +1,201 @@
|
||||
// ============================================================================
|
||||
// 文件名: CalibrationProcessor.cs
|
||||
// 描述: 标定处理器，实现图像坐标系到世界坐标系的转换
|
||||
// 功能:
|
||||
// - 基于多点标定计算透视变换矩阵（支持4点及以上）
|
||||
// - 像素坐标到世界坐标的转换
|
||||
// - 标定数据的保存和加载（JSON格式）
|
||||
// - 从CSV文件导入标定点数据
|
||||
// 算法: 使用DLT（Direct Linear Transformation）方法求解单应性矩阵
|
||||
// ============================================================================
|
||||
|
||||
using Emgu.CV;
|
||||
using System.IO;
|
||||
using System.Text.Json;
|
||||
|
||||
namespace XP.Camera.Calibration;
|
||||
|
||||
/// <summary>
|
||||
/// 九点标定处理器
|
||||
/// </summary>
|
||||
public class CalibrationProcessor
|
||||
{
|
||||
public class CalibrationPoint
|
||||
{
|
||||
public double PixelX { get; set; }
|
||||
public double PixelY { get; set; }
|
||||
public double WorldX { get; set; }
|
||||
public double WorldY { get; set; }
|
||||
}
|
||||
|
||||
public class CalibrationData
|
||||
{
|
||||
public List<CalibrationPoint> Points { get; set; } = new List<CalibrationPoint>();
|
||||
public double[] TransformMatrix { get; set; } = new double[9];
|
||||
public DateTime CalibrationTime { get; set; }
|
||||
}
|
||||
|
||||
private Matrix<double>? _transformMatrix;
|
||||
|
||||
/// <summary>
|
||||
/// 执行九点标定
|
||||
/// </summary>
|
||||
public bool Calibrate(List<CalibrationPoint> points)
|
||||
{
|
||||
if (points.Count < 4)
|
||||
return false;
|
||||
|
||||
int n = points.Count;
|
||||
var A = new Matrix<double>(2 * n, 8);
|
||||
var b = new Matrix<double>(2 * n, 1);
|
||||
|
||||
for (int i = 0; i < n; i++)
|
||||
{
|
||||
double u = points[i].PixelX;
|
||||
double v = points[i].PixelY;
|
||||
double x = points[i].WorldX;
|
||||
double y = points[i].WorldY;
|
||||
|
||||
A[2 * i, 0] = u;
|
||||
A[2 * i, 1] = v;
|
||||
A[2 * i, 2] = 1;
|
||||
A[2 * i, 3] = 0;
|
||||
A[2 * i, 4] = 0;
|
||||
A[2 * i, 5] = 0;
|
||||
A[2 * i, 6] = -x * u;
|
||||
A[2 * i, 7] = -x * v;
|
||||
b[2 * i, 0] = x;
|
||||
|
||||
A[2 * i + 1, 0] = 0;
|
||||
A[2 * i + 1, 1] = 0;
|
||||
A[2 * i + 1, 2] = 0;
|
||||
A[2 * i + 1, 3] = u;
|
||||
A[2 * i + 1, 4] = v;
|
||||
A[2 * i + 1, 5] = 1;
|
||||
A[2 * i + 1, 6] = -y * u;
|
||||
A[2 * i + 1, 7] = -y * v;
|
||||
b[2 * i + 1, 0] = y;
|
||||
}
|
||||
|
||||
var h = new Matrix<double>(8, 1);
|
||||
CvInvoke.Solve(A, b, h, Emgu.CV.CvEnum.DecompMethod.Svd);
|
||||
|
||||
_transformMatrix = new Matrix<double>(3, 3);
|
||||
_transformMatrix[0, 0] = h[0, 0];
|
||||
_transformMatrix[0, 1] = h[1, 0];
|
||||
_transformMatrix[0, 2] = h[2, 0];
|
||||
_transformMatrix[1, 0] = h[3, 0];
|
||||
_transformMatrix[1, 1] = h[4, 0];
|
||||
_transformMatrix[1, 2] = h[5, 0];
|
||||
_transformMatrix[2, 0] = h[6, 0];
|
||||
_transformMatrix[2, 1] = h[7, 0];
|
||||
_transformMatrix[2, 2] = 1.0;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 像素坐标转世界坐标
|
||||
/// </summary>
|
||||
public System.Drawing.PointF PixelToWorld(System.Drawing.PointF pixel)
|
||||
{
|
||||
if (_transformMatrix == null)
|
||||
return pixel;
|
||||
|
||||
double u = pixel.X;
|
||||
double v = pixel.Y;
|
||||
|
||||
double w = _transformMatrix[2, 0] * u + _transformMatrix[2, 1] * v + _transformMatrix[2, 2];
|
||||
double x = (_transformMatrix[0, 0] * u + _transformMatrix[0, 1] * v + _transformMatrix[0, 2]) / w;
|
||||
double y = (_transformMatrix[1, 0] * u + _transformMatrix[1, 1] * v + _transformMatrix[1, 2]) / w;
|
||||
|
||||
return new System.Drawing.PointF((float)x, (float)y);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 保存标定文件
|
||||
/// </summary>
|
||||
public void SaveCalibration(string filePath, List<CalibrationPoint> points)
|
||||
{
|
||||
var data = new CalibrationData
|
||||
{
|
||||
Points = points,
|
||||
TransformMatrix = new double[9],
|
||||
CalibrationTime = DateTime.Now
|
||||
};
|
||||
|
||||
if (_transformMatrix != null)
|
||||
{
|
||||
for (int i = 0; i < 3; i++)
|
||||
for (int j = 0; j < 3; j++)
|
||||
data.TransformMatrix[i * 3 + j] = _transformMatrix[i, j];
|
||||
}
|
||||
|
||||
var json = JsonSerializer.Serialize(data, new JsonSerializerOptions { WriteIndented = true });
|
||||
File.WriteAllText(filePath, json);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 加载标定文件
|
||||
/// </summary>
|
||||
public bool LoadCalibration(string filePath)
|
||||
{
|
||||
if (!File.Exists(filePath))
|
||||
return false;
|
||||
|
||||
try
|
||||
{
|
||||
var json = File.ReadAllText(filePath);
|
||||
var data = JsonSerializer.Deserialize<CalibrationData>(json);
|
||||
|
||||
if (data == null || data.TransformMatrix == null || data.TransformMatrix.Length != 9)
|
||||
return false;
|
||||
|
||||
_transformMatrix = new Matrix<double>(3, 3);
|
||||
for (int i = 0; i < 3; i++)
|
||||
for (int j = 0; j < 3; j++)
|
||||
_transformMatrix[i, j] = data.TransformMatrix[i * 3 + j];
|
||||
|
||||
return true;
|
||||
}
|
||||
catch
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从CSV文件加载标定点
|
||||
/// </summary>
|
||||
public List<CalibrationPoint> LoadPointsFromCsv(string filePath)
|
||||
{
|
||||
var points = new List<CalibrationPoint>();
|
||||
if (!File.Exists(filePath))
|
||||
return points;
|
||||
|
||||
var lines = File.ReadAllLines(filePath);
|
||||
for (int i = 0; i < lines.Length; i++)
|
||||
{
|
||||
if (i == 0 && (lines[i].Contains("PixelX") || lines[i].Contains("像素"))) continue;
|
||||
|
||||
var parts = lines[i].Split(',');
|
||||
if (parts.Length >= 4)
|
||||
{
|
||||
if (double.TryParse(parts[0].Trim(), out double px) &&
|
||||
double.TryParse(parts[1].Trim(), out double py) &&
|
||||
double.TryParse(parts[2].Trim(), out double wx) &&
|
||||
double.TryParse(parts[3].Trim(), out double wy))
|
||||
{
|
||||
points.Add(new CalibrationPoint
|
||||
{
|
||||
PixelX = px,
|
||||
PixelY = py,
|
||||
WorldX = wx,
|
||||
WorldY = wy
|
||||
});
|
||||
}
|
||||
}
|
||||
}
|
||||
return points;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user