双目三维重建（已知相机参数、二维坐标，重建得到三维坐标）

已知条件：
1、相机的内参（fx、fy、cx、cy）和相机之间的转换矩阵（包括旋转矩阵和平移向量）
2、两个相机拍摄的图片上各个关键点的二维坐标（这里是21个关键点）。
目标：
重建得到 以左侧相机为原点、其位姿为坐标轴的三维空间中，各个关键点的三维坐标。

将重建得到的三维坐标，重新投影回二维，进行验证。这里只需要验证右侧相机（因为重建时以左侧相机为原点，所以左侧几乎不会有误差）。

# 相机参数
mu_0 = 287.0003014802804
mv_0 = 286.5858295260351
u0_0 = 320.00447633321426
v0_0 = 206.77214159669958
K_left = np.array([[mu_0, 0, u0_0],
                   [0, mv_0, v0_0],
                   [0, 0, 1]])

mu_1 = 285.8614011452586
mv_1 = 285.573137981048
u0_1 = 318.9629702508964
v0_1 = 205.6089527591745
K_right = np.array([[mu_1, 0, u0_1],
                    [0, mv_1, v0_1],
                    [0, 0, 1]])

# 从相机0转到相机1
rotation_between_cameras = np.array([[ 0.9999459466328783, -0.01000255192324575, -0.002837739857843686],
                                     [0.010015969823272587, 0.9999385368953055, 0.004754238127810525],
                                     [0.002790010927812807, -0.004782403862014076, 0.99998467210869]])
translation_between_cameras = np.array([ -0.11123304101780733, -3.94474957703958e-05, -0.00046830862819280834])

# 获得三维关键点坐标
depths=np.zeros((21))
points_norm_left=np.zeros((21,2))
points_norm_right=np.zeros((21,2))
for i in range(len(label3d_left)):
    x_0=label3d_left[i][0]
    y_0=label3d_left[i][1]
    x_1=label3d_right[i][0]
    y_1=label3d_right[i][1]

    point_left = np.array([x_0, y_0, 1])
    point_right = np.array([x_1, y_1, 1])

    # 从图像坐标转换到归一化相机坐标系
    point_norm_left = np.linalg.inv(K_left).dot(point_left) # 相机坐标=内参逆矩阵*像素坐标
    point_norm_right = np.linalg.inv(K_right).dot(point_right)
    # 计算视差
    disparity = point_norm_left[0] - point_norm_right[0]

    # 计算深度
    baseline = np.linalg.norm(translation_between_cameras)
    depth = baseline / disparity
    depths[i]=depth
    print("Depth:", depth)
    points_norm_left[i][0]=point_norm_left[0]*depth
    points_norm_left[i][1]=point_norm_left[1]*depth



#投影回二维验证
label3d_new = np.concatenate((points_norm_left,depths.reshape(depths.shape[0],1)),axis=1) # 以cam0为原点的世界坐标
label3d_new_left=label3d_new.copy()
label3d_new_left[:,0] = label3d_new_left[:,0]/label3d_new_left[:,2]
label3d_new_left[:,1] = label3d_new_left[:,1]/label3d_new_left[:,2]
label3d_new_left[:,2]=np.ones(21)
project2d_left = K_left.dot(label3d_new_left.T) # 像素坐标=内参*相机坐标


label3d_new_right=rotation_between_cameras.dot(label3d_new.T).T+translation_between_cameras # 以cam1为原点的世界坐标
label3d_new_right[:,0] = label3d_new_right[:,0]/label3d_new_right[:,2]
label3d_new_right[:,1] = label3d_new_right[:,1]/label3d_new_right[:,2]
label3d_new_right[:,2]=np.ones(21)
project2d_right = K_right.dot(label3d_new_right.T)

project2d_left=project2d_left.T
project2d_right=project2d_right.T
print(project2d_right[:,:2]-label3d_right[:,:2])

ps：这里没用畸变参数，可以在计算的时候再加一下畸变参数。