记录：
1.opencv的calibrateCamera()函数实现相机标定
2.已知相机内外参矩阵，通过ProjectPoints()函数实现理论角点计算
3.通过理论角点及实际角点计算误差
4.采用getOptimalNewCameraMatrix()函数及已知的相机内外参矩阵计算最优的相机矩阵
5.通过相机内外参矩阵及最优相机矩阵，采用UnDistort()获取无畸变的图像
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using OpenCvSharp;
using System.Threading.Tasks;
using Size = OpenCvSharp.Size;
using Point = OpenCvSharp.Point;
using System.IO;
using System.Reflection;
namespace TargetMeasureObject.Processor
{
class CameraCalibrator:NormalProcessor
{
private int len_dist_coeffs;//畸变系数长度，4(k1,k2,p1,p2)，5[,k3]，6[,k4]
private string images_directory;//棋盘格图像路径
private int n;//棋盘格图像数量
private Mat[] org_images;//棋盘格原始图像
private Mat[] corned_images;//绘制角点的图像
private Mat[] undistort_images;//消除畸变的图像
private double physical_interval;//棋盘格角点物理间距，单位m
private Size cp_shape_wh;//棋盘格角点排列（宽度上角点数量，高度上角点数量）
private List<Point3f> cp_world;//世界坐标角点模板
private List<Point2f[]> cp_pixel;//像素坐标角点集合
private string[] extension = {
"jpg", "tif", "jpeg" };
private double[,] cam_intrinsic;//相机内参
private double[] cam_dist_coeffs;//相机畸变参数
private Vec3d[] rot_mat_extrinsic;//相机外参-旋转矩阵
private Vec3d[] trans_vec_extrinsic;//相机外参-平移矩阵
private bool calibrated_status = false;//标定结束状态
private bool visualize;//可视化
public double[] error_reprojection;//二次投影误差
public CameraCalibrator()
{
}
public CameraCalibrator(string imgs_dir,Size cSeq,double interval,int dist_len,bool visual)
{
this.images_directory = imgs_dir;
this.physical_interval = interval;
this.cp_shape_wh= cSeq;
this.visualize = visual;
this.len_dist_coeffs = dist_len;
this.Initial();
}
/// <summary>
/// 初始化棋盘的世界坐标系的点
/// </summary>
private void Initial()
{
int w = this.cp_shape_wh.Width;
int h = this.cp_shape_wh.Height;
//相机内参矩阵初始化3*3
this.cam_intrinsic = new double[3, 3];
//相机畸变参数矩阵初始化(k1,k2,p1,p2[,k3[,k4,k5]])
this.cam_dist_coeffs = new double[this.len_dist_coeffs];
//世界坐标系角点初始化模板:对应索引*标定板网格间隔(unit=>m)
this.cp_world = new List<Point3f>();
for (int i = 0; i < h; i++)
{
for(int j = 0; j < w; j++)
{
Point3f pt = new Point3f(j,i,0);
pt = pt.Multiply(this.physical_interval);
this.cp_world.Add(pt);
}
}
}
/// <summary>
/// 执行标定
/// </summary>
public void Calibrate()
{
this.cp_pixel = new List<Point2f[]>();
string[] directs = Directory.GetFiles(this.images_directory);
this.n = directs.Length;
this.org_images = new Mat[n];
this.corned_images = new Mat[n];
List<List<Point3f>> cp_world = new List<List<Point3f>>();
for (int i = 0; i < n; i++)
{
//获取标定图像
this.org_images[i] = Cv2.ImRead(directs[i]);
//复制角点图像
this.corned_images[i] = this.org_images[i].Clone();
Mat gray_img = new Mat();
Cv2.CvtColor(this.org_images[i], gray_img, ColorConversionCodes.BGR2GRAY);
//查询像素坐标系棋盘角点
bool ret = Cv2.FindChessboardCorners(gray_img, this.cp_shape_wh, out Point2f[] corners);
if (ret)
{
//像素坐标系角点添加
this.cp_pixel.Add(corners);
//世界坐标系角点添加
cp_world.Add(this.cp_world);
//如果要求显示每张标定图像进行角点绘制
if (this.visualize)
{
bool found = false;
//绘制棋盘角点
Cv2.DrawChessboardCorners(this.corned_images[i], this.cp_shape_wh, corners, found);
string win_name = $"corner image show:{
i}";
Cv2.ImShow(win_name, this.corned_images[i]);
Cv2.WaitKey(1000);
Cv2.DestroyWindow(win_name);
}
}
}
//相机标定，获取相机内参，畸变参数，旋转矩阵，平移向量
Cv2.CalibrateCamera(cp_world, this.cp_pixel, this.org_images[0].Size(),
this.cam_intrinsic, this.cam_dist_coeffs,
out this.rot_mat_extrinsic, out this.trans_vec_extrinsic);
this.calibrated_status= true;
}
/// <summary>
/// 二次投影误差计算
/// 对世界坐标系点，通过旋转矩阵、位移向量、相机内参矩阵、相机畸变系数计算出新的投影的像素坐标点，
/// 计算新投影的像素坐标角点与实际图像中的像素角点误差
/// </summary>
/// <returns>所有样本误差均值</returns>
public double ReprojectErrorCalc()
{
this.error_reprojection = new double[this.n];
for (int i = 0; i < n; i++)
{
Point2f[] cp_reprojects=new Point2f[this.cp_shape_wh.Width*this.cp_shape_wh.Height];
Cv2.ProjectPoints(this.cp_world,
new double[] {
this.rot_mat_extrinsic[i].Item0, this.rot_mat_extrinsic[i].Item1, this.rot_mat_extrinsic[i].Item2 },
new double[] {
this.trans_vec_extrinsic[i][0], this.trans_vec_extrinsic[i][1], this.trans_vec_extrinsic[i][2] },
this.cam_intrinsic, this.cam_dist_coeffs, out cp_reprojects, out double[,] jacobian);
double[,] repro_data = new double[cp_reprojects.Length, 2];
double[,] cp_pixel_data = new double[cp_reprojects.Length, 2];
for (int j = 0; j < cp_reprojects.Length; j++)
{
repro_data[j, 0] = cp_reprojects[j].X;
repro_data[j, 1] = cp_reprojects[j].Y;
cp_pixel_data[j, 0] = this.cp_pixel[i][j].X;
cp_pixel_data[j, 1] = this.cp_pixel[i][j].Y;
}
Mat repro_mat = new Mat(cp_reprojects.Length, 2, MatType.CV_64FC1, repro_data);
Mat cp_pixel_mat = new Mat(cp_reprojects.Length, 2, MatType.CV_64FC1, cp_pixel_data);
double err = Cv2.Norm(cp_pixel_mat,repro_mat,NormTypes.L2)/(double)repro_data.GetLength(0);
this.error_reprojection[i] = err;
}
return this.error_reprojection.ToArray().Average();
}
/// <summary>
/// 消除图像畸变
/// </summary>
/// <param name="visualize">是否可见</param>
public void RemoveDistorts(bool visualize=true)
{
int w = this.org_images[0].Width;
int h = this.org_images[0].Height;
if (!this.calibrated_status) return;
this.undistort_images = new Mat[this.n];
//相机内参、畸变参数需矩阵化,防止报错方实现cv.Undistort()
Mat intrinsic_mat = new Mat(3, 3, MatType.CV_64FC1, this.cam_intrinsic);
Mat coeffs_mat = new Mat(5, 1, MatType.CV_64FC1, this.cam_dist_coeffs);
for (int i = 0; i < this.n; i++)
{
var camera_matrix_array= Cv2.GetOptimalNewCameraMatrix(this.cam_intrinsic, this.cam_dist_coeffs,
this.cp_shape_wh, 0, this.cp_shape_wh,out Rect rect);
Mat camera_matrix_mat = new Mat(camera_matrix_array.GetLength(0),
camera_matrix_array.GetLength(1),MatType.CV_64FC1, camera_matrix_array);
//无畸变图像需初始化，不得为空
this.undistort_images[i] = new Mat();
//消除图像畸变并输出
Cv2.Undistort(this.org_images[i], this.undistort_images[i], intrinsic_mat,
coeffs_mat, camera_matrix_mat);
if (visualize)
{
string wn = $"compared:{
i}";
Mat compare_imgs=new Mat(this.org_images[i].Height, this.org_images[i].Width*2,MatType.CV_8UC3);
compare_imgs[new Rect(0, 0, w, h)] = org_images[i];
compare_imgs[new Rect(w - 1, 0, w, h)] = this.undistort_images[i];
Cv2.ImShow("compare", compare_imgs);
Cv2.WaitKey();
Cv2.DestroyAllWindows();
}
}
}
}
}