YOLO 检测目标坐标估算¶
书接上回：在 Research - Camera Calibration 中，我们探讨了一个实际的工程问题：一个装有单目相机的 SLAM 小车系统，在已知车体相对于 map 系下的位姿、相机相对于车体的外参、相机图像经过 YOLO 检测目标之后，推算出检测到目标的实际坐标（map 系下坐标，或车体坐标）？在之前的解答中，我们使用了非常直接的求解方法，但是在实际测试中发现，效果不是很理想。另外，从理论上来说，单次求解使用的帧数越多，结果越稳定，但是由于多个帧的方程是叠加起来的，帧数越多维数越大，求解速度会变慢。因此，本博客尝试使用另外一种方法：射线法，试图得到更加稳定的解。
射线法求解¶
假设：
- 图像通过 YOLO 模型，得到检测框，以检测框中心像素坐标 \((u,v)\) 作为检测目标物位置。
- 去畸变后，相机内参矩阵为 \(\mathbf{K}\in\mathbb{R}^{3\times 3}\) ，车体到相机的静态外参为 \({}_L^C\mathbf{T}\in\mathbb{R}^{4\times 4}\) .
- 从里程计消息中，提取出车体到世界系的位姿变换 \({}_L^W\mathbf{T}\in\mathbb{R}^{4\times 4}\) .
- 检测目标物在世界系中静止不动，在世界系、车体系、相机系中的坐标分别为 \(\mathbf{X}_W,\mathbf{X}_L,\mathbf{X}_C\) . 我们需要先求解 \(\mathbf{X}_W\) ，因为它是常数，然后可以变换到车体系中用于可视化。
首先，目标物从像素系到相机系的变换为：
忽略系数 \(Z_C\) ，得到 \(\mathbf{X}_C=\mathbf{K}^{-1}\cdot[u,v,1]^T\) ，因为我们不关心它的模长，只关心它所在的射线起点为相机系原点（光心），经过目标物。对其归一化得到该射线方向的单位向量：
然后计算相机系到世界系的位姿变换：
为避免矩阵求逆，我们直接写出 \({}_C^W\mathbf{T}\) 的表达式：
然后把单位方向向量变换到世界系： \(\mathbf{d}_W=\mathbf{R}_{CW}\mathbf{d}_C\) ，同时，相机的光心坐标即为平移向量 \(\mathbf{C}_W=\mathbf{t}_{CW}\) .
因此，在世界系下，一帧图像+位姿，对应一条“以相机光心为起点，经过目标物的射线”，即 \(\mathbf{C}_W+\lambda\mathbf{d}_W,\;\lambda>0\) .
在相机随车体运动过程中，每一帧都对应一条这样的射线，理论上这些射线相交于同一点，也就是目标点所在位置 \(\mathbf{X}_W\) . 实际中由于噪声和误差的存在，这些若干条射线可能有多个交点，因此使用最小二乘法求最优解。
对于世界系下的任意一点 \(\mathbf{X}\) ，若以射线上的某一点为起点，以该点 \(\mathbf{X}\) 为终点组成的向量，可以表示为 \(\mathbf{X}-\mathbf{C}\) ，该向量在射线上投影对应的向量为 \(\mathbf{d}_W\mathbf{d}_W^T(\mathbf{X}-\mathbf{C})\) （因为 \(\mathbf{d}_W\) 也是单位向量），因此点 \(\mathbf{X}\) 到射线的最短距离向量，即为残差 \(\mathbf{r}\) ：
因此，点 \(\mathbf{X}\) 到射线上的最短距离即为残差的模长 \(\|\mathbf{r}\|\) . 令 \(\mathbf{P}=\mathbf{I}-\mathbf{d}_W\mathbf{d}_W^T\) ，有 \(\mathbf{P}^T\mathbf{P}=\mathbf{P}\) .
我们希望求解出来的坐标 \(\mathbf{X}_W\) 到各条直线的距离之和最小，写为加权最小二乘形式：
转换为求解方程：
初始时刻设置 \(\mathbf{A}=\mathbf{0},\mathbf{b}=\mathbf{0}\) . 在求解过程中，我们需要逐个构造 \(\mathbf{P}_i,\mathbf{C}_i\) 并加入已有的 \(\mathbf{A},\mathbf{b}\) ，最终求解 \(\mathbf{A}\mathbf{X}=\mathbf{b}\) 可得到目标物的世界系坐标 \(\mathbf{X}_W\) ，可以再变换到车体系 \(\mathbf{X}_L={}_W^L\mathbf{T}\cdot\mathbf{X}_W=(\mathbf{R}_{LW})^T(\mathbf{X}_W-\mathbf{t}_{LW})\) ，方便进行可视化。
存在的问题：
- 当车体自身纯旋转时，也就是相机光心不动（在相机和车体位于同一竖直方向时），此时即使是多帧图像和位姿，它们对应的也是同一个射线。
- 当车体只前进，尤其是当目标物接近图像中心时，也就是相机光心几乎向着目标物前进，此时多帧图像和位姿对应的也是同一条射线。
地面假设¶
之前的推导中我们知道，在只有一帧图像+位姿的情况下，是无法确定人员在地图中的三维坐标的。但是如果我们做出如下假设：场景中人员始终静止站在地面上。也就是人员脚部的地图坐标的高度 \(Z_W=0\) .
经过 YOLO 推理后的检测框，将其底边中心的坐标作为人员脚部对应的像素坐标 \((u,v)\) . 也就是说，我们将求解的坐标限制在一个平面 \(Z_W=0\) 上，既能减小自由度，又能尽量减小前述方法中射线交点对噪声的敏感度（因为两个夹角很小的射线可能收到噪声影响，交点变得很远，但是如果先求出两条射线在平面 \(Z_W=0\) 的交点再求中心点，会更加准确）。
首先，我们还是根据已知的 LiDAR-相机外参，即 LiDAR->Camera 变换 \(\mathbf{T}_{LC}\) 和由里程计得到的 LiDAR->World 变换 \(\mathbf{T}_{LW}\) ，计算出世界坐标系到相机坐标系的 World->Camera 变换：
令 \(\mathbf{T}_{WC}=[\mathbf{R}\mid \mathbf{t}]\) ，可以直接写出其表达式以避免对矩阵 \(\mathbf{T}_{LW}\) 求逆：
然后得到世界坐标系（地图系）到像素坐标系的变换关系：
其中 \(\mathbf{K}\) 为相机内参矩阵，通过标定得到，是已知量。我们令 \(Z_W=0\) ，消去尺度因子 \(Z_C\) ，得到：
写为矩阵形式：
由于我们限制人员坐标 \(Z_W=0\) ，因此一帧图像+位姿即可确定该人员的地图坐标 \((X_W,Y_W,0)\) .
为了求解的稳定性和减小噪声的影响，我们还是希望根据多帧坐标进行优化求解，动态寻找人员最可能的坐标。也就是我们存储多帧图像+位姿计算得到的二维平面坐标 \(\mathbf{p}_i=(x_i,y_i)\) ，求解最可能的坐标 \(\mathbf{p}\) 作为最终解。这实际上是一个估计问题。
最小化距离的平方和¶
最常用的为最小二乘准则，也就是寻找一个点，使得该点到所有观测点的欧氏距离平方和最小。在统计学上，如果假设噪声服从均值为零的高斯分布（正态分布），那么最小二乘准则得到的结果与最大似然估计的结果是完全一致的。目标函数为：
目标函数的最小值有闭式解：
即为所有点的重心。
最小化距离之和¶
在最小二乘法中，离群点对目标函数的残差惩罚是平方律的，一个极端离群点可能会显著将均值“拉向”自己。我们希望减小极端离群点对均值的影响，因此使用 L1 的目标函数，在存在误识别/跳变/遮挡等偶发大误差时更稳健和鲁棒：
与最小二乘法不同，该目标函数的最小值没有一个简单的封闭解，因为其导数包含平方根，代数形式复杂。但是，这个函数是一个凸函数，这意味着它只有一个全局最小值，没有局部最小值。因此，我们可以通过迭代算法来稳定地找到这个解。
最常用的算法是 Weiszfeld 算法，它本质上是一种迭代重加权最小二乘法（Iteratively Reweighted Least Squares, IRLS）。
目标函数的梯度为：
该式天然构成了一个迭代式。因此我们设计迭代步骤为：
（1）初始化。
选择一个初始点 \(\mathbf{p}^{(0)}\) ，可以选择第一个观测点，或者前两三个观测点的重心。
（2）迭代更新。当前第 \(k\) 步的时候，用 \(\mathbf{p}^{(k)}\) 估计 \(\mathbf{p}^{(k+1)}\) ：
（3）收敛条件。相邻两次迭代的结果差异较小 \(\|\mathbf{p}^{(k+1)}-\mathbf{p}^{(k)}\|<\varepsilon\) .