针孔相机模型与深度图转换成点云

@

目录

• 一、原理：针孔相机模型
  • 1.1 针孔相机模型推导
  • 1.2 实例
• 二、深度图转换成点云

一、原理：针孔相机模型

1.1 针孔相机模型推导

相机结构一般用如下透镜模型解释， 穿过镜头中心的光线没有改变方向,镜头中心称为“光心”

透镜模型可以简化为针孔模型——将透镜替换成小孔

• 光心对应小孔中心

一般为了分析简单,将成像平面画在对称位置,这样图像不再颠倒。
空间的3D点和图像传感器上的位置关系通过下面的图给出

• 传感器平面上的图像点看成是从空间点(X,Y,Z)到原点的连线和传感器平面的交点
  
  利用相似三角形能够看出图像传感器平面上的像素位置和3D空间点的位置关系
  
  

下面以矩阵形式给出相机图像和3D坐标之间的关系

1.2 实例

二、深度图转换成点云

已知每一个像素点的深度就可以用上面的公式转换成点云，代码如下

import numpy as np

# 加载深度数据
img = np.genfromtxt('img_dep_640x480.csv', delimiter=',').astype(np.float32)

# 参数
CAM_WID, CAM_HGT = 640, 480
CAM_FX, CAM_FY = 795.209, 793.957
CAM_CX, CAM_CY = 332.031, 231.308

# 转换
x, y = np.meshgrid(range(CAM_WID), range(CAM_HGT))
x = x.astype(np.float32) - CAM_CX
y = y.astype(np.float32) - CAM_CY

img_z = img.copy()
if False:  # 如果需要矫正视线到Z的转换的话使能
    f = (CAM_FX + CAM_FY) / 2.0
    img_z *= f / np.sqrt(x ** 2 + y ** 2 + f ** 2)

pc_x = img_z * x / CAM_FX  # X=Z*(u-cx)/fx
pc_y = img_z * y / CAM_FY  # Y=Z*(v-cy)/fy

pc = np.array([pc_x.ravel(), pc_y.ravel(), img_z.ravel()]).T

# 结果保存
np.savetxt('pc.csv', pc, fmt='%.18e', delimiter=',', newline='\n')

# 从CSV文件加载点云并显示
pc = np.genfromtxt('pc.csv', delimiter=',').astype(np.float32)

import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

ax = plt.figure(1).gca(projection='3d')
ax.plot(pc[:, 0], pc[:, 1], pc[:, 2], 'b.', markersize=0.5)
plt.title('point cloud')
plt.show()