Tsai算法

最新推荐文章于 2023-03-03 00:27:59 发布

置顶 td092

最新推荐文章于 2023-03-03 00:27:59 发布

阅读量2.1k

点赞数

分类专栏：机器人

欢迎交流，有评论就更

本文链接：https://blog.csdn.net/weixin_38258767/article/details/104960500

版权

机器人专栏收录该内容

63 篇文章 9 订阅

订阅专栏

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}$
$\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;
}