工业机器人手眼标定（眼在手）方程AX=XB采用Tsai两步法进行求解的过程并分析

参考博客：
https://blog.csdn.net/yunlinwang/article/details/51622143

该博客内容基本把Tsai求解该方程的过程已经阐述清楚了。只是没有写一个反对称矩阵，虽然简单，还是给出可以直接运行的程序吧！！！下面这个程序在配置好Opencv环境之后就可以直接运行。

1. Rodrigues(Rgij, rgij);该函数是根据旋转矩阵求旋转向量，即罗德里格斯公式
2. theta_gij = norm(rgij);该函数是求向量的2范数，也就是求向量的模
3. skew(Pgij + Pcij);（自定义），求一个向量的反对称矩阵。
   具体原理可以参考上面博客。以下只给出直接运行的代码。

需要注意的是：该方法求解该方程只适合A和B是位姿矩阵的形式，旋转矩阵加平移向量。旋转矩阵部分满足旋转矩阵的约束条件，向量单位化且两两正交

#include<iostream>
#include <vector>
#include <functional>
#include <algorithm>
#include <string>
#include<map>
#include <opencv2/opencv.hpp>
using namespace cv;
using namespace std;
#define PRINT_INT(x, y) x#y
Mat skew( Mat res )
{
Mat result = (Mat_<double>(3, 3) << 0, -res.at<double>(2), res.at<double>(1),
res.at<double>(2), 0, -res.at<double>(0),
-res.at<double>(1), res.at<double>(0), 0);

return result;
}

void Tsai_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 rgij(3, 1, CV_64FC1);
Mat rcij(3, 1, CV_64FC1);

double theta_gij;
double theta_cij;

Mat rngij(3, 1, CV_64FC1);
Mat rncij(3, 1, CV_64FC1);

Mat Pgij(3, 1, CV_64FC1);
Mat Pcij(3, 1, CV_64FC1);

Mat tempA(3, 3, CV_64FC1);
Mat tempb(3, 1, CV_64FC1);

Mat A;
Mat b;
Mat pinA;

Mat Pcg_prime(3, 1, CV_64FC1);
Mat Pcg(3, 1, CV_64FC1);
Mat PcgTrs(1, 3, CV_64FC1);

Mat Rcg(3, 3, CV_64FC1);
Mat eyeM = Mat::eye(3, 3, CV_64FC1);

Mat Tgij(3, 1, CV_64FC1);
Mat Tcij(3, 1, CV_64FC1);

Mat tempAA(3, 3, CV_64FC1);
Mat tempbb(3, 1, CV_64FC1);
Mat AA;
Mat bb;
Mat pinAA;

Mat Tcg(3, 1, CV_64FC1);

for (int i = 0; i < nStatus; i++)
{
Hgij[i](Rect(0, 0, 3, 3)).copyTo(Rgij);
Hcij[i](Rect(0, 0, 3, 3)).copyTo(Rcij);

Rodrigues(Rgij, rgij);
Rodrigues(Rcij, rcij);

theta_gij = norm(rgij);
theta_cij = norm(rcij);

rngij = rgij / theta_gij;
rncij = rcij / theta_cij;

Pgij = 2 * sin(theta_gij / 2) * rngij;
Pcij = 2 * sin(theta_cij / 2) * rncij;

tempA = skew(Pgij + Pcij);
tempb = Pcij - Pgij;

A.push_back(tempA);
b.push_back(tempb);
}

//Compute rotation
invert(A, pinA, DECOMP_SVD);

Pcg_prime = pinA * b;
Pcg = 2 * Pcg_prime / sqrt(1 + norm(Pcg_prime) * norm(Pcg_prime));
PcgTrs = Pcg.t();
Rcg = (1 - norm(Pcg) * norm(Pcg) / 2) * eyeM + 0.5 * (Pcg * PcgTrs + sqrt(4 - norm(Pcg)*norm(Pcg))*skew(Pcg));

//Computer Translation
for (int i = 0; i < nStatus; i++)
{
Hgij[i](Rect(0, 0, 3, 3)).copyTo(Rgij);
Hcij[i](Rect(0, 0, 3, 3)).copyTo(Rcij);
Hgij[i](Rect(3, 0, 1, 3)).copyTo(Tgij);
Hcij[i](Rect(3, 0, 1, 3)).copyTo(Tcij);

tempAA = Rgij - eyeM;
tempbb = Rcg * Tcij - Tgij;

AA.push_back(tempAA);
bb.push_back(tempbb);
}

invert(AA, pinAA, DECOMP_SVD);
Tcg = pinAA * bb;
cout << Rcg << endl;
Rcg.copyTo(Hcg(Rect(0, 0, 3, 3)));
Tcg.copyTo(Hcg(Rect(3, 0, 1, 3)));
Hcg.at<double>(3, 0) = 0.0;
Hcg.at<double>(3, 1) = 0.0;
Hcg.at<double>(3, 2) = 0.0;
Hcg.at<double>(3, 3) = 1.0;

}

void testMat(Mat res)
{
res.at<double>(0, 2) = 199;
}

int main(int argc,char *agrv[])
{
vector<Mat> res1, res2;
Mat a1 = (Mat_<double>(4, 4) <<
0.9397,         0,    0.3420, 20.56,
0.2418 ,   0.7071, - 0.6645, 10.26,
-0.2418,    0.7071,    0.6645, 5.23,
0, 0, 0, 1);
Mat b1 = (Mat_<double>(4, 4) <<
0.6964, - 0.7071,    0.1228, 30.56,
0.6964 ,   0.7071  ,  0.1228, 10.26,
-0.1736,         0 ,   0.9848, 5.20,
0, 0, 0, 1);

res1.push_back(a1);
res2.push_back(b1);
Mat result = (Mat_<double>(4, 4));
Tsai_HandEye(result, res1, res2);
cout << result << endl;
}