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分解求
计算中心值:
3D点集之间计算转移矩阵,旋转R,转移T,新增缩放s (总结全面)_第1张图片
点集去中心化,去除转移矩阵的影响:
在这里插入图片描述
计算协方差矩阵:
3D点集之间计算转移矩阵,旋转R,转移T,新增缩放s (总结全面)_第2张图片
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 射影变换求解

与仿射变换求解类似

你可能感兴趣的:(3D视觉,多视图几何,计算机视觉,转移矩阵)