[手眼标定] 工业机器人手眼标定（眼在手）方程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;
}