Dive Deeper into Rectifying Homography for Stereo Camera Online Self-Calibration¹

论文作者¹ 在实践中发现使用 高精度在线无标记的双目外参标定系 中提出的算法时，观察到了平移向量估计的不稳定性。他们的方法令人担忧，因为它产生的系数矩阵在最佳点附近几乎是半正定的。这一特性有可能导致平移向量优化过程中的不稳定性，尤其是在检测到的关键点不够准确的情况下，导致迭代步长无法趋近于零。相比之下，该文的算法侧重于使用两个独立的旋转矩阵 $\mathbf R_l = [\mathbf r_{l, 1}, \mathbf r_{l, 2}, \mathbf r_{l, 3}]^T$ 和 $\mathbf R_r = [\mathbf r_{r, 1}, \mathbf r_{r, 2}, \mathbf r_{r, 3}]^T$ 分别旋转左右摄像机坐标系，而不是 $\bf R$ 和 $\bf t$ 的优化，并只在垂直方向上累积残差。这一策略可有效防止在平移矢量优化过程中出现正半有限系数矩阵，从而提高算法处理初始估计的能力，并且提高了整体稳定性，即使在关键点质量不尽如人意的情况下也是如此。

1. Dive deeper into rectifying homography - 深入研究校准单应法

首先向介绍立体校正的数学前言，通常在进行立体匹配之前要先进行立体校正。左、右立体图像中的一对对应点 $\mathbf p_l = [u_l, v_l]^T$ 和 $\mathbf p_r = [u_r, v_r]^T$ 可以通过其相机对应的已知的内参矩阵 $\mathbf K_l$ 和 $\mathbf K_r$ 转换到相机坐标 $\mathbf p_l^C$ 和 $\mathbf p_r^C$ 下:

$$ z_l \tilde{\mathbf p}_l = \mathbf K_l \mathbf p_l^C \ , \quad z_r \tilde{\mathbf p}_r = \mathbf K_r \mathbf p_r^C \tag{1} \label{1} $$

其中，$\tilde{\mathbf p} _{l,r}$ 是 $\mathbf p _{l,r}$ 的齐次坐标。将左右图像平面投影到与立体装备基线平行的共同平面上的立体矫正过程可表示如下：

$$ \begin{cases} z_l’ \tilde{\mathbf p}_l’ = \mathbf K \mathbf p_l^{C}{}’ = \mathbf K \mathbf R_l \mathbf p_l^C = z_l \mathbf K \mathbf R_l \mathbf K_l^{-1} \tilde{\mathbf p}_l \newline z_r’ \tilde{\mathbf p}_r’ = \mathbf K \mathbf p_r^{C}{}’ = \mathbf K \mathbf R_r \mathbf p_r^C = z_r \mathbf K \mathbf R_r \mathbf K_r^{-1} \tilde{\mathbf p}_r \end{cases} \tag{2} \label{2} $$

其中 $\mathbf p_l^C {}’ = [x_l’, y_l’, z_l’]^T$ 和 $\mathbf p_r^C {}’ = [x_r’, y_r’, z_r’]^T$ 分别是 $\mathbf p_l^C$ 和 $\mathbf p_r^C$ 在校正后新的左右相机坐标系中的坐标，$\tilde{\mathbf p}_l’ = [u_l’, v_l’]^T$ 和 $\tilde{\mathbf p}_r’ = [u_r’, v_r’]^T$ 分别是 $\mathbf p_l^C {}’$ 和 $\mathbf p_r^C {}’$ 在校正后的左右图像中的投影，$\bf K$ 是新定义的相机内参。由于未经校准的立体几何中固有尺度比例不可估性，因此将 $\bf t$ 视为单位向量，并得到以下表达式（单位矩阵 $\mathbf I = [\mathbf i_1, \mathbf i_2, \mathbf i_3]^T$），

$$ \mathbf p_l^C {}’ = \mathbf p_r^C {}’ + \mathbf i_1 \tag{3} \label{3} $$

将式 $\eqref{2},\eqref{3}$ 与 $\tilde{\mathbf p}_r^C = \mathbf P \tilde{\mathbf p}_l^C = \begin{bmatrix} \bf R & \bf t \newline \mathbf 0^T & 1 \end{bmatrix} \tilde{\mathbf p}_l^C$ 联合起来得：

$$ \begin{cases} \mathbf R = \mathbf R_r^{-1} \mathbf R_l \newline \mathbf t = -\mathbf r_{r,1} \end{cases} \tag{4} \label{4} $$

根据立体相机校正原理（² 中 7.3.7 小节），可以得出 $\mathbf R_r$ 的表达式如下：

$$ \mathbf R_r = [-\mathbf t \quad \mathbf i_3 \times \mathbf r_{r,1} \quad \mathbf r_{r,1} \times \mathbf r_{r, 2}]^T \tag{5} \label{5} $$

