对应点集配准的四元数法（ICP算法中的一个关键步骤）参考
算法流程如下：1.首先得到同名点集P和X。2.计算P和X的均值up和ux。3.由P和X构造协方差矩阵sigma。4.由协方差矩阵sigma构造4*4对称矩阵Q。5.计算Q的特征值与特征向量。其中Q最大特征值对应的特征向量即为最佳旋转向量q。6.通过四元数q得到旋转矩阵R。7.根据R计算最佳平移向量qr。clear all;close all;clc; %生成原始点集X=[];Y=[];Z=[];for i=-180:2:180 for j=-90:2:90
