李伟
222 lines
7.9 KiB
C#
222 lines
7.9 KiB
C#
// ============================================================================
|
|
// 文件名: 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;
|
|
}
|
|
}
|
|
}
|