机器人手眼标定求解C++

机器人手眼标定从数学上看是求解矩阵方程：

AX=XB

A 为工具手两次运动对应变换矩阵，B 为摄像机两次运动对应的变换矩阵，X是需要求解的摄像机坐标系到工具手坐标系的变换矩阵，即手眼标定。

可以用李群和李代数的知识求解矩阵方程

AX=XB

首先保持标定板不动，从三个不同的位置拍摄标定板采集图像，得到三个摄像机外参矩阵，两个变换矩阵B1和B2。然后又机器人控制器得到机器人的三个位姿矩阵，工具手的两个变换矩阵A1和A2。

C++实现如下

//Hgij为机器人执行器末端坐标系之间相对位置姿态的齐次变换矩阵；[A1,A2]

//Hcij为摄像机坐标系之间相对位置姿态的齐次变换矩阵；[B1,B2]

//Hcg为相机与机器人执行器末端之间的相对位置姿态齐次矩阵。

void Navy_HandEye(Mat Hcg, vector<Mat> Hgij, vector<Mat> Hcij)
{
    CV_Assert(Hgij.size() == Hcij.size());
    int nStatus = Hgij.size();

    Mat Rgij(3, 3, CV_64FC1);
    Mat Rcij(3, 3, CV_64FC1);

    Mat alpha1(3, 1, CV_64FC1);
    Mat beta1(3, 1, CV_64FC1);
    Mat alpha2(3, 1, CV_64FC1);
    Mat beta2(3, 1, CV_64FC1);
    Mat A(3, 3, CV_64FC1);
    Mat B(3, 3, CV_64FC1);

    Mat alpha(3, 1, CV_64FC1);
    Mat beta(3, 1, CV_64FC1);
    Mat M(3, 3, CV_64FC1, Scalar(0));

    Mat MtM(3, 3, CV_64FC1);
    Mat veMtM(3, 3, CV_64FC1);
    Mat vaMtM(3, 1, CV_64