Vins 前端中高效的去畸变的方式解析
1.0 畸变是如何产生的
\(\quad\)我们先来想想3D点是如何投影到图像平面的：世界坐标点经过一个外参矩阵得到相机坐标系下的位置，由于我们经常用到的是归一化平面坐标，所以这里还要各坐标除以 \(Z\)，之后经过一个相机内参矩阵得到图像坐标系下坐标。
\(\quad\) 以下内容节选自14讲，我们来复习一下相机的成像：
\(\quad\)像素坐标系通常的定义方式是：原点 \(o^{\prime}\) 位于图像的左上角, \(u\) 轴向右与 \(x\) 轴平行, \(v\) 轴向下与 \(y\) 轴平行。像素坐标系与成像平面之间, 相差了一个缩放和一个原点的平移。 我们设像素坐标在 \(u\) 轴上缩放了 \(\alpha\) 倍, 在 \(v\) 上缩放了 \(\beta\) 倍。同时, 原点平移了 \(\left[c_{x}, c_{y}\right]^{T}\) 。 那么, \(P^{\prime}\) 的坐标与像素坐标 \([u, v]^{T}\) 的关系为:
\[\left\{\begin{array}{l} u=\alpha X^{\prime}+c_{x} \\ v=\beta Y^{\prime}+c_{y} \end{array}\right. \]代入式 (5.3) 并把 \(\alpha f\) 合并成 \(f_{x}\) , 把 \(\beta f\) 合并成 \(f_{y}\) , 得:
\[\left\{\begin{array}{l} u=f_{x} \frac{X}{Z}+c_{x} \\ v=f_{y} \frac{Y}{Z}+c_{y} \end{array} .\right. \]其中, \(f\) 的单位为米, \(\alpha, \beta\) 的单位为像素每米, 所以 \(f_{x}, f_{y}\) 的单位为像素。把该式写 成矩阵形式, 会更加简洁, 不过左侧需要用到齐次坐标:
\[\left(\begin{array}{l} u \\ v \\ 1 \end{array}\right)=\frac{1}{Z}\left(\begin{array}{ccc} f_{x} & 0 & c_{x} \\ 0 & f_{y} & c_{y} \\ 0 & 0 & 1 \end{array}\right)\left(\begin{array}{l} X \\ Y \\ Z \end{array}\right) \triangleq \frac{1}{Z} \boldsymbol{K} \boldsymbol{P} . \]我们按照传统的习惯, 把 \(Z\) 挪到左侧:
\[Z\left(\begin{array}{l} u \\ v \\ 1 \end{array}\right)=\left(\begin{array}{ccc} f_{x} & 0 & c_{x} \\ 0 & f_{y} & c_{y} \\ 0 & 0 & 1 \end{array}\right)\left(\begin{array}{c} X \\ Y \\ Z \end{array}\right) \triangleq \boldsymbol{K} \boldsymbol{P} . \]
\(\quad\)那么图像畸变发生在哪个环节呢？为了获得好的成像效果，我们在相机的前方加了透镜。透镜的加入对成像过程中光线的传播会产生新的影响，这个影响就是使图像发生了畸变。仔细想，不难想到由于透镜的影响，世界点投影到相机归一化平面上时已经发生了畸变。
\(\quad\)反过来想，我们的视觉SLAM系统首先得到的是特征点的像素坐标，我们知道内参之后，就可以反投影到相机归一化平面上，而我们检测到的特征点像素坐标就是畸变后的，所以这时候得到的归一化平面坐标也是畸变了的。我们就在此阶段去畸变，得到畸变前归一化坐标。
2.0 畸变
摘抄14讲的内容
\(\quad\)为了获得好的成像效果, 我们在相机的前方加了透镜。透镜的加入对成像过程中光线 的传播会产生新的影响: 一是透镜自身的形状对光线传播的影响, 二是在机械组装过程中, 透镜和成像平面不可能完全平行, 这也会使得光线穿过透镜投影到成像面时的位置发生变化。
\(\quad\)由透镜形状引起的畸变称之为径向畸变。在针孔模型中, 一条直线投影到像素平面上还是一条直线。可是, 在实际拍摄的照片中, 摄像机的透镜往往使得真实环境中的一条直线在图片中变成了曲线。越靠近图像的边缘, 这种现象越明显。由于实际加工制作的透 镜往往是中心对称的, 这使得不规则的畸变通常径向对称。它们主要分为两大类, 桶形畸变和枕形畸变, 如图所示。
\(\quad\)桶形畸变是由于图像放大率随着离光轴的距离增加而减小, 而枕形掎变却恰好相反。 在这两种畸变中, 穿过图像中心和光轴有交点的直线还能保持形状不变。
\(\quad\)除了透镜的形状会引入径向畸变外, 在相机的组装过程中由于不能使得透镜和成像面 严格平行也会引入切向畸变。如图所示。
\(\quad\)为更好地理解径向畸变和切向畸变, 我们用更严格的数学形式对两者进行描述。我们 知道平面上的任意一点 \(p\) 可以用笛卡尔坐标表示为 \([x, y]^{T}\) , 也可以把它写成极坐标的形式 \([r, \theta]^{T}\) , 其中 \(r\) 表示点 \(p\) 离坐标系原点的距离, \(\theta\) 表示和水平轴的夹角。径向畸变可看成坐标点沿着长度方向发生了变化 \(\delta r\) , 也就是其距离原点的长度发生了变化。切向畸变可以 看成坐标点沿着切线方向发生了变化, 也就是水平夹角发生了变化 \(\delta \theta\) 。
\[\begin{array}{l} x_{\text {corrected }}=x\left(1+k_{1} r^{2}+k_{2} r^{4}+k_{3} r^{6}\right) \\ y_{\text {corrected }}=y\left(1+k_{1} r^{2}+k_{2} r^{4}+k_{3} r^{6}\right) \end{array} . \]
\(\quad\)对于径向畸变, 无论是桶形掎变还是枕形畸变, 由于它们都是随着离中心的距离增加而增加。我们可以用一个多项式函数来描述畸变前后的坐标变化: 这类畸变可以用和距中心距离有关的二次及高次多项式函数进行纠正:\(\quad\)其中 \([x, y]^{T}\) 是末纠正的点的坐标, \(\left[x_{\text {corrected }}, y_{\text {corrected }}\right]^{T}\) 是纠正后的点的坐标, 注意它们 都是归一化平面上的点, 而不是像素平面上的点。在上式描述的纠正模型中, 对于畸变较小的图像中心区域, 畸变纠正主要是 \(k_{1}\) 起作用。而对于畸变较大的边缘区域主要是 \(k_{2}\) 起作用。普通摄像头用这两个系数就能很 好的纠正径向畸变。对畸变很大的摄像头, 比如鱼眼镜头, 可以加入 \(k_{3}\) 畸变项对畸变进行纠止。
\[\begin{array}{l} x_{\text {corrected }}=x+2 p_{1} x y+p_{2}\left(r^{2}+2 x^{2}\right) \\ y_{\text {corrected }}=y+p_{1}\left(r^{2}+2 y^{2}\right)+2 p_{2} x y \end{array} . \]
\(\quad\)另一方面, 对于切向畸变, 可以使用另外的两个参数 \(p_{1}, p_{2}\) 来进行纠正:因此, 联合两个式 子, 对于相机坐标系中的一点 \(P(X, Y, Z)\) , 我们能够 通过五个畸变系数找到这个点在像素平面上的正确位置:
\[\left\{\begin{array}{l} x_{\text {corrected }}=x\left(1+k_{1} r^{2}+k_{2} r^{4}+k_{3} r^{6}\right)+2 p_{1} x y+p_{2}\left(r^{2}+2 x^{2}\right) \\ y_{\text {corrected }}=y\left(1+k_{1} r^{2}+k_{2} r^{4}+k_{3} r^{6}\right)+p_{1}\left(r^{2}+2 y^{2}\right)+2 p_{2} x y \end{array} .\right. \]
- 将三维空间点投影到归一化图像平面。设它的归一化坐标为 \([x, y]^{T}\) 。
- 对归一化平面上的点进行径向畸变和切向畸变纠正。
\[\left\{\begin{array}{l} u=f_{x} x_{\text {corrected }}+c_{x} \\ v=f_{y} y_{\text {corrected }}+c_{y} \end{array}\right. \]
- 将纠正后的点通过内参数矩阵投影到像素平面, 得到该点在图像上的正确位置。
在上面的纠正畸变的过程中, 我们使用了五个畸变项。实际应用中, 可以灵活选择纠 正模型, 比如只选择 \(k_{1}, p_{1}, p_{2}\) 这三项等。
3.0 vins中去畸变的思想
\(\quad\)VINS中只考虑径向畸变，对于径向畸变，无论是桶形畸变还是枕形畸变，它们都是随着离中心的距离增加而增加。我们知道去畸变的公式如上面推导，可知如若已知畸变前的位置（也就是我们检测得到的特征点的位置，也就是上面公式中的\([x,y]\)），我们很容易得到畸变后的位置，而我们要做的是反推，由畸变后的位置推导畸变前的位置，这样一来，求解将高度非线性化。求解复杂，计算耗时。
\(\quad\)这里用的方法和OpenCV不同, 假设现在求 \(A_{d}\) 点的去畸变点 \(A\) 的坐标, 那么我们将 \(A_{d}\) 的坐标直接代入畸变模型中, 求得再次 畸变的点 \(A_{d 1}\) 坐标, 并求得点 \(A_{d 1}\) 和 \(A_{d}\) 之间的距离 \(d_{1}\) , 因为越靠近中心这个 \(d\) 越小, 此时肯定小于真实点 \(A\) 的坐标到 \(A_{d}\) 的 距离 \(d_{t}\) 。所以我们在朝真实畸变方向(再次畸变的反方向)上加上这个 \(d_{1}\) , 得到一个靠近真实值的方向, 在以这个点再进行一次 畸变求得距离 \(d_{2}\), \(d_{2}\) 肯定大于 \(d_{1}\) 小于 \(d_{t}\), \(d_{2}\) 在加到坐标 \(A_{d}\) 上得到更靠近的结果, 以此迭代, 如图所示, 迭代三次的示意 图; 代码中一共进行了8次迭代。由于VINS这种方法是正向计算的, 并没对高度非线性的掎变方程进行求解, 因此, 这种方法 比OpenCV中的方法更加快捷;
-
最核心的思想
我当前得到的带有畸变的坐标点，应该也是某个点的去除畸变后的坐标。
-
代码解析
//首先从函数参数来看，它的输入是一个二维的像素坐标，它的输出是一个三维的无畸变的归一化坐标 void PinholeCamera::liftProjective(const Eigen::Vector2d &p, Eigen::Vector3d &P) const { double mx_d, my_d, mx2_d, mxy_d, my2_d, mx_u, my_u; double rho2_d, rho4_d, radDist_d, Dx_d, Dy_d, inv_denom_d; // double lambda; // Lift points to normalised plane // 这里的mx_d和my_d就是最初的、有畸变的归一化坐标[x_d,y_d]^T mx_d = m_inv_K11 * p(0) + m_inv_K13; // x_d = (u - cx)/ fx my_d = m_inv_K22 * p(1) + m_inv_K23; // y_d = (v - cy)/ fy if (m_noDistortion) //如果没有畸变，则直接返回归一化坐标 { mx_u = mx_d; my_u = my_d; } else //如果有畸变，则计算无畸变的归一化坐标 { if (0) { //...... } else { // 递归失真模型 int n = 8; //迭代8次 Eigen::Vector2d d_u; // distortion()函数的作用是计算公式 \Delta x 和 \Delta x; d_u=[\Delta x,\Delta y] distortion(Eigen::Vector2d(mx_d, my_d), d_u); // 近似值 // 此处的mx_d和my_d相当A_d，它们在迭代过程保持不变 mx_u = mx_d - d_u(0); //x = x_d - \Delta x my_u = my_d - d_u(1); //y = y_d - \Delta y //此时的mx_u和my_u相当于被更新了一次的点A_1 for (int i = 1; i < n; ++i) { //上面已经迭代了1次，for循环中迭代7次 distortion(Eigen::Vector2d(mx_u, my_u), d_u); mx_u = mx_d - d_u(0); my_u = my_d - d_u(1); } } } // Obtain a projective ray P << mx_u, my_u, 1.0; }参考公式，只是将公式右边的系数为\(1\) 的 \(x\) 移动到公式的左边
\[\Delta x = x_{distored} - x \\ \Delta y = y_{distored}-y \]//计算 \Delta x 和 \Delta x void PinholeCamera::distortion(const Eigen::Vector2d &p_u, Eigen::Vector2d &d_u) const { // 畸变参数 double k1 = mParameters.k1(); double k2 = mParameters.k2(); double p1 = mParameters.p1(); double p2 = mParameters.p2(); double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u; mx2_u = p_u(0) * p_u(0); //x^2 my2_u = p_u(1) * p_u(1); //y^2 mxy_u = p_u(0) * p_u(1); //xy rho2_u = mx2_u + my2_u; //r^2=x^2+y^2 rad_dist_u = k1 * rho2_u + k2 * rho2_u * rho2_u; // k1 * r^2 + k2 * r^4 d_u << p_u(0) * rad_dist_u + 2.0 * p1 * mxy_u + p2 * (rho2_u + 2.0 * mx2_u),// \Delta x = x(k1 * r^2 + k2 * r^4)+2 * p1 * xy +p2(r^2+2x^2) p_u(1) * rad_dist_u + 2.0 * p2 * mxy_u + p1 * (rho2_u + 2.0 * my2_u); // \Delta y = y(k1 * r^2 + k2 * r^4)+2 * p2 * xy +p1(r^2+2y^2) }