关于九点标定的理解，并在不借助Opencv、Halcon等算法库下，使用C#代码实现九点标定

前言

由于工作上需要使用机械手对产品进行贴附操作，这其中涉及到图像坐标系与机械手坐标系的变换。由于是初次学习，将学习过程以及见解记录于此，以便后续学习。
文章中如有错误，恳请大佬们给点意见，谢谢！！！

手眼标定与九点标定之间关系的理解

说起建立视觉像素坐标与机器人或轴基坐标系的关系，就脱不开手眼标定这个概念，通过手眼标定，可以将工业相机看到的物体的像素坐标转换到机器人坐标上，这样机器人就能够通过转换后的坐标完成抓取任务。相机相当于赋予了机器人”视觉“。

手眼标定细分为九点标定，十五点标定等。刚学习的时候非常容易将两者混淆，认为手眼标定就是九点标定，这是一个错误的概念。

九点标定只是手眼标定的子概念，而且九点标定只能实现对平面物体的抓取和定位，本质上是2D形式的 “手眼标定” ，无法做到三维空间的转换！！！
九点标定操作上是机器人运动九次，同时记录同一个物体九次像素坐标和机器人坐标，便可以求解X。其实从理论上来说，三个点便可以完成坐标系的转换，使用九点和十五点无非是增加参数，减少误差，通过最小二乘法来得到误差更小的结果。

九点最小二乘法

当我们获取图像坐标与机器人对应的九个坐标后，用（$x_n，y_n$)来表示图像上的九个角点坐标，用（$x_n^{,}，y_n^{,}$)表示机器人九个点位坐标。
因此，我们可以构造如下形式的矩阵：

展开：

此时可将其看成：AX = B 形式，X 即为我们要求的图像坐标系到机器人坐标系的变换关系。
由于A为3 * 9的矩阵，B 也为3 * 9的矩阵，所以需要进行转置的操作，最终构造成如下的形式：

最后我们就得到了图像坐标到机器人坐标的变换关系X，此时我们就获得了图像坐标系与机器人坐标系的对应关系，这样我们可以用图像任意一点来求出对应的机器人坐标！

在不借助Opencv、Halcon等工具下，使用C#代码实现九点标定

其实通过上面的叙述，使用代码来实现的关键点就是矩阵计算：

第一步：矩阵相乘的实现：

        ///   
 
        ///   矩阵乘法 
        ///   
 
        ///     
        ///     
        public static double[,] MultiplyMatrix(double[,] input1, double[,] input2)
        {
            int y = input1.GetLength(0);
            int x = input2.GetLength(1);
            int len = input2.GetLength(0);
            double[,] ansxtx = new double[y, x];

            for (int i = 0; i < y; i++)           //矩阵xt的行数
            {
                for (int j = 0; j < x; j++)        //矩阵x的列数
                {
                    double a = 0;                  //单个元素的乘积
                    for (int k = 0; k < len; k++)
                    {
                        a = a + input1[i, k] * input2[k, j];   //矩阵xt的第i行k列与矩阵x的第k行j列的乘积
                    }
                    ansxtx[i, j] = a;            //将矩阵xt的i行与矩阵x的j列的乘积放入新矩阵的i行j列
                }
            }
            return ansxtx;
        }

第二步矩阵转置的实现：

        ///   
 
        ///   矩阵的转置 
        ///   
 
        ///     
        public static double[,] TransPose(double[,] iMatrix)
        {
            int row = iMatrix.GetLength(0);
            int column = iMatrix.GetLength(1);
            //double[,] iMatrix = new double[column, row];
            double[,] TempMatrix = new double[row, column];
            double[,] iMatrixT = new double[column, row];
            for (int i = 0; i < row; i++)
            {
                for (int j = 0; j < column; j++)
                {
                    TempMatrix[i, j] = iMatrix[i, j];
                }
            }
            for (int i = 0; i < column; i++)
            {
                for (int j = 0; j < row; j++)
                {
                    iMatrixT[i, j] = TempMatrix[j, i];
                }
            }
            return iMatrixT;

        }

