相机模型
符号定义： C P a ^CP_a CPa表示 P a P_a Pa点在坐标系 C C C中的坐标， I C T ^C_IT ICT表示坐标系 I I I相对于坐标系 C C C的坐标变换。设B点为主轴与相平面之间的交点，相机坐标系 C C C中点 P o P_o Po坐标为： ( P o x , P o y , P o z ) T (P_{ox}, P_{oy}, P_{oz})^T (Pox,Poy,Poz)T，P点为线段 P o C P_oC PoC与相平面的交点，则易得 P B ⊥ B C PB \perp BC PB⊥BC， Δ P o A C \Delta P_oAC ΔPoAC与 Δ P B C \Delta PBC ΔPBC相似，得P点在相机坐标系C中的坐标 C P ^CP CP为 f P o z ( P o x , P o y , P o z ) T \frac{f}{P_{oz}}(P_{ox}, P_{oy}, P_{oz})^T Pozf(Pox,Poy,Poz)T，设 I P = ( u , v , 0 ) ^IP=(u, v, 0) IP=(u,v,0)。
推导1. 当相平面原点在主轴上
相平面原点在主轴上即，相平面原点在相机坐标为
(
0
,
0
,
f
)
T
(0, 0, f)^T
(0,0,f)T，容易得到：
C
T
I
=
(
1
0
0
0
0
1
0
0
0
0
1
f
0
0
0
1
)
C
T
I
−
1
=
(
1
0
0
0
0
1
0
0
0
0
1
−
f
0
0
0
1
)
^CT_I = \left( \begin{array}{cccc} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & f \\ 0 & 0 & 0 & 1 \end{array} \right) \\ ^CT_I^{-1} = \left( \begin{array}{cccc} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & -f \\ 0 & 0 & 0 & 1 \end{array} \right)
CTI=⎝⎜⎜⎛10000100001000f1⎠⎟⎟⎞CTI−1=⎝⎜⎜⎛10000100001000−f1⎠⎟⎟⎞
则由坐标变换得：
I
P
=
C
I
T
C
P
^IP=^I_CT^CP
IP=CITCP，计算得：
(
u
v
0
)
=
f
P
o
z
(
P
o
x
P
o
y
0
)
\left( \begin{array}{c} u \\ v \\ 0 \end{array} \right) = \frac{f}{P_{oz}} \left( \begin{array}{c} P_{ox} \\ P_{oy} \\ 0 \end{array} \right)
⎝⎛uv0⎠⎞=Pozf⎝⎛PoxPoy0⎠⎞
推导2. 当相平面原点不在主轴上
设相平面原点在相机坐标为
(
c
x
,
c
y
,
f
)
T
(c_x, c_y, f)^T
(cx,cy,f)T，注意
c
x
,
c
y
c_x, c_y
cx,cy的定义，容易得到：
C
T
I
=
(
1
0
0
c
x
0
1
0
c
y
0
0
1
f
0
0
0
1
)
C
T
I
−
1
=
(
1
0
0
−
c
x
0
1
0
−
c
y
0
0
1
−
f
0
0
0
1
)
^CT_I = \left( \begin{array}{cccc} 1 & 0 & 0 & c_x \\ 0 & 1 & 0 & c_y \\ 0 & 0 & 1 & f \\ 0 & 0 & 0 & 1 \end{array} \right) \\ ^CT_I^{-1} = \left( \begin{array}{cccc} 1 & 0 & 0 & -c_x \\ 0 & 1 & 0 & -c_y \\ 0 & 0 & 1 & -f \\ 0 & 0 & 0 & 1 \end{array} \right)
CTI=⎝⎜⎜⎛100001000010cxcyf1⎠⎟⎟⎞CTI−1=⎝⎜⎜⎛100001000010−cx−cy−f1⎠⎟⎟⎞
同理推导得到：
(
u
v
0
)
=
f
P
o
z
(
P
o
x
P
o
y
0
)
+
(
−
c
x
−
c
y
0
)
\left( \begin{array}{c} u \\ v \\ 0 \end{array} \right) = \frac{f}{P_{oz}} \left( \begin{array}{c} P_{ox} \\ P_{oy} \\ 0 \end{array} \right) + \left( \begin{array}{c} -c_x \\ -c_y \\ 0 \end{array} \right)
⎝⎛uv0⎠⎞=Pozf⎝⎛PoxPoy0⎠⎞+⎝⎛−cx−cy0⎠⎞
反之由坐标变换得：
C
P
=
I
C
T
I
P
^CP=^C_IT^IP
CP=ICTIP，计算得：
(
P
o
x
P
o
y
P
o
z
)
=
P
o
z
f
(
u
+
c
x
v
+
c
y
f
)
\left( \begin{array}{c} P_{ox} \\ P_{oy} \\ P_{oz} \end{array} \right) = \frac{P_{oz}}{f} \left( \begin{array}{c} u + c_x\\ v + c_y \\ f \end{array} \right)
⎝⎛PoxPoyPoz⎠⎞=fPoz⎝⎛u+cxv+cyf⎠⎞
整理为矩阵形式得：
P
o
z
(
u
v
1
)
=
(
f
0
−
c
x
0
f
−
c
y
0
0
1
)
⏟
K
(
1
0
0
0
0
1
0
0
0
0
1
0
)
⏟
(
I
∣
0
)
(
P
o
x
P
o
y
P
o
z
1
)
P_{oz}\left( \begin{array}{c} u \\ v \\ 1 \end{array} \right) = \underbrace{\left(\begin{array}{ccc} f & 0 & -c_{x} \\ 0 & f & -c_{y} \\ 0 & 0 & 1 \end{array}\right)}_{K} \underbrace{\left(\begin{array}{llll} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \end{array}\right)}_{(I \mid 0)} \left( \begin{array}{c} P_{ox} \\ P_{oy} \\ P_{oz} \\ 1 \end{array} \right)
Poz⎝⎛uv1⎠⎞=K
⎝⎛f000f0−cx−cy1⎠⎞(I∣0)
⎝⎛100010001000⎠⎞⎝⎜⎜⎛PoxPoyPoz1⎠⎟⎟⎞
矩阵K称为相机内参矩阵。若考虑世界坐标与相机坐标的坐标变换
W
C
T
^C_WT
WCT，则世界坐标的点
W
P
=
(
X
,
Y
,
Z
,
1
)
T
^WP=(X, Y, Z, 1)^T
WP=(X,Y,Z,1)T与相平面点坐标的关系可表示为：
C
Z
(
u
v
1
)
=
[
f
0
−
c
x
0
f
−
c
y
0
0
1
]
[
W
C
R
∣
W
C
t
]
(
X
Y
Z
1
)
^CZ \left( \begin{array}{c} u \\ v \\ 1 \end{array} \right) = \left[\begin{array}{ccc} f & 0 & -c_{x} \\ 0 & f & -c_{y} \\ 0 & 0 & 1 \end{array}\right][^C_WR \mid ^C_Wt]\left(\begin{array}{l} X \\ Y \\ Z \\ 1 \end{array}\right)
CZ⎝⎛uv1⎠⎞=⎣⎡f000f0−cx−cy1⎦⎤[WCR∣WCt]⎝⎜⎜⎛XYZ1⎠⎟⎟⎞
C
Z
^CZ
CZ为相机坐标系的坐标表示。到这里所有的坐标系都表征物理单位，如果将像平面的坐标变换到像素平面，设像平面坐标系与像素平面坐标系重合，需要做再做一次尺度变换。设每行/每列像素对应的像坐标系的物理间隔分别为
s
u
,
s
v
s_u, s_v
su,sv，则存在以下变换：
(
i
j
1
)
=
[
1
s
u
0
0
0
1
s
v
0
0
0
1
]
(
u
v
1
)
\left( \begin{array}{c} i \\ j \\ 1 \end{array} \right) = \left[\begin{array}{ccc} \frac{1}{s_u} & 0 & 0 \\ 0 & \frac{1}{s_v} & 0 \\ 0 & 0 & 1 \end{array}\right] \left( \begin{array}{c} u \\ v \\ 1 \end{array} \right)
⎝⎛ij1⎠⎞=⎣⎡su1000sv10001⎦⎤⎝⎛uv1⎠⎞
综上，最终的像素坐标与世界坐标间的变换关系为：
C
Z
(
i
j
1
)
=
[
1
s
u
0
0
0
1
s
v
0
0
0
1
]
[
f
0
−
c
x
0
f
−
c
y
0
0
1
]
[
W
C
R
∣
W
C
t
]
(
X
Y
Z
1
)
^CZ \left( \begin{array}{c} i \\ j \\ 1 \end{array} \right) = \left[\begin{array}{ccc} \frac{1}{s_u} & 0 & 0 \\ 0 & \frac{1}{s_v} & 0 \\ 0 & 0 & 1 \end{array}\right] \left[\begin{array}{ccc} f & 0 & -c_{x} \\ 0 & f & -c_{y} \\ 0 & 0 & 1 \end{array}\right][^C_WR \mid ^C_Wt]\left(\begin{array}{l} X \\ Y \\ Z \\ 1 \end{array}\right)
CZ⎝⎛ij1⎠⎞=⎣⎡su1000sv10001⎦⎤⎣⎡f000f0−cx−cy1⎦⎤[WCR∣WCt]⎝⎜⎜⎛XYZ1⎠⎟⎟⎞
若与其他推导出现公式上的差异，请注意这里的 c x , c y c_x, c_y cx,cy定义。
pybullet中camera实现
camera在世界坐标系中放置位置如下图所示
代码参考：panda_grasp_sim_2
class Camera:
def __init__(self):
"""
初始化相机参数，计算相机内参
"""
self.fov = 60*math.pi/180
self.length = 0.7 # 焦距
self.H = self.length * math.tan(0.5*self.fov) # 图像上方点的到中心点的实际距离(height) m
# 计算 f/su和 f/sv
self.A = (HEIGHT / 2) * self.length / self.H
# 计算内参矩阵（注意上文中Cx, Cy单位为m）
self.InMatrix = np.array([[self.A, 0, WIDTH/2 - 0.5], [0, self.A, HEIGHT/2 - 0.5], [0, 0, 1]], dtype=np.float)
# 计算相机坐标系相对于世界坐标系的变换矩阵
# 欧拉角xyz: (pi, 0, 0) 平移(0, 0, 0.7)
rotMat = eulerAnglesToRotationMatrix([math.pi, 0, 0])
self.transMat = getTransfMat([0, 0, 0.7], rotMat)
"""
helper functions
"""
def img2camera(self, pt, dep):
"""
获取像素点pt在相机坐标系中的坐标
pt: [x, y]
dep: 深度值
return: [x, y, z]
"""
pt_in_img = np.array([[pt[0]], [pt[1]], [1]], dtype=np.float)
ret = np.matmul(np.linalg.inv(self.InMatrix), pt_in_img) * dep
return list(ret.reshape((3,)))
# print('坐标 = ', ret)
def camera2img(self, coord):
"""
将相机坐标系中的点转换至图像
coord: [x, y, z]
return: [row, col]
"""
z = coord[2]
coord = np.array(coord).reshape((3, 1))
rc = (np.matmul(self.InMatrix, coord) / z).reshape((3,))
return list(rc)[:-1]
def camera2world(self, coord):
"""
获取相机坐标系中的点在世界坐标系中的坐标
corrd: [x, y, z]
return: [x, y, z]
"""
coord.append(1.)
coord = np.array(coord).reshape((4, 1))
coord_new = np.matmul(self.transMat, coord).reshape((4,))
return list(coord_new)[:-1]
def world2camera(self, coord):
"""
获取世界坐标系中的点在相机坐标系中的坐标
corrd: [x, y, z]
return: [x, y, z]
"""
coord.append(1.)
coord = np.array(coord).reshape((4, 1))
coord_new = np.matmul(np.linalg.inv(self.transMat), coord).reshape((4,))
return list(coord_new)[:-1]
def world2img(self, coord):
"""
获取世界坐标系中的点在图像中的坐标
corrd: [x, y, z]
return: [row, col]
"""
# 转到相机坐标系
coord = self.world2camera(coord)
# 转到图像
pt = self.camera2img(coord) # [y, x]
return [int(pt[1]), int(pt[0])]
def img2world(self, pt, dep):
"""
获取像素点的世界坐标
pt: [x, y]
dep: 深度值 m
"""
coordInCamera = self.img2camera(pt, dep)
return self.camera2world(coordInCamera)
4668
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
