九点标定参考代码

using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Forms;
using OpenCvSharp;
using OpenCvSharp.Extensions;
using Size = OpenCvSharp.Size;

namespace WindowsFormsApp1
{
public partial class Form1 : Form
{
public Form1()
{
InitializeComponent();
}
///
/// 本方法认为RMS误差可准确反应转换误差
/// 通过不断修改机器人的坐标来降低RMS误差来提高九点标定精度，因为九点标定的机器人坐标是人来判断的点位的是不准的
/// 通过不断迭代进行最优逼近
///
///
///
private void Form1_Load(object sender, EventArgs e)
{
Point2d[] camera_coordinates = new Point2d[9];
Point2d[] robot_coordinates = new Point2d[9];
for (int i = 0; i < 9; i++)//从表格中读取图像X Y 机器人X Y根据个人方式获取
{
camera_coordinates[i] = new Point2d(Convert.ToDouble(dgvCalibra.Rows[i].Cells[0].Value), Convert.ToDouble(dgvCalibra.Rows[i].Cells[1].Value));
robot_coordinates[i] = new Point2d(Convert.ToDouble(dgvCalibra.Rows[i].Cells[2].Value), Convert.ToDouble(dgvCalibra.Rows[i].Cells[3].Value));
}
Mat mat_c = new Mat(9, 2, MatType.CV_64F);//图像坐标
Mat mat_r = new Mat(9, 2, MatType.CV_64F);//机器人坐标
for (int i = 0; i < 9; i++)
{
mat_c.Set(i, 0, camera_coordinates[i].X);
mat_c.Set(i, 1, camera_coordinates[i].Y);
mat_r.Set(i, 0, robot_coordinates[i].X);
mat_r.Set(i, 1, robot_coordinates[i].Y);
}
double a = 0;
a = RMS(mat_c, mat_r, camera_coordinates, robot_coordinates);
}
private double RMS(Mat mat_c, Mat mat_r, Point2d[] camera_coordinates, Point2d[] robot_coordinates)
{
Mat warp = Cv2.EstimateAffine2D(mat_c, mat_r);//仿射变换
double rms = 0;
if (warp.Width == 3 && warp.Height == 2)
{
var A = warp.Get(0, 0);
var B = warp.Get(0, 1);
var C = warp.Get(0, 2);
var D = warp.Get(1, 0);
var E = warp.Get(1, 1);
var F = warp.Get(1, 2);
/RMS误差/
double sumX = 0, sumY = 0;
Point2d[] Calc = new Point2d[9];
for (int i = 0; i < 9; i++)
{
Point2d pt = new Point2d();
pt.X = A * camera_coordinates[i].X + B * camera_coordinates[i].Y + C;
pt.Y = D * camera_coordinates[i].X + E * camera_coordinates[i].Y + F;
Calc[i] = (pt);
sumX += Math.Pow(robot_coordinates[i].X - Calc[i].X, 2);
sumY += Math.Pow(robot_coordinates[i].Y - Calc[i].Y, 2);
}
rms = Math.Sqrt(sumX / 9) + Math.Sqrt(sumY / 9);
}
for (int k = 0; k < 3; k++)
{
for (int i = 0; i < 9; i++)
{
/对X操作******/
robot_coordinates[i].X += 0.01;
mat_r.Set(i, 0, robot_coordinates[i].X);
double curr = RMS_Add(mat_c, mat_r, camera_coordinates, robot_coordinates);
if (curr < rms)
{
while (curr < rms)
{
robot_coordinates[i].X += 0.01;
mat_r.Set(i, 0, robot_coordinates[i].X);
rms = curr;
curr = RMS_Add(mat_c, mat_r, camera_coordinates, robot_coordinates);
}
robot_coordinates[i].X -= 0.01;
mat_r.Set(i, 0, robot_coordinates[i].X);
}
else
{
robot_coordinates[i].X -= 0.02;
mat_r.Set(i, 0, robot_coordinates[i].X);
curr = RMS_Add(mat_c, mat_r, camera_coordinates, robot_coordinates);
while (curr < rms)
{
robot_coordinates[i].X -= 0.01;
mat_r.Set(i, 0, robot_coordinates[i].X);
rms = curr;
curr = RMS_Add(mat_c, mat_r, camera_coordinates, robot_coordinates);
}
robot_coordinates[i].X += 0.01;
mat_r.Set(i, 0, robot_coordinates[i].X);
}
/对Y操作************/
robot_coordinates[i].Y += 0.01;
mat_r.Set(i, 1, robot_coordinates[i].Y);
curr = RMS_Add(mat_c, mat_r, camera_coordinates, robot_coordinates);
if (curr < rms)
{
while (curr < rms)
{
robot_coordinates[i].Y += 0.01;
mat_r.Set(i, 1, robot_coordinates[i].Y);
rms = curr;
curr = RMS_Add(mat_c, mat_r, camera_coordinates, robot_coordinates);
}
robot_coordinates[i].Y -= 0.01;
mat_r.Set(i, 1, robot_coordinates[i].Y);
}
else
{
robot_coordinates[i].Y -= 0.02;
mat_r.Set(i, 1, robot_coordinates[i].Y);
curr = RMS_Add(mat_c, mat_r, camera_coordinates, robot_coordinates);
while (curr < rms)
{
robot_coordinates[i].Y -= 0.01;
mat_r.Set(i, 1, robot_coordinates[i].Y);
rms = curr;
curr = RMS_Add(mat_c, mat_r, camera_coordinates, robot_coordinates);
}
robot_coordinates[i].Y += 0.01;
mat_r.Set(i, 1, robot_coordinates[i].Y);
}
}
}
for (int i = 0; i < dgvCalibra.Rows.Count; i++)
{
dgvCalibra.Rows[i].Cells[2].Value = robot_coordinates[i].X.ToString(“F3”);
dgvCalibra.Rows[i].Cells[3].Value = robot_coordinates[i].Y.ToString(“F3”);
}
warp = Cv2.EstimateAffine2D(mat_c, mat_r);
var A1 = warp.Get(0, 0);
var B1 = warp.Get(0, 1);
var C1 = warp.Get(0, 2);
var D1 = warp.Get(1, 0);
var E1 = warp.Get(1, 1);
var F1 = warp.Get(1, 2);
Config.SetConfig(“Calibra_A”, A1.ToString());//将仿射变换矩阵保存
Config.SetConfig(“Calibra_B”, B1.ToString());
Config.SetConfig(“Calibra_C”, C1.ToString());
Config.SetConfig(“Calibra_D”, D1.ToString());
Config.SetConfig(“Calibra_E”, E1.ToString());
Config.SetConfig(“Calibra_F”, F1.ToString());
return rms;
}
private double RMS_Add(Mat mat_c, Mat mat_r, Point2d[] camera_coordinates, Point2d[] robot_coordinates)
{
Mat warp = Cv2.EstimateAffine2D(mat_c, mat_r);
double rms = 0;
if (warp.Width == 3 && warp.Height == 2)
{
var A = warp.Get(0, 0);
var B = warp.Get(0, 1);
var C = warp.Get(0, 2);
var D = warp.Get(1, 0);
var E = warp.Get(1, 1);
var F = warp.Get(1, 2);
/******RMS误差/
double sumX = 0, sumY = 0;
Point2d[] Calc = new Point2d[9];
for (int i = 0; i < 9; i++)
{
Point2d pt = new Point2d();
pt.X = A * camera_coordinates[i].X + B * camera_coordinates[i].Y + C;//图像坐标通过仿射矩阵转换为机器人坐标
pt.Y = D * camera_coordinates[i].X + E * camera_coordinates[i].Y + F;
Calc[i] = (pt);
sumX += Math.Pow(robot_coordinates[i].X - Calc[i].X, 2);
sumY += Math.Pow(robot_coordinates[i].Y - Calc[i].Y, 2);
}
rms = Math.Sqrt(sumX / 9) + Math.Sqrt(sumY / 9);
}
return rms;
}
}
}