Tsai算法

Tsai算法主要用於求解手眼標定中的AX=XB問題，它屬於二階段法，先求解旋轉R後求平移t。

• 首先，求解R:
  $skew(P_{gij}+P_{cij})P'_{cg}=P_{cij}-P_{gij}$
  其中：$P=2sin\frac{\theta}{2}u^T$,$(u,\theta)$爲旋轉的Rodrigues表示$P'_{cg}=\frac{1}{\sqrt{4-|P_{cg}|^{2}}}P_{cg}$
  $skew(V)= \begin{bmatrix} 0&-v_z&v_y \\ v_z&0&-v_x\\-v_y&v_x&0 \end{bmatrix}$
  上式是AX=b形式的方程組，可以利用SVD求解出$P'_{cg}$,並通過$P_{cg}=\frac{2P'_{cg}}{\sqrt{1+|P'_{cg}|^2}}$求得$P_{cg}$。
  然後，通過
  $R=(1-\frac{|P|^2}{2})I+\frac{1}{2}(PP^T+\sqrt{4-|P|^2}skew(P))$
  得到旋轉R。

• 接下來，求解t：
  $(R_{gij}-I)T_{cg}=R_{cg}T_{cij}-T_{gij}$
  上式同樣是AX=b形式的方程組，可以解得t

代碼實現：
https://github.com/zjulion/handeyecat

GeoTransform sovleAXequalXB(std::vector<GeoTransform>& vA, std::vector<GeoTransform>& vB)
{
	GeoTransform H;
	H.setIdentity();
	if (vA.size() != vB.size())
	{
		printf("A and B must be same size.\n");
		return H;
	}

	const int n = vA.size();

	RotMat R_a, R_b;
	Geo3d r_a, r_b;

	Eigen::MatrixXf A(n*3, 3);
	Eigen::MatrixXf b(n*3, 1);
	A.setZero();
	b.setZero(); 

	for (int i = 0; i < n; ++i)
	{
		R_a = vA[i].linear();
		R_b = vB[i].linear();
		
		Geo3d rod_a = rodrigues2(R_a);
		Geo3d rod_b = rodrigues2(R_b);

		float theta_a = rod_a.norm();
		float theta_b = rod_b.norm();

		rod_a /= theta_a;
		rod_b /= theta_b;

		Geo3d P_a = 2*sin(theta_a/2)*rod_a;
		Geo3d P_b = 2*sin(theta_b/2)*rod_b;		 

		Eigen::Matrix3f rot = skew(Geo3d(P_b+P_a));
		Geo3d v = P_b - P_a;
		
		A.middleRows(3 * i, 3) = rot;
		b.middleRows(3*i, 3) = v;
	}
	// 3 by 3*n 
	Eigen::MatrixXf pinA = svdInverse(A); 

	// 3 by 1 = 3 by 3*n multi 3*n by 1
	Geo3d H_ba_prime = pinA * b;
	 
	Geo3d H_ba = 2 * H_ba_prime / sqrt(1 + lxsq(H_ba_prime.norm()));

	// 1 by 3
	Eigen::MatrixXf H_ba_Trs = H_ba.transpose();

	RotMat R_ba = (1 - lxsq(H_ba.norm()) / 2) * RotMat::Identity() 
		+ 0.5 * (H_ba * H_ba_Trs + sqrt(4 - lxsq(H_ba.norm()))*skew(H_ba));
	
	A.setZero();
	b.setZero();
	for (int i = 0; i < n; ++i)
	{
		RotMat AA = vA[i].linear() - RotMat::Identity();
		Geo3d bb = R_ba * vB[i].translation() - vA[i].translation();
		A.middleRows(3 * i, 3) = AA;
		b.middleRows(3 * i, 3) = bb;
	}
	pinA = svdInverse(A);
	Geo3d t_ba = pinA * b;
	H.linear() = R_ba;
	H.translation() = t_ba;
	std::string fname("H.txt");	
	printf("Calibration Finished. Saved at %s.\n", fname.c_str());

	cout << "H: \n" << H.matrix() << endl;
	ofstream out(fname);
	out <<"# Calibrated At " << fname  << endl;
	out << H.matrix() << endl;
	out.close(); 

	return H;
}

參考文獻：https://ieeexplore.ieee.org/document/34770