3D点集之间计算转移矩阵，旋转R，转移T，新增缩放s

1.问题描述：

给点两个d维空间中的点集合

常用的转换关系可能包括以下几种：
（1）等距变换。需要求R和T。等距变换前后长度，面积，线之前的角度不变。常见于坐标转换、物体移动，姿态变化等。自由度6（3+3）

（2）相似变换。需要求R和T及缩放系数s。 常用于非刚体移动，或不同目标匹配对齐，目标缩放等。自由度7（6+1）

（3）仿射变换（正交投影）。平移变换T 和非均匀变换（A）的复合，A是可逆矩阵，不要求是正交矩阵。仿射变换对图像旋转+平移+缩放+切变（shear，也叫倾斜变换），相比前两种，变换图像的形状发生了改变，但是原图中平行线仍然保持平行。自由度12（9+3）

(4)射影变换（透视变换）
射影变换的不变量：重合关系、长度的交比，自由度：15（右下角v=1）
射影变换是对图像的旋转+平移+缩放+切边+射影。平行线变换后可能不再平行，而是交于一点。如平视图转换为鸟瞰图。

关于二维空间（图像）的变化可参考：基础知识：二维常见变换

2.1 等距变换求解

其求解数学表达式如下：

wi 表示每个点对之前的权重。
对于该问题共有6个自由度，至少需要两组点对，对矩阵求最小二乘解，该方法我们暂不讨论。
主流的做法是使用去中心化点集协方差矩阵SVD求解，步骤如下：
（1）构建上述问题模型：


（2）去中心化及协方差矩阵SVD分解求
计算中心值：

点集去中心化，去除转移矩阵的影响：

计算协方差矩阵：

SVD分解，求旋转矩阵

（3）计算转移矩阵

具体推导过程可参见：

利用SVD求得两个对应点集合的旋转矩阵R和转移矩阵t的数学推导
Least-Squares Rigid Motion Using SVD

python 代码参考 计算两个对应点集之间的旋转矩阵R和转移矩阵T
如下：

from numpy import *
from math import sqrt

def estimate_quilong_transform_3D(A, B):
    assert len(A) == len(B)
    N = A.shape[0];
    mu_A = mean(A, axis=0)
    mu_B = mean(B, axis=0)

    AA = A - tile(mu_A, (N, 1))
    BB = B - tile(mu_B, (N, 1))
    H = transpose(AA) * BB

    U, S, Vt = linalg.svd(H)
    R = Vt.T * U.T

    if linalg.det(R) < 0:
        print "Reflection detected"
        Vt[2, :] *= -1
        R = Vt.T * U.T

    t = -R * mu_A.T + mu_B.T

    return R, t

2.2 相似变换求解

在等距变换的基础上，点集P、Q的比例不一致。典型的应用为：3D模型融合，比如将给一个人头3D模型加上眼镜3d模型，两模型尺寸不一致，模型匹配时我们在人头和眼镜上各选取4个关键点，求解相似变换矩阵。
点集合P、Q的比例s不影响旋转矩阵R的求解，但是会影响T的结果。这是因为放大缩小是基于某一个参考坐标进行的，默认情况下对集合P的坐标乘以s进行缩放，等效于参考坐标为集合P的原点，那么集合P的中心位置也等效于乘了s，对应到T有平移分量。更合理的做法是将集合P的中心移到坐标原点，然后乘以缩放系数。
相似变换求解的难点通常在于点集P、Q的选取较为随意，无严格定位和对齐，因此估计出来的变化矩阵误差较大。
我们依然使用去中心化点集协方差矩阵SVD求解相似变换矩阵，
（1）估计出缩放系数s
首先计算点集合P、Q中所有点连成的线，线段个数为：n*(n-1)/2，然后计算所有线段的Ld阶长度。然后对两集合中所有线段长度相除得到n*(n-1)/2缩放系数。去除过短的线段和异常的缩放系数，剩下的取平均即可得到缩放系数的估计s。

（2）旋转矩阵R与转移矩阵T
旋转矩阵R求解过程与2.1相同。
不同于2.1的是，在2.1中计算转移矩阵时加入s

代码如下：

def get_all_side_length(points):
	all_dis=[]
	for i in range(len(points)-1):
		for j in range(i+1,len(points))：
			all_dis.append(points[i]-points[j])
	all_dis=np.array(all_dis)
	return np.linalg.norm(all_dis,axis=1)
	
def get_scale(A,B):
	dis_A=get_all_side_length(np.array(A))
	dis_B=get_all_side_length(np.array(B))
	scale=np.abs(dis_B/dis_A）
	mask=np.abs(scale)>0.0001  
	scale_sort=np.sort(scale[mask].reshape(-1))
	d_n=len(scale_sort)
	s_mean=scale_sort[int(d_n/4):int(d_n*3/4)].mean() #only use medium data
	return s_mean

def estimate_similarity_transform_3D(A, B):
	## 求解R 
    assert len(A) == len(B)
    N = A.shape[0];
    mu_A = mean(A, axis=0)
    mu_B = mean(B, axis=0)

    AA = A - tile(mu_A, (N, 1))
    BB = B - tile(mu_B, (N, 1))
    H = transpose(AA) * BB

    U, S, Vt = linalg.svd(H)
    R = Vt.T * U.T

    if linalg.det(R) < 0:
        print "Reflection detected"
        Vt[2, :] *= -1
        R = Vt.T * U.T

	s_mean=get_scale(A,B)

    t = -s_mean * R * mu_A.T + mu_B.T

    return R, t	,s_mean

2.3 仿射变换求解

仿射变换A的求解是典型的最小二乘求解方法，我们使用np.linalg.lstsq()方法。
自由度12，至少需要4对三维点（不共线）
代码：

def estimate_affine_transform_3D(A, B):
	'''
	A:[n,3] B[n,3]
	return:
	P_Affine:(3,4) the third row is [0,0,0,1]
	'''
	A_homo=np.hstack((A,np.ones([A.shape[0],1]))) #nx4
	P=np.linalg.lstsq(A_homo,Y)[0].T  #Affine matrix 3 x 4
	return P

def P2sRt(P)
	''' decompositing camera matrix P
		P:(3,4) Affine Camera Matrix
	Returns:
		s:
		R:(3,3) rotation matrix
		t:(3,) translation
	'''
	t=P[:,3]
	R1=P[0:1,:3]
	R2=P[1:2,:3]
	s=(np.linalg.norm(R1) + np.linalg.norm(R2))/2.0
	r1=R1/np.linalg.norm(R1)
	r2=R2/np.linalg.norm(R2)
	r3=np.cross(r1,r2) #叉乘，三维空间两向量决定的平面法向量
	
	R=np.concatenate((r1,r2,r3),0)
	return s,R,t

2.4 射影变换求解

与仿射变换求解类似