由于一对校正良好的立体图像符合 $v_l’ = v_r’$ 条件，根据³提出并解决了以下基于立体校准的 $\mathbf R_l$ 和 $\mathbf R_r$ 估计优化问题的约束条件：

$$ \cfrac{\mathbf r_{l, 2}^T \mathbf p_l^C}{\mathbf r_{l, 3}^T \mathbf p_l^C} = \cfrac{\mathbf r_{r, 2}^T \mathbf p_r^C}{\mathbf r_{r, 3}^T \mathbf p_r^C} \tag{6} \label{6} $$ 后续，将它用来定制能量函数。

2. Energy function and its solution for single-pair cases - 单对图像下的能量函数及其解法

根据 $\eqref{5}$ 和 $\eqref{9}$ 的引入立体校正约束，作者定义了一个 $(N+1)$ 的入口向量： $$ \mathbf e(\theta_l, \theta_r) = [e_0, e_1, \cdots, e_N]^T \tag{7} \label{7} $$ 其中， $$ \begin{cases} e_0(\theta_l, \theta_r) = \mathbf i_3^T \mathbf r_{r, 2} = \mathbf i_2^T \exp(\theta_l^\wedge) \mathbf i_3 \newline e_i(\theta_l, \theta_r) = \cfrac{\mathbf r_{l, 2}^T \mathbf p_l^C}{\mathbf r_{l, 3}^T \mathbf p_l^C} - \cfrac{\mathbf r_{r, 2}^T \mathbf p_r^C}{\mathbf r_{r, 3}^T \mathbf p_r^C} = \cfrac{\mathbf i_2^T \exp(\theta_l^\wedge) \mathbf p_l^C}{\mathbf i_3^T \exp(\theta_l^\wedge) \mathbf p_l^C} - \cfrac{\mathbf i_2^T \exp(\theta_r^\wedge) \mathbf p_r^C}{\mathbf i_3^T \exp(\theta_r^\wedge) \mathbf p_r^C} , \quad i \in [1, N] \cap \mathbb Z \end{cases} \tag{8} \label{8} $$ $\theta_l$ 和 $\theta_r$ 分别表示与 $\mathbf R_l$ 和 $\mathbf R_r$ 对应的旋转向量。$e_1, \cdots, e_N$ 表示 $v_l’$ 和 $v_r’$ 之间的残差。因此，可以将立体相机自标定的能量函数表述如下： $$ E = \lVert \mathbf e(\theta_l, \theta_r) \rVert_2^2 = \lVert \mathbf i_2^T \exp(\theta_l^\wedge) \mathbf i_3 \rVert_2^2 + \sum_{i = 1}^N \left\lVert \cfrac{\mathbf i_2^T \exp(\theta_l^\wedge) \mathbf p_l^C}{\mathbf i_3^T \exp(\theta_l^\wedge) \mathbf p_l^C} - \cfrac{\mathbf i_2^T \exp(\theta_r^\wedge) \mathbf p_r^C}{\mathbf i_3^T \exp(\theta_r^\wedge) \mathbf p_r^C} \right\rVert_2^2 \tag{9} \label{9} $$ 通过最小化 $\eqref{9}$ 可以得到最优的 $\mathbf R_l$ 和 $\mathbf R_r$ 。为此，将 $e_i(\theta_l, \theta_r)$ 的一阶泰勒展开公式化如下： $$ e_i(\hat\theta_l + \delta\theta_l, \hat\theta_r + \delta\theta_r) \approx e_i(\hat\theta_l , \hat\theta_r) + J_i(\hat\theta_l , \hat\theta_r) \delta\theta \tag{10} \label{10} $$ 其中 $\hat\theta = [\hat\theta_l^T, \hat\theta_r^T]^T$ 为当前估计值，$\delta\theta = [\delta\theta_l^T, \delta\theta_r^T]^T$ 为增量，有： $$ \begin{align*} J_0(\theta_l, \theta_r) &= [\mathbf 0^T, \cfrac{\partial\mathbf r_{r,23}}{\partial \theta_r}] = [0, 0, 0, -\mathbf i_2^T (\mathbf R_r \mathbf i_3)^\wedge] \newline J_i(\theta_l, \theta_r) &= [\cfrac{\partial e_i}{\partial \theta_l}, \cfrac{\partial e_i}{\partial \theta_r}], \quad i \in [1, N] \cap \mathbb Z \end{align*} \tag{11} \label{11} $$ 有， $$ \begin{align*} \cfrac{\partial e_i}{\partial \theta_l} &= \cfrac{-\mathbf r_{l,3}^T \mathbf p_{l,i}^C \mathbf i_2^T (\mathbf R_l \mathbf p_{l,i}^C)^\wedge + \mathbf r_{l,2}^T \mathbf p_{l,i}^C \mathbf i_3^T (\mathbf R_l \mathbf p_{l,i}^C)^\wedge}{(\mathbf r_{l, 3}^T \mathbf p_{l,i}^C)^2} \newline \cfrac{\partial e_i}{\partial \theta_r} &= \cfrac{\mathbf r_{r,3}^T \mathbf p_{r,i}^C \mathbf i_2^T (\mathbf R_r \mathbf p_{r,i}^C)^\wedge - \mathbf r_{r,2}^T \mathbf p_{r,i}^C \mathbf i_3^T (\mathbf R_r \mathbf p_{r,i}^C)^\wedge}{(\mathbf r_{r, 3}^T \mathbf p_{r,i}^C)^2} \end{align*} \tag{12} \label{12} $$ 由此，最小化 $E$ 可以转变为： $$ \arg \min_{\delta\theta} \sum_{i=0}^N \lVert e_i(\hat\theta_l , \hat\theta_r) + J_i(\hat\theta_l , \hat\theta_r) \delta\theta \rVert_2^2 \tag{13}\label{13} $$ $\delta\theta$ 可以用 Levenberg-Marquardt（LM）算法求解 ，具体如下： $$ (\sum_{i=0}^N w_i J_i^T J_i +\lambda I) \delta\theta = - \sum_{i=0}^N w_i J_i^T e_i \tag{14} \label{14} $$ 其中 $\lambda$ 是 LM 算法中使用的阻尼系数 $w_i = \begin{cases} 1, & \lvert e_i \rvert \leq c_t \newline c_t / \lvert e_i \rvert, & \lvert e_i \rvert > c_t \end{cases}$ 是分配给第 $i$ 特征点对的权重，而 $c_t$ 则代表预先定义的 Huber 准则阈值。然后，我们可以通过以下方法更新 $\hat\theta_l$ 和 $\hat\theta_r$ : $$ \begin{cases} \hat{\bf R}_l \gets \exp(\delta\theta_l^\wedge) \hat{\bf R}_l, \quad \hat\theta_l \gets \log(\hat{\bf R}_l^\vee) \newline \hat{\bf R}_r \gets \exp(\delta\theta_r^\wedge) \hat{\bf R}_r, \quad \hat\theta_r \gets \log(\hat{\bf R}_r^\vee) \end{cases}\tag{15} \label{15} $$ 当 $\lVert\delta⁢\theta\rVert_2$ 足够小时， 迭代更新过程就会终止。因此，立体摄像机的外参数 $\bf R$ 和 $\bf t$ 可以通过将 $\mathbf R_l$ 和 $\mathbf R_r$ 代入 $\eqref{4}$ 得到。

