Apriltag中计算的Homography
首先,在进行apriltag码检测时,如果检测到会一并计算出图像上apriltag码四个角点对应的homography矩阵,这个homography将这些点映射到到标准的(-1,1),(1,1),(1,-1),(-1,-1)顶点。在上面的示例一中,由homography和apriltag角点为:
H = [ 3.3831e-01 7.066e-01 -1.8602e+00
-5.1398e-01 1.6081e-01 -1.8558e+00
5.1039e-04 -7.7972e-05 -8.6540e-03]
%% 角点的齐次坐标
p1= [319.6915 165.3677 1.00]'
p2= [276.2611 313.7463 1.00]'
p3= [99.1906 268.6764 1.00]'
p4= [161.4450 127.7792 1.00]'
我们可以验证:
inv(H)*p1 = [ 110.05 110.05 -110.05]' = [-1 -1 1]
inv(H)*P2 = [-123.98 123.98 -123.98]' = [ 1 -1 1]
inv(H)*p3 = [-121.63 -121.63 -121.63]' = [ 1 1 1]
inv(H)*p4 = [ 108.20 -108.20 -108.20]' = [-1 1 1]
这里inv(H)是将相机图像上apriltag码角点映射到(-1,1),(1,1),(1,-1),(-1,-1)的homography。
Apriltag中的相机外参估计方法
通过给定相机的内参K,就可以利用homography对相机相对于apriltag码的方位进行估计。下面通过分析Apriltag的源码,阐述一下利用homography估计相机方位的方法。Apriltag中使用的方法属于技巧性的,
假设相机的内参矩阵为:
K
=
[
f
x
0
c
x
0
f
y
c
y
0
0
1
]
\mathbf{K}=\left[\begin{array}{ccc} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{array}\right]
K=⎣⎡fx000fy0cxcy1⎦⎤
那么相机的投影矩阵就为
P
=
K
[
R
∣
t
]
\mathbf{P=K[R|t]}
P=K[R∣t],空间上的点
X
\mathbf{X}
X通过该矩阵变为图像上的像素点
x
=
P
X
\mathbf{x=PX}
x=PX。
同时,我们设定Apriltag码所在的平面是在X-Y平面上(
Z
=
0
Z=0
Z=0),其中心为坐标原点。那么有:
x
=
K
[
R
∣
t
]
[
X
Y
0
1
]
\mathbf{x}=\mathbf{K[R|t]}\left[\begin{array}{ccc} X \\ Y \\ 0 \\ 1 \end{array}\right]
x=K[R∣t]⎣⎢⎢⎡XY01⎦⎥⎥⎤
因此我们可以将其中
R
\mathbf{R}
R的第三列去掉,得到
x
=
K
[
r
0
r
1
t
]
[
X
Y
1
]
\mathbf{x}=\mathbf{K[r_{0}\ r_{1}\ t]}\left[\begin{array}{ccc} X \\ Y \\ 1 \end{array}\right]
x=K[r0 r1 t]⎣⎡XY1⎦⎤
其中
r
0
,
r
1
\mathbf{r_0,r_1}
r0,r1是
R
\mathbf{R}
R的第一二列。
实际上
K
[
r
0
r
1
t
]
\mathbf{K[r_{0}\ r_{1}\ t]}
K[r0 r1 t]就构成了空间平面上点到图像上点的homography。那么就有一个疑问,apriltag中计算的homography不是将apriltag码的角点映射到单位方形的吗? 是的,我们可以假想,将空间平面上的ariltag码缩小成单位方形,其实对相机的方向并没有影响,只对位置有影响。令
[
X
Y
1
]
=
[
λ
0
0
0
λ
0
0
0
1
]
[
X
′
Y
′
1
]
\left[\begin{array}{c} X \\ Y \\ 1 \end{array}\right]= \left[\begin{array}{ccc} \lambda & 0 & 0 \\ 0 & \lambda & 0 \\ 0 & 0 & 1 \end{array}\right] \left[\begin{array}{c} X' \\ Y' \\ 1 \end{array}\right]
⎣⎡XY1⎦⎤=⎣⎡λ000λ0001⎦⎤⎣⎡X′Y′1⎦⎤
其中
[
X
,
Y
,
1
]
′
[X,Y,1]'
[X,Y,1]′为缩放后的单位方形的角点。因此有:
x
=
K
[
r
0
r
1
t
]
[
λ
0
0
0
λ
0
0
0
1
]
[
X
′
Y
′
1
]
\mathbf{x}=\mathbf{K[r_{0}\ r_{1}\ t]} \left[\begin{array}{ccc} \lambda & 0 & 0 \\ 0 & \lambda & 0 \\ 0 & 0 & 1 \end{array}\right] \left[\begin{array}{ccc} X' \\ Y' \\ 1 \end{array}\right]
x=K[r0 r1 t]⎣⎡λ000λ0001⎦⎤⎣⎡X′Y′1⎦⎤
那么我们可以令
K
[
λ
r
0
λ
r
1
t
]
=
K
[
r
0
′
r
1
′
t
]
\mathbf{K}[\lambda \mathbf{r}_0\ \lambda \mathbf{r}_1\ \mathbf{t}]=\mathbf{K[r'_{0}\ r'_{1}\ t]}
K[λr0 λr1 t]=K[r0′ r1′ t]为apriltag计算出的
H
′
\mathbf{H}'
H′。
就有如下分解等式:
f
x
r
00
′
+
c
x
r
20
′
=
h
00
f
x
r
01
′
+
c
x
r
21
′
=
h
01
f
x
t
x
+
c
x
t
z
=
h
02
f
y
r
10
′
+
c
y
r
20
′
=
h
10
f
y
r
11
′
+
c
y
r
21
′
=
h
11
f
y
t
y
+
c
y
t
z
=
h
12
r
20
′
=
h
20
r
21
′
=
h
21
t
z
=
h
22
\begin{aligned} f_xr'_{00}+c_xr'_{20}=h_{00} \\ f_xr'_{01}+c_xr'_{21}=h_{01} \\ f_xt_x + c_xt_z = h_{02} \\ f_yr'_{10}+c_yr'_{20}=h_{10} \\ f_yr'_{11}+c_yr'_{21}=h_{11} \\ f_yt_y + c_yt_z = h_{12} \\ r'_{20} = h_{20} \\ r'_{21} = h_{21} \\ t_z = h_{22} \end{aligned}
fxr00′+cxr20′=h00fxr01′+cxr21′=h01fxtx+cxtz=h02fyr10′+cyr20′=h10fyr11′+cyr21′=h11fyty+cytz=h12r20′=h20r21′=h21tz=h22
通过上式便可以解出
r
0
′
\mathbf{r}'_0
r0′、
r
1
′
\mathbf{r}'_1
r1′和
t
\mathbf{t}
t。由于
H
′
\mathbf{H}'
H′的各列本身都是非单位化的,因此计算出的
r
0
′
\mathbf{r}'_0
r0′和
r
1
′
\mathbf{r}'_1
r1′就需要进行单位化处理。apriltag源码中是这样做的:
r
0
′
′
=
r
0
′
∣
∣
r
0
′
∣
∣
∣
∣
r
1
′
∣
∣
,
r
1
′
′
=
r
1
′
∣
∣
r
0
′
∣
∣
∣
∣
r
1
′
∣
∣
,
t
′
=
t
∣
∣
r
0
′
∣
∣
∣
∣
r
1
′
∣
∣
\mathbf{r''_0}=\frac{\mathbf{r'_0}}{\sqrt{||\mathbf{r'_0}||||\mathbf{r'_1}||} }, \qquad \mathbf{r''_1}=\frac{\mathbf{r'_1}}{\sqrt{||\mathbf{r'_0}||||\mathbf{r'_1}||} }, \qquad t'=\frac{\mathbf{t}}{\sqrt{||\mathbf{r'_0}||||\mathbf{r'_1}||} }
r0′′=∣∣r0′∣∣∣∣r1′∣∣r0′,r1′′=∣∣r0′∣∣∣∣r1′∣∣r1′,t′=∣∣r0′∣∣∣∣r1′∣∣t
至于为什么
t
\mathbf{t}
t也要做除法,其实这是符合实际的。从另一个角度看,因为有
[
r
0
′
r
1
′
t
]
=
K
−
1
H
′
[\mathbf{r'_0\ r'_1\ t}]=\mathbf{K^{-1}H'}
[r0′ r1′ t]=K−1H′,当我们知道
K
−
1
H
′
\mathbf{K^{-1}H'}
K−1H′,就可以使得
K
−
1
H
′
\mathbf{K^{-1}H'}
K−1H′的前两列单位化来得到
r
0
′
\mathbf{r}'_0
r0′和
r
1
′
\mathbf{r}'_1
r1′。假如要除以某个数来实现单位化,那么
K
−
1
H
′
\mathbf{K^{-1}H'}
K−1H′的第三列显示也要同时除以该数才能保持正确性。
到此我们应该清楚,单位化后
r
0
,
r
1
\mathbf{r_0,r_1}
r0,r1和
r
0
′
,
r
1
′
\mathbf{r'_0,r'_1}
r0′,r1′是一样的,只有
t
\mathbf{t}
t和
t
′
\mathbf{t'}
t′的不同。对于在相机图像上同一个apriltag码,
t
\mathbf{t}
t表示相机到
R
\mathbf{R}
R表示方向上实际大小aprilta码的距离,
t
′
\mathbf{t'}
t′则表示相机到同一方向上实际大小为单位方形的apriltag码的距离。因为是对同一个Apriltag方形在
t
\mathbf{t}
t方向上的比例缩放,所以如果知道实际apriltag码的尺寸就可以通过比例计算出相机到实际apriltag码的距离。若apriltag码的宽度为
w
w
w,那么相机到实际apriltag码的距离就为
t
=
w
t
′
\mathbf{t}=w\mathbf{t'}
t=wt′。
apriltag中还将 R \mathbf{R} R矩阵进行SVD分解来进一步提高R的准确性。因为除以 ∣ ∣ r 0 ′ ∣ ∣ ∣ ∣ r 1 ′ ∣ ∣ \sqrt{||\mathbf{r'_0}||||\mathbf{r'_1}||} ∣∣r0′∣∣∣∣r1′∣∣并不一定会使得 r 0 , r 1 \mathbf{r_0,r_1} r0,r1是精确地单位化的,只是使得它们非常接近单位化。我们令 R = U Σ V T \mathbf{R=U\Sigma V^T} R=UΣVT,然后令 R ′ = U V T \mathbf{R'=UV^T} R′=UVT即可。这是因为精确的 R \mathbf{R} R是单位正交矩阵,可以证明(利用 R R T = I \mathbf{RR^T=I} RRT=I)分解中 Σ = I \mathbf{\Sigma=I} Σ=I
到此,apriltag计算出旋转矩阵
R
\mathbf{R}
R和位置
t
′
\mathbf{t'}
t′。然后返回
4
×
4
4\times 4
4×4矩阵:
M
=
[
R
t
′
0
1
]
\mathbf{M}=\left[\begin{array}{cc} \mathbf{R} & \mathbf{t'} \\ 0 & 1 \end{array} \right]
M=[R0t′1]
注意Apriltag功能包输出的是
t
′
\mathbf{t'}
t′,要获得实际apriltag码的位置
t
\mathbf{t}
t还需要自行进行上述的比例缩放。
在图像上标记Apriltag码的方向
得到了R和t后,相机的投影矩阵为 P = K [ R T ∣ − t ] \mathbf{P=K[R^T|-t]} P=K[RT∣−t]。
现在仍以apriltag中心为参考坐标系,取apriltag的法向量为 n = [ 0 , 0 , − 1 ] \mathbf{n}=[0,0,-1] n=[0,0,−1],这也表示了向量的顶端的位置点,将该位置点投影到图像上,得到的点与apriltag的图像中心点的连线即为法线在图像上的投影,用来表示apriltag的方向。
下面是在图像上显示apriltag方向的代码以及测试的结果
#计算并显示apriltag码的方向
M,e1,e2=at_detector.detection_pose(tag, cam_params)
P=M[:3,:4]
P=np.matmul(K,P)
x=np.matmul(P,np.array([[0],[0],[-1],[1]]))
x=x/x[2]
cv2.line(frame, tuple(tag.center.astype(int)), tuple(x[:2].astype(int)), (0,0,255),2)
下面的图中,将摄像头安装在机器人上,使用的Apriltag码的宽度为0.08m,估计出的距离为0.48 m,和实际量出来的距离基本是一致的。