基于参考点的点云对齐（Matlab&Eigen）

1. 背景

立体视觉中，为了将不同位姿下重建的模型转换到同一个坐标系中，通常会在物体上或周围粘贴圆形标志点，根据3对以上圆形标志点的坐标，解算两个位姿的相对关系。

2. 理论基础

第一个角度的标志点三维坐标为 P = { p 1 , p 2 , . . . , p n } P=\{p_1,p_2,...,p_n\} P={p1​,p2​,...,pn​},第二角度对应点的三维坐标为 Q = { q 1 , q 2 , . . . q n } Q=\{q_1,q_2,...q_n\} Q={q1​,q2​,...qn​}. 粘贴标志点正是利于我们找到对应的点，不再像ICP一样利用最近邻点去寻找对应点。我们的目的就是求解两个角度的刚体变换RT矩阵。用数学表达式表示为：$$RT=argmin\sum _{i=1}^n(w_i||R{p}_i+T-q_i|{|}^2)$$关于该部分的推导过程，可参照：通过SVD分解求解RT
这里主要提醒该推导后没有提及的部分，即反射修正。 该过程计算的R矩阵一定是正交矩阵，但不一定是旋转矩阵，它还可能是反射变换，可以通过矩阵的行列式来判断，若行列式是-1，则是反射变换的矩阵。若行列式是+1，则是旋转矩阵。

3. 实现代码

这里主要给出MATLAB以及C++使用EIGEN库的实现过程，这里考虑了权重因素，若不考虑，则权重平均分配即可。

3.1 Matlab：

clear;clc;
data1 = [-37.0991 -72.3808 -662.87 0.0204913;
	87.7909 -71.975 -686.096 0.0360159;
	22.8645 -10.0038 -743.771 0.0292969];
data2 = [-171.164 -14.6164 -660.198 0.0126706;
	-47.77 -8.08739 -689.577 0.0017593;
	-118.297 51.3259 -743.167 0.002179];
w = [0.334814
0.298856
0.36633];
W = diag(w);
P = data2(:,1:3)';
Q = data1(:,1:3)';
mean_P = P * w;
mean_Q = Q * w;
p = P-repmat(mean_P,1,3);
q = Q-repmat(mean_Q,1,3);
H = p * W * q';
[U,S,V] = svd(H);
Det = det(V*U');
M = [1,0,0;
    0,1,0;
    0,0,Det];
R = V * M * U';
T = (-1)*R*mean_P+mean_Q;

3.2 C++基于Eigen库：

vs完整工程：https://download.csdn.net/download/weixin_43956164/15735568
github: https://github.com/yuleaf612/SolRtByEigen

int solTRByEigen(double dR[][3], double *dT, double(*vPtp)[3], double(*vPtq)[3], double *Wi, int nNum)
{ 
	using namespace Eigen;
	MatrixXd AvgP, AvgQ;
	AvgP = MatrixXd::Zero(3, 1);
	AvgQ = MatrixXd::Zero(3, 1);
	for (int i = 0; i< nNum; i++)
	{
		AvgP(0, 0) += Wi[i] * vPtp[i][0];
		AvgP(1, 0) += Wi[i] * vPtp[i][1];
		AvgP(2, 0) += Wi[i] * vPtp[i][2];

		AvgQ(0, 0) += Wi[i] * vPtq[i][0];
		AvgQ(1, 0) += Wi[i] * vPtq[i][1];
		AvgQ(2, 0) += Wi[i] * vPtq[i][2];
	}

	MatrixXd P(3, nNum), Q(3, nNum);
	MatrixXd W;
	W = MatrixXd::Zero(nNum, nNum);
	for (int i = 0; i < nNum; i++)
	{
		P(0, i) = vPtp[i][0] - AvgP(0, 0);
		P(1, i) = vPtp[i][1] - AvgP(1, 0);
		P(2, i) = vPtp[i][2] - AvgP(2, 0);

		Q(0, i) = vPtq[i][0] - AvgQ(0, 0);
		Q(1, i) = vPtq[i][1] - AvgQ(1, 0);
		Q(2, i) = vPtq[i][2] - AvgQ(2, 0);

		W(i,i) = Wi[i];
	}

	Matrix3d H, U, V;
	MatrixXd A;
	A = P*W;
	H = A*Q.transpose();

	JacobiSVD<MatrixXd> svd(H, ComputeThinU | ComputeThinV);
	U = svd.matrixU();
	V = svd.matrixV();

	Matrix3d R;
	R = V * U.transpose();
	if (R.determinant() < 0)
	{
		V(0, 0) = -V(0, 0);
		V(1, 0) = -V(1, 0);
		V(2, 0) = -V(2, 0);
		R = V * U.transpose();
	}
	MatrixXd T(3, 1);
	T = (-1)*R*AvgP + AvgQ;

	dR[0][0] = R(0, 0), dR[0][1] = R(0, 1), dR[0][2] = R(0, 2);
	dR[1][0] = R(1, 0), dR[1][1] = R(1, 1), dR[1][2] = R(1, 2);
	dR[2][0] = R(2, 0), dR[2][1] = R(2, 1), dR[2][2] = R(2, 2);
	dT[0] = T(0, 0), dT[1] = T(1, 0), dT[2] = T(2, 0);

	return 0;
}