TURBO-569：更新工程结构；将导航相机标定和校准功能迁移到XP.Camera类
This commit is contained in:
李伟
@@ -0,0 +1,221 @@
|
||||
// ============================================================================
|
||||
// 文件名: ChessboardCalibrator.cs
|
||||
// 描述: 棋盘格标定器，实现基于棋盘格的相机内参标定
|
||||
// 功能:
|
||||
// - 从多张棋盘格图像中检测角点
|
||||
// - 计算相机内参矩阵和畸变系数
|
||||
// - 图像去畸变处理
|
||||
// - 计算重投影误差评估标定质量
|
||||
// - 标定结果的保存和加载（JSON格式）
|
||||
// 算法: 使用 Zhang's 标定方法进行相机标定
|
||||
// ============================================================================
|
||||
|
||||
using Emgu.CV;
|
||||
using Emgu.CV.CvEnum;
|
||||
using Emgu.CV.Structure;
|
||||
using Emgu.CV.Util;
|
||||
using System.IO;
|
||||
using System.Runtime.InteropServices;
|
||||
using System.Text.Json;
|
||||
|
||||
namespace XP.Camera.Calibration;
|
||||
|
||||
public class ChessboardCalibrator
|
||||
{
|
||||
public class CalibrationResult
|
||||
{
|
||||
public double[][] CameraMatrix { get; set; } = new double[3][];
|
||||
public double[] DistortionCoeffs { get; set; } = Array.Empty<double>();
|
||||
public double ReprojectionError { get; set; }
|
||||
public DateTime CalibrationTime { get; set; }
|
||||
}
|
||||
|
||||
private Mat? _cameraMatrix;
|
||||
private Mat? _distCoeffs;
|
||||
private double _reprojectionError;
|
||||
private List<double> _perImageErrors = new List<double>();
|
||||
|
||||
public double ReprojectionError => _reprojectionError;
|
||||
public List<double> PerImageErrors => _perImageErrors;
|
||||
|
||||
// 进度报告委托
|
||||
public delegate void ProgressReportHandler(int current, int total, string message);
|
||||
|
||||
public event ProgressReportHandler? ProgressChanged;
|
||||
|
||||
public bool CalibrateFromImages(List<string> imagePaths, int boardWidth, int boardHeight, float squareSize, out string errorMsg)
|
||||
{
|
||||
errorMsg = "";
|
||||
var objectPoints = new VectorOfVectorOfPoint3D32F();
|
||||
var imagePoints = new VectorOfVectorOfPointF();
|
||||
var imageSize = new System.Drawing.Size();
|
||||
|
||||
var objp = new MCvPoint3D32f[boardWidth * boardHeight];
|
||||
for (int i = 0; i < boardHeight; i++)
|
||||
for (int j = 0; j < boardWidth; j++)
|
||||
objp[i * boardWidth + j] = new MCvPoint3D32f(j * squareSize, i * squareSize, 0);
|
||||
|
||||
int validImages = 0;
|
||||
int totalImages = imagePaths.Count;
|
||||
|
||||
// 第一阶段：检测角点
|
||||
for (int idx = 0; idx < totalImages; idx++)
|
||||
{
|
||||
var path = imagePaths[idx];
|
||||
ProgressChanged?.Invoke(idx + 1, totalImages * 2, $"检测角点 ({idx + 1}/{totalImages})");
|
||||
|
||||
var img = CvInvoke.Imread(path, ImreadModes.Grayscale);
|
||||
if (img.IsEmpty) continue;
|
||||
|
||||
imageSize = img.Size;
|
||||
var corners = new VectorOfPointF();
|
||||
|
||||
if (CvInvoke.FindChessboardCorners(img, new System.Drawing.Size(boardWidth, boardHeight), corners))
|
||||
{
|
||||
// 亚像素级角点精化
|
||||
CvInvoke.CornerSubPix(img, corners, new System.Drawing.Size(5, 5), new System.Drawing.Size(-1, -1),
|
||||
new MCvTermCriteria(30, 0.001));
|
||||
|
||||
objectPoints.Push(new VectorOfPoint3D32F(objp));
|
||||
imagePoints.Push(corners);
|
||||
validImages++;
|
||||
}
|
||||
img.Dispose();
|
||||
}
|
||||
|
||||
if (validImages < 3)
|
||||
{
|
||||
errorMsg = $"有效图像不足，需要至少3张，当前{validImages}张";
|
||||
ProgressChanged?.Invoke(0, 100, "标定失败");
|
||||
return false;
|
||||
}
|
||||
|
||||
// 第二阶段：执行标定
|
||||
ProgressChanged?.Invoke(totalImages, totalImages * 2, "执行相机标定...");
|
||||
|
||||
_cameraMatrix = new Mat();
|
||||
_distCoeffs = new Mat();
|
||||
var rvecs = new VectorOfMat();
|
||||
var tvecs = new VectorOfMat();
|
||||
|
||||
_reprojectionError = CvInvoke.CalibrateCamera(objectPoints, imagePoints, imageSize, _cameraMatrix, _distCoeffs,
|
||||
rvecs, tvecs, CalibType.Default, new MCvTermCriteria(30, 1e-6));
|
||||
|
||||
// 第三阶段：计算每张图像的重投影误差
|
||||
_perImageErrors.Clear();
|
||||
for (int i = 0; i < objectPoints.Size; i++)
|
||||
{
|
||||
ProgressChanged?.Invoke(totalImages + i + 1, totalImages * 2, $"计算重投影误差 ({i + 1}/{objectPoints.Size})");
|
||||
|
||||
var projectedPoints = new VectorOfPointF();
|
||||
CvInvoke.ProjectPoints(objectPoints[i], rvecs[i], tvecs[i], _cameraMatrix, _distCoeffs, projectedPoints);
|
||||
|
||||
double error = 0;
|
||||
for (int j = 0; j < projectedPoints.Size; j++)
|
||||
{
|
||||
var dx = imagePoints[i][j].X - projectedPoints[j].X;
|
||||
var dy = imagePoints[i][j].Y - projectedPoints[j].Y;
|
||||
error += Math.Sqrt(dx * dx + dy * dy);
|
||||
}
|
||||
_perImageErrors.Add(error / projectedPoints.Size);
|
||||
}
|
||||
|
||||
ProgressChanged?.Invoke(totalImages * 2, totalImages * 2, "标定完成");
|
||||
return true;
|
||||
}
|
||||
|
||||
public Mat UndistortImage(Mat inputImage)
|
||||
{
|
||||
if (_cameraMatrix == null || _distCoeffs == null)
|
||||
return inputImage;
|
||||
|
||||
var output = new Mat();
|
||||
CvInvoke.Undistort(inputImage, output, _cameraMatrix, _distCoeffs);
|
||||
return output;
|
||||
}
|
||||
|
||||
public Mat? DrawChessboardCorners(string imagePath, int boardWidth, int boardHeight)
|
||||
{
|
||||
var img = CvInvoke.Imread(imagePath);
|
||||
if (img.IsEmpty) return null;
|
||||
|
||||
var gray = new Mat();
|
||||
CvInvoke.CvtColor(img, gray, ColorConversion.Bgr2Gray);
|
||||
var corners = new VectorOfPointF();
|
||||
|
||||
if (CvInvoke.FindChessboardCorners(gray, new System.Drawing.Size(boardWidth, boardHeight), corners))
|
||||
{
|
||||
CvInvoke.CornerSubPix(gray, corners, new System.Drawing.Size(11, 11), new System.Drawing.Size(-1, -1),
|
||||
new MCvTermCriteria(30, 0.001));
|
||||
CvInvoke.DrawChessboardCorners(img, new System.Drawing.Size(boardWidth, boardHeight), corners, true);
|
||||
}
|
||||
|
||||
gray.Dispose();
|
||||
return img;
|
||||
}
|
||||
|
||||
public void SaveCalibration(string filePath)
|
||||
{
|
||||
if (_cameraMatrix == null || _distCoeffs == null)
|
||||
throw new InvalidOperationException("请先执行标定");
|
||||
|
||||
var result = new CalibrationResult
|
||||
{
|
||||
CameraMatrix = new double[3][],
|
||||
DistortionCoeffs = new double[_distCoeffs.Rows],
|
||||
ReprojectionError = _reprojectionError,
|
||||
CalibrationTime = DateTime.Now
|
||||
};
|
||||
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
result.CameraMatrix[i] = new double[3];
|
||||
for (int j = 0; j < 3; j++)
|
||||
{
|
||||
double[] data = new double[1];
|
||||
Marshal.Copy(_cameraMatrix.DataPointer + (i * 3 + j) * 8, data, 0, 1);
|
||||
result.CameraMatrix[i][j] = data[0];
|
||||
}
|
||||
}
|
||||
|
||||
for (int i = 0; i < _distCoeffs.Rows; i++)
|
||||
{
|
||||
double[] data = new double[1];
|
||||
Marshal.Copy(_distCoeffs.DataPointer + i * 8, data, 0, 1);
|
||||
result.DistortionCoeffs[i] = data[0];
|
||||
}
|
||||
|
||||
var json = JsonSerializer.Serialize(result, new JsonSerializerOptions { WriteIndented = true });
|
||||
File.WriteAllText(filePath, json);
|
||||
}
|
||||
|
||||
public bool LoadCalibration(string filePath)
|
||||
{
|
||||
if (!File.Exists(filePath)) return false;
|
||||
|
||||
try
|
||||
{
|
||||
var json = File.ReadAllText(filePath);
|
||||
var result = JsonSerializer.Deserialize<CalibrationResult>(json);
|
||||
|
||||
if (result == null) return false;
|
||||
|
||||
_reprojectionError = result.ReprojectionError;
|
||||
|
||||
_cameraMatrix = new Mat(3, 3, DepthType.Cv64F, 1);
|
||||
for (int i = 0; i < 3; i++)
|
||||
for (int j = 0; j < 3; j++)
|
||||
Marshal.Copy(new[] { result.CameraMatrix[i][j] }, 0, _cameraMatrix.DataPointer + (i * 3 + j) * 8, 1);
|
||||
|
||||
_distCoeffs = new Mat(result.DistortionCoeffs.Length, 1, DepthType.Cv64F, 1);
|
||||
for (int i = 0; i < result.DistortionCoeffs.Length; i++)
|
||||
Marshal.Copy(new[] { result.DistortionCoeffs[i] }, 0, _distCoeffs.DataPointer + i * 8, 1);
|
||||
|
||||
return true;
|
||||
}
|
||||
catch
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user