如图所示，坐标
O
c
−
x
c
y
c
z
c
O_c-x_cy_cz_c
Oc−xcyczc表示相机坐标系，
O
−
x
y
O-xy
O−xy表示物理成像坐标系，
O
−
u
v
O-uv
O−uv表示像素坐标系（像素坐标的原点位于图像左上角）。在相机坐标系中，
z
c
z_c
zc轴与相机的光轴重合，成像坐标系的原点位于光轴上，并且设两坐标系原点之间的距离为焦距
f
f
f；设在像素坐标系中成像坐标系原点的坐标为
(
u
0
,
v
0
)
(u_0,\ v_0)
(u0, v0)。
对于空间中一点P，设其在相机坐标系中的坐标为
P
c
(
x
c
,
y
c
,
z
c
)
P_c(x_c,\ y_c,\ z_c)
Pc(xc, yc, zc)，在成像坐标系中的坐标为
(
x
,
y
)
(x,\ y)
(x, y)，在像素坐标系中的坐标为
(
u
,
v
)
(u,\ v)
(u, v)。根据简单的几何知识，容易得到各坐标之间的转换关系：
{
x
=
f
z
c
x
c
y
=
f
z
c
y
c
(
1
)
\begin{cases} x = \frac{f}{z_c}x_c \\ y = \frac{f}{z_c}y_c \end{cases} \quad (1)
{x=zcfxcy=zcfyc(1)
{ u = x d x + u 0 v = y d y + v 0 ( 2 ) \begin{cases} u = \frac{x}{d_x} + u_0 \\ v = \frac{y}{d_y} + v_0 \end{cases} \quad (2) {u=dxx+u0v=dyy+v0(2)
{ x ˙ = f z c 2 ( z c x ˙ c − x c z ˙ c ) y ˙ = f z c 2 ( z c y ˙ c − y c z ˙ c ) ( 3 ) \begin{cases} \dot{x} = \frac{f}{z_c^2}(z_c\dot{x}_c - x_c\dot{z}_c) \\ \dot{y} = \frac{f}{z_c^2}(z_c\dot{y}_c - y_c\dot{z}_c) \end{cases} \quad (3) {x˙=zc2f(zcx˙c−xcz˙c)y˙=zc2f(zcy˙c−ycz˙c)(3)
{
u
˙
=
x
˙
d
x
v
˙
=
y
˙
d
y
(
4
)
\begin{cases} \dot{u} = \frac{\dot{x}}{d_x} \\ \dot{v} = \frac{\dot{y}}{d_y} \end{cases} \quad (4)
{u˙=dxx˙v˙=dyy˙(4)
其中，
d
x
,
d
y
d_x,\ d_y
dx, dy分别表示单位像素点在
x
,
y
x,\ y
x, y方向上的长度。
在IBVS的场景下，考虑空间点
P
P
P固定、移动相机的情况（我们需要得到
P
P
P点图像坐标变化率
[
u
˙
,
v
˙
]
T
[\dot{u},\ \dot{v}]^T
[u˙, v˙]T与相机运动速度
r
˙
=
[
v
,
ω
]
T
\dot{r} = [\pmb{v},\ \pmb{\omega}]^T
r˙=[v, ω]T之间的关系，即图像雅克比矩阵），则点
P
P
P在相机坐标系中的坐标变化率
P
˙
c
\dot{P}_c
P˙c与相机运动速度的关系可以表示为（不知道怎么来的）：
P
˙
c
=
−
ω
×
P
c
−
v
(
5
)
\dot{P}_c = -\pmb{\omega} \times P_c - \pmb{v} \quad (5)
P˙c=−ω×Pc−v(5)
对上式展开可以得到：
{
x
˙
c
=
−
v
x
−
ω
y
z
c
+
ω
z
y
c
y
˙
c
=
−
v
y
−
ω
z
x
c
+
ω
x
z
c
z
˙
c
=
−
v
z
−
ω
x
y
c
+
ω
y
x
c
(
6
)
\begin{cases} \dot{x}_c = -v_x - \omega_yz_c + \omega_zy_c \\ \dot{y}_c = -v_y - \omega_zx_c + \omega_xz_c \\ \dot{z}_c = -v_z - \omega_xy_c + \omega_yx_c \end{cases} \quad (6)
⎩
⎨
⎧x˙c=−vx−ωyzc+ωzycy˙c=−vy−ωzxc+ωxzcz˙c=−vz−ωxyc+ωyxc(6)
将(1)(6)式代入(3)式，可得：
{
x
˙
=
−
f
z
c
v
x
+
x
z
c
v
z
+
x
y
f
ω
x
−
f
2
+
x
2
f
ω
y
+
y
ω
z
y
˙
=
−
f
z
c
v
y
+
y
z
c
v
z
+
f
2
+
y
2
f
ω
x
−
x
y
f
ω
y
−
x
ω
z
(
7
)
\begin{cases} \dot{x} = -\frac{f}{z_c}v_x + \frac{x}{z_c}v_z + \frac{xy}{f}\omega_x - \frac{f^2 + x^2}{f}\omega_y + y\omega_z \\ \dot{y} = -\frac{f}{z_c}v_y + \frac{y}{z_c}v_z + \frac{f^2 + y^2}{f}\omega_x - \frac{xy}{f}\omega_y - x\omega_z \\ \end{cases} \quad (7)
{x˙=−zcfvx+zcxvz+fxyωx−ff2+x2ωy+yωzy˙=−zcfvy+zcyvz+ff2+y2ωx−fxyωy−xωz(7)
再将(2)(7)式代入(4)式，可得：
[
u
˙
v
˙
]
=
[
−
f
d
x
z
c
0
u
−
u
0
z
c
d
y
(
u
−
u
0
)
(
v
−
v
0
)
f
−
f
2
+
d
x
2
(
u
−
u
0
)
2
d
x
f
d
y
(
v
−
v
0
)
d
x
0
−
f
d
y
z
c
v
−
v
0
z
c
f
2
+
d
y
2
(
v
−
v
0
)
2
d
y
f
−
d
x
(
u
−
u
0
)
(
v
−
v
0
)
f
−
d
x
(
u
−
u
0
)
d
y
]
[
v
x
v
y
v
z
ω
x
ω
y
ω
z
]
=
J
i
m
a
g
e
⋅
r
˙
\begin{aligned} \begin{bmatrix} \dot{u} \\ \dot{v} \end{bmatrix} &= \begin{bmatrix} -\frac{f}{d_xz_c} & 0 & \frac{u - u_0}{z_c} & \frac{d_y(u - u_0)(v - v_0)}{f} & -\frac{f^2 + d_x^2(u - u_0)^2}{d_xf} & \frac{d_y(v - v_0)}{d_x} \\ 0 & -\frac{f}{d_yz_c} & \frac{v - v_0}{z_c} & \frac{f^2 + d_y^2(v - v_0)^2}{d_yf} & -\frac{d_x(u - u_0)(v - v_0)}{f} & -\frac{d_x(u - u_0)}{d_y} \\ \end{bmatrix} \begin{bmatrix} v_x \\ v_y \\ v_z \\ \omega_x \\ \omega_y \\ \omega_z \end{bmatrix} \\ &= J_{image} \cdot \dot{r} \end{aligned}
[u˙v˙]=
−dxzcf00−dyzcfzcu−u0zcv−v0fdy(u−u0)(v−v0)dyff2+dy2(v−v0)2−dxff2+dx2(u−u0)2−fdx(u−u0)(v−v0)dxdy(v−v0)−dydx(u−u0)
vxvyvzωxωyωz
=Jimage⋅r˙
从而得到图像雅克比矩阵：
J
i
m
a
g
e
=
[
−
f
d
x
z
c
0
u
−
u
0
z
c
d
y
(
u
−
u
0
)
(
v
−
v
0
)
f
−
f
2
+
d
x
2
(
u
−
u
0
)
2
d
x
f
d
y
(
v
−
v
0
)
d
x
0
−
f
d
y
z
c
v
−
v
0
z
c
f
2
+
d
y
2
(
v
−
v
0
)
2
d
y
f
−
d
x
(
u
−
u
0
)
(
v
−
v
0
)
f
−
d
x
(
u
−
u
0
)
d
y
]
J_{image} = \begin{bmatrix} -\frac{f}{d_xz_c} & 0 & \frac{u - u_0}{z_c} & \frac{d_y(u - u_0)(v - v_0)}{f} & -\frac{f^2 + d_x^2(u - u_0)^2}{d_xf} & \frac{d_y(v - v_0)}{d_x} \\ 0 & -\frac{f}{d_yz_c} & \frac{v - v_0}{z_c} & \frac{f^2 + d_y^2(v - v_0)^2}{d_yf} & -\frac{d_x(u - u_0)(v - v_0)}{f} & -\frac{d_x(u - u_0)}{d_y} \\ \end{bmatrix}
Jimage=
−dxzcf00−dyzcfzcu−u0zcv−v0fdy(u−u0)(v−v0)dyff2+dy2(v−v0)2−dxff2+dx2(u−u0)2−fdx(u−u0)(v−v0)dxdy(v−v0)−dydx(u−u0)
扫一扫
5412
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
