二维坐标和三维坐标相互转换

原理参考世界坐标系、相机坐标系、图像坐标系、像素坐标系之间的转换

我的需求是将二维点转换成三维点，旋转三维点，再转换为二维点，此处的旋转跟相机成像的旋转矩阵又没有关系。
假定像素坐标为(u,v)，处理的输入数据是像素坐标，图像坐标为(x,y)，相机坐标为(Xc, Yc)，世界坐标为(Xw, Yw)。

此处忽略dxdy，世界坐标即是所求的三维坐标。
需要说明一下，我做的只是矩阵相乘，比较笨的方法，计算速度也很慢。
在我的代码中，去掉旋转步骤，输入的关键点和输出的关键点一致。

def _2D_to_3D_to_2D(camMatrix, zrIfI, rIt, fr, ft, keypoints):
	# camMatrix 是相机内参，此处可忽略
	# zrIfI是zC*r.I*f.I，zC是相机坐标的z轴值，r是旋转矩阵，r.I求旋转矩阵的逆
	# f是焦距矩阵，f.I求焦距矩阵的逆
	# rIt是r.I*t，r.I同上，t为平移向量
	# fr是f * r
	# ft是f * t
	# keypoint是要变换的二维关键点[u,v]、
	# 根据输入的二维关键点手动设置的z值，因为二维转换成三维的z轴是模拟的，
	# 而后续还需转换回二维（无z值），故此处设置成合适值
	zC = 4.5 
	# 旋转值
	sin_ = math.sin(45)
	cos_ = math.cos(45)
	# 输入的关键点是ntu120数据集，该数据集的keypoints有四维
	# 相当于[[[[u,v]]]]，只需改变最后一维，将二维变成三维，
	# 如代码重keypoints.shape[3]+1所示
	pk  = np.zeors(keypoints.shape[0], keypoints.shape[1], keypoints.shape[2], keypoints.shape[3]+1)
	for i, k1 in enumerate(keypoints):
		for j, k2 in enumerate(k1):
			for m, k3 in enumerate(k2):
				_kp = np.matrix([k3[0], k3[1], 1), np.float64).transpose()
				# 二维坐标转三维坐标
				_kp = zrIfI * _kp - rIt
				x = np.float64(_kp[0][0])
				y = np.float64(_kp[1][0])
				z = np.float64(_kp[2][0])
				
				# 绕y轴旋转45°
				_kp = np.array([x*cos_+z*sin_, y, z*cos_-x*sin_])
				
				# 三维坐标转二维坐标
				_pk = (fr * _pk + ft) /zC
				u = np.float64(_pk[0][0])
				v = np.float64(_pk[1][0])
				_pk = np.array([u,v])
				pk[i][j][m] = _pk
	return pk

# 相机内参
_camMatrix = np.matrix([[364.032, 0, 258.891],
						[0, 364.032, 209.32],
						[0, 0, 1]])
# 焦距矩阵
_f = np.matrix([[364.032, 0, 0],
				[0, 364.032, 0],
				[0, 0, 1]])
# 旋转矩阵
_r = np.matrix([[math.cos(0), 0, math.sin(0)],
				[0, 1, 0],
				[-math.sin(0), 0, math.cos(0)]])
# 平移向量
_t = np.matrix([134.0870803179094, 132.7580766544178, 200.3789038923399])*0.1
_t = _t.transpose()

# 求各矩阵向量的逆
rI = np.linalg.inv(_r)
fI = _f.I
zC = 4.5
zrIfI = rI * zC * fI
rIt = rI * _t
fr = _f * _r
ft = _f * _t

keypoints = load('./ntu120.kpl')
kp = _2D_to_3D_to_2D(camMatrix, zrIfI, rIt, fr, ft, keypoints)						
print(kp)						

ntu数据集使用Kinect V2相机，相机参数获取参考以下博文：
kinect v2详细参数和原理介绍
kinect 2.0 学习笔记_深度相机内参