3. Global optimization for multi-pair cases - 多对情况下的全局优化

在获得每对立体图像的外参数 $\mathbf R^k$ 和 $\mathbf t^k$ （上标 k 表示第 k 对立体图像）后，就可以推导出立体摄像机的全局最佳外参数 $\mathbf R^* $ 和 $\mathbf t^*$ 。考虑到 $\mathbf t^k$ 已经归一化，它们可以被投射到一个限定区域内的球面上。旋转轴 $\mathbf v^k$ 也有类似的分布，可以使用罗德里格斯旋转公式计算如下：

$$ \cases{ s^k = \arccos( \cfrac{tr(\mathbf R^k) - 1}{2}) \newline \mathbf v^k{}^\wedge = \cfrac{\mathbf R^k - \mathbf R^k{}^T}{2\sin s^k} }\tag{16}\label{16} $$

其中 $s^k$ 是旋转角度。因此，最佳位移向量 $\mathbf t^*$ 和旋转轴 $\mathbf v^k$ 可以通过找到球面上投影分布最密集的位置来确定。这可以通过以下使用 M 组外在参数 $\mathbf R^k$ 和 $\mathbf t^k$ 的全局优化过程来实现：

$$ \begin{align*} \mathbf t^* &= \arg\max_{\mathbf t^* } \sum_{k=1}^M \mathbf t^k {}^T \mathbf t^* = \cfrac{\sum_{k=1}^M \mathbf t^k}{\lVert \sum_{k=1}^M \mathbf t^k \rVert_2} \newline \mathbf v^* &= \arg\max_{\mathbf v^* } \sum_{k=1}^M \mathbf v^k {}^T \mathbf v^* = \cfrac{\sum_{k=1}^M \mathbf v^k}{\lVert \sum_{k=1}^M \mathbf v^k \rVert_2} \end{align*} \tag{17} \label{17} $$

因此，最佳旋转矩阵 $\mathbf R^*$ 可按如下方式求得：

$$ \mathbf R^* = \exp(\theta^* {}^\wedge) = \exp((s^* \mathbf v^*)^\wedge) \tag{18}\label{18} $$

其中 $s^* $ 是通过对所有旋转角度的中心倾向测量 $s^k$ 确定的，而 $\theta^*$ 则代表全局最优旋转矢量。

4. 程序实现

论文作者给出了一个不完善且有点小 bug 的 python 版本的程序 TongjiZhaohb/StereoCalibrator ，个人实现的 C++ 版本 LSXiang/StereoCalibrator 。

Reference