Apriltag使用之二：方位估计(定位)

最新推荐文章于 2023-12-06 18:23:13 发布

arczee

最新推荐文章于 2023-12-06 18:23:13 发布

阅读量1.4w

点赞数 14

分类专栏： Python 视觉机器人文章标签： opencv apriltag python

本文链接：https://blog.csdn.net/shenyan0712/article/details/102742918

版权

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中使用的方法属于技巧性的，

假设相机的内参矩阵为：
$\mathbf{K}=\left[\begin{array}{ccc} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{array}\right]$
那么相机的投影矩阵就为 $\mathbf{P=K[R|t]}$ ，空间上的点 $\mathbf{X}$ 通过该矩阵变为图像上的像素点 $\mathbf{x=PX}$ 。
同时，我们设定Apriltag码所在的平面是在X-Y平面上( $Z = 0$ )，其中心为坐标原点。那么有：
$\mathbf{x}=\mathbf{K[R|t]}\left[\begin{array}{ccc} X \\ Y \\ 0 \\ 1 \end{array}\right]$
因此我们可以将其中 $\mathbf{R}$ 的第三列去掉，得到
$\mathbf{x}=\mathbf{K[r_{0}\ r_{1}\ t]}\left[\begin{array}{ccc} X \\ Y \\ 1 \end{array}\right]$
其中 $\mathbf{r_0,r_1}$ 是 $\mathbf{R}$ 的第一二列。

实际上 $\mathbf{K[r_{0}\ r_{1}\ t]}$ 就构成了空间平面上点到图像上点的homography。那么就有一个疑问，apriltag中计算的homography不是将apriltag码的角点映射到单位方形的吗？ 是的，我们可以假想，将空间平面上的ariltag码缩小成单位方形，其实对相机的方向并没有影响，只对位置有影响。令
$\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]$

最低0.47元/天解锁文章