双目三维重建(已知相机参数、二维坐标,重建得到三维坐标)

已知条件:
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:这里没用畸变参数,可以在计算的时候再加一下畸变参数。

  • 3
    点赞
  • 5
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值