二维坐标和三维坐标相互转换_matlab实现二维像素坐标与相机坐标系下三维坐标转换

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

我的需求是将二维点转换成三维点，旋转三维点，再转换为二维点，此处的旋转跟相机成像的旋转矩阵又没有关系。
假定像素坐标为(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
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17