第三步 矩阵求逆的实现：

        ///   
 
        ///   矩阵的逆矩阵 
        ///   
 
        ///     
        public static double[,] InverseMatrix(double[,] iMatrix)
        {
            int i = 0;
            int row = iMatrix.GetLength(0);
            double[,] MatrixZwei = new double[row, row * 2];
            double[,] iMatrixInv = new double[row, row];
            for (i = 0; i < row; i++)
            {
                for (int j = 0; j < row; j++)
                {
                    MatrixZwei[i, j] = iMatrix[i, j];
                }
            }
            for (i = 0; i < row; i++)
            {
                for (int j = row; j < row * 2; j++)
                {
                    MatrixZwei[i, j] = 0;
                    if (i + row == j)
                        MatrixZwei[i, j] = 1;
                }
            }

            for (i = 0; i < row; i++)
            {
                if (MatrixZwei[i, i] != 0)
                {
                    double intTemp = MatrixZwei[i, i];
                    for (int j = 0; j < row * 2; j++)
                    {
                        MatrixZwei[i, j] = MatrixZwei[i, j] / intTemp;
                    }
                }
                for (int j = 0; j < row; j++)
                {
                    if (j == i)
                        continue;
                    double intTemp = MatrixZwei[j, i];
                    for (int k = 0; k < row * 2; k++)
                    {
                        MatrixZwei[j, k] = MatrixZwei[j, k] - MatrixZwei[i, k] * intTemp;
                    }
                }
            }

            for (i = 0; i < row; i++)
            {
                for (int j = 0; j < row; j++)
                {
                    iMatrixInv[i, j] = MatrixZwei[i, j + row];
                }
            }
            return iMatrixInv;
        }

打印矩阵的方法：

	/// 

	/// 打印矩阵
	/// 
	/// 待打印矩阵
	public static void PrintMatrix(double[,] matrix, string title = "")
	{
	    //1.标题值为空则不显示标题
	    if (!String.IsNullOrWhiteSpace(title))
	    {
	        Console.WriteLine(title);
	    }
	
	    //2.打印矩阵
	    for (int i = 0; i < matrix.GetLength(0); i++)
	    {
	        for (int j = 0; j < matrix.GetLength(1); j++)
	        {
	            Console.Write(matrix[i,j] + "\t");
	            //注意不能写为：Console.Write(matrix[i][j] + '\t');
	        }
	        Console.WriteLine();
	    }
	
	    //3.空行
	    Console.WriteLine();
	}

计算：

// 图像坐标
double[,] ImgPoint = new double[9, 3] { { 2958.151668, 944.8847238, 1 }, { 2961.854794, 1160.378599, 1 }, { 2965.287042, 1376.539532, 1 }, { 3174.458477, 941.4391491, 1 }, { 3178.070408, 1156.888123, 1 },
{ 3181.673486, 1373.124953 ,1},{ 3390.915162, 937.5764968,1 },{ 3394.673043, 1153.490735 ,1},{ 3398.01699, 1369.494267,1 },};


// 机械手坐标
double[,] RobPoint = new double[9, 3] { { -1, -1, 1 }, { 0, -1, 1 }, { 1, -1, 1 }, { -1, 0, 1 }, { 0, 0, 1 }, { 1, 0, 1 }, { -1, 1, 1 }, { 0, 1, 1 }, { 1, 1, 1 } };

/// X = (A^T * A)^-1 * A^T * B

var AT = MatrixMethod.TransPose(ImgPoint);
var AAT = MatrixMethod.MultiplyMatrix(AT, ImgPoint);
var AATInvert = MatrixMethod.InverseMatrix(AAT);

var Img2Rob = MatrixMethod.MultiplyMatrix(MatrixMethod.MultiplyMatrix(AATInvert, AT), RobPoint);

MatrixMethod.PrintMatrix(Img2Rob);  // 打印出结果