引用资料（理论部分其实就是把第一个的不详细和错误的地方说了一下，翻译了一下第二个文献以及不明了的地方说一下O(∩_∩)O哈哈~）：
高翔《视觉SLAM十四讲》
K. S. ARUN, T. S. HUANG, AND S. D. BLOSTEIN
Least-Squares Fitting of Two 3-D Point Sets
问题描述
假设存在两个点云集合和
求：一个欧氏变换使得
求解问题
解：
假设误差项为
那么问题转化为优化问题：
定义质心为：
那么有：
因为
所以问题转化为：
因为左右两边都大于等于零，而且左边只和相关，可以先求出R在利用R求解第二项
那么按照书里计算过程
- 计算两组质心位置p,p',然后计算每个点的去质心坐标：
2.根据以下优化问题计算旋转矩阵：
3.根据2的结果计算t
展开关于R的误差项有：
因为第一项与R无管，第二项由于与R也无关那么问题转化为
令
因为问题是求解
即：
假设最优解
那么
（因为R是正交矩阵）
对H进行SVD分解
可以得到
那么
令
因为
所以
是
最优解
现在只要证明
是A的第i列，因为那么有
根据Schwarz不等式
即
注意这个计算需要H是满秩，
几个情况需要考虑
1.H是满秩，上的点非共平面
2.上的点共平面，可以对H求出的解的为0特征值的特征向量计算取反，使得
3.上的点共线，不能用SVD求解
代码
//这个是将点云dstPoint利用RT配到srcPoint上的 srcPoint=dstPoint*R+T
void registrateNPoint(std::vector& srcPoints,std::vector& dstPoints,cv::Mat&R,cv::Mat&T){
if(srcPoints.size()!=dstPoints.size()||srcPoints.size()<3||dstPoints.size()<3)
{
std::cout<<"srcPoints.size():\t"<(0,i)=srcPoints[i].x - srcCentricPt.x;
srcMat.at(1,i)=srcPoints[i].y - srcCentricPt.y;
srcMat.at(2,i)=srcPoints[i].z - srcCentricPt.z;
dstMat.at(0,i)=dstPoints[i].x - dstCentricPt.x;
dstMat.at(1,i)=dstPoints[i].y - dstCentricPt.y;
dstMat.at(2,i)=dstPoints[i].z - dstCentricPt.z;
}
cv::Mat matS = srcMat * dstMat.t();
cv::Mat matU, matW, matV;
cv::SVDecomp(matS, matW, matU, matV);
cv::Mat matTemp = matU * matV;
double det = cv::determinant(matTemp);
double datM[] = {1, 0, 0, 0, 1, 0, 0, 0, det};
cv::Mat matM(3, 3, CV_64FC1, datM);
cv::Mat matR = matV.t() * matM * matU.t();
double tx,ty,tz;
tx = dstCentricPt.x- (srcCentricPt.x* matR.at(0,0) + srcCentricPt.y* matR.at(0,1) + srcCentricPt.z* matR.at(0,2));
ty = dstCentricPt.y- (srcCentricPt.x* matR.at(1,0) + srcCentricPt.y* matR.at(1,1) + srcCentricPt.z * matR.at(1,2));
tz = dstCentricPt.z- (srcCentricPt.x* matR.at(2,0) + srcCentricPt.y* matR.at(2,1) + srcCentricPt.z * matR.at(2,2));
double datT[]={tx,ty,tz};
cv::Mat matT(3, 1, CV_64F,datT);
matR.copyTo(R);
matT.copyTo(T);
}