三维空间点刚性配准

The goal here is to register two set of point extracted on a prostate. We will use rigid transformation (rotation and translation –no scaling). I suggest to use the technique proposed by Arun5 (the pdf file of this paper is given). In our case we have a two set of 6 corresponding points which re in the directory
“3-6-prostate”:
• p1 = ProstateSetPoints_1.xlsx a first subset of the real surface points.
• p2 = ProstateSetPoints_2.xlsx a second subset of the real surface points.

1. Using Arun’s method find the rotation and translation matrix which allows to align p1 on p2.
   Caution: because the two sets of points are not defined in the same manner, rigid registration
   will not exactly align the points of ProstateSetPoints_1.xls with the points of
   ProstateSetPoints_2.xlsx.
2. Describe the rotation matrix as a combination of Euler angles
3. Describe the rotation matrix as one rotation around a specific axis (Rodriguez formula or
   quaternion)
4. Align all the points of ProstateSetPoints_2.xlsx in the ProstateSetPoints_1.xlsx referential.

将6个三维点进行配准，画6个点在一张图上，红色为ProstateSetPoints_1，蓝色为ProstateSetPoints_2。

参考Arun论文《Least-Squares Fitting of Two 3-D Point Sets》中利用最小二乘法的配准方法。假设存在两个点云集合，点云配准的最终目的是通过一定的旋转和平移变换将不同坐标系下的两组或者多组点云数据统一到同一参考坐标系下。这个过程，可以通过一组刚性变换矩阵来完成。假设映射变换为H，这H可以用以下的公式来表示。

其中A代表旋转矩阵，T代表平移向量，V代表透视变换向量，S代表整体的比例因子。因为根据一系列图片得到的点云数据只存在旋转和平移变换，不存在形变，所以将V设为零向量，比例因子S=1。

映射变换H可以表示为：

其中，旋转矩阵R3X3和平移矩阵T3X1可以通过以下公式来表示：

其中α、β、γ分别表示点沿x、y、z轴的旋转角度，tx、ty、tz分别表示点沿x、y、z轴的平移量。

刚性变换矩阵中涉及到六个未知数α、β、γ、 tx、ty、tz。要唯一确定这六个未知参数，需要六个线性方程，即至少需要在待匹配点云重叠区域找到3组对应点对，且3组对应点对不能共线，才可以得到这几个未知数的值，进而完成刚性矩阵的参数估计。通常情况下，人们会选择尽可能多的对应点对，进一步提高刚性变换矩阵的参数估计精度。

旋转矩阵：

平移矩阵：

配准结果：

代码：

clear all;
close all;
clc;
 
X=[-1.5,49.5,0.5,-53.5,5.5,-0.5]
Y=[39.66,0.66,-40.33,7.66,-7.33,-0.33]
Z=[-0.5,-0.5,-0.5,-0.5,48.5,-46.5]
 
X2=[152.98,207.1,155.65,102.61,158.34,150.97]
Y2=[206.94,172.77,129.1,171.72,160.38,174.07]
Z2=[57.51,50.77,51.3,58.1,97.61,8.01]
 
P=[X' Y' Z']
X =[X2' Y2' Z2']
plot3(P(:,1),P(:,2),P(:,3),'b.');
hold on;
plot3(X(:,1),X(:,2),X(:,3),'r.');
 
up = mean(P);
ux = mean(X);
 
P1=P-up;
X1=X-ux;
 
sigma=P1'*X1/(length(X1));
 
[u s v] = svd(sigma);
RR=u*v';
 
qr=ux-up*RR;
 
Pre = P*RR+qr;
 
figure;
plot3(P(:,1),P(:,2),P(:,3),'b.');
hold on;
plot3(X(:,1),X(:,2),X(:,3),'r.');
plot3(Pre(:,1),Pre(:,2),Pre(:,3),'go');