針孔相機模型與深度圖轉換成點雲

@

目錄

• 一、原理：針孔相機模型
  • 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()