VIO第6讲：投影模型与三角化

3 三角化

  在三角化之前，复习一遍视觉投影模型与坐标变换，这对于求雅可比和三角化都是非常重要的。

3.1 坐标变换与投影模型

① 世界系->相机系$X_w{Y}_w{Z}_w>>X_c{Y}_c{Z}_c$

  已知某点P在世界坐标系中的坐标$P_w=(X_w,Y_w,Z_w{)}^T$, 求其在相机坐标系中的坐标$P_c=(X_c,Y_c,Z_c{)}^T$.

第一种方式Pc = Rcw*Pw + tcw

$$\left[\begin{array}{c}Xc\\ Yc\\ Zc\end{array}\right]=R_{cw}\cdot \left[\begin{array}{c}Xw\\ Yw\\ Zw\end{array}\right]+t_{cw}$$

  合并为齐次矩阵（投影矩阵）表达如下
$$\left[\begin{array}{c}Xc\\ Yc\\ Zc\\ 1\end{array}\right]=\left[\begin{array}{cc}{R}_{cw}& t_{cw}\\ \to & \\ O& 1\end{array}\right]\cdot \left[\begin{array}{c}Xw\\ Yw\\ Zw\\ 1\end{array}\right]$$

第二种方式Pc = Rcw*(Pw - twc) = Rcw*Pw - Rcw*twc

实际中使用第一种方式中的投影矩阵（3*4），其中的tcw往往由第二种方式推出tcw = -Rcw*twc

相机系坐标除以深度Z后，就是归一化坐标

② 相机->图像$X_c{Y}_c{Z}_c>>xy$

  实际上图像系坐标与归一化坐标差一个焦距f的缩放！

  引入图像坐标系，即先求出相机坐标系下的点$P_c=(X_c,Y_c,Z_c{)}^T$在图像坐标系下的表达$p(x,y)$。利用相似三角形以及比例的原理，有以下推导及公式：
$$\begin{array}{rl}\frac{x}{f}& =\frac{X_c}{Z_c}\\ \frac{y}{f}& =\frac{Y_c}{Z_c}\end{array}$$
  所以，我们就能够推出上面结论：图像系坐标与归一化坐标差一个焦距f的缩放
$$x=\frac{f{X}_c}{Z_c}\text{and}y=\frac{f{Y}_c}{Z_c}$$
  转换为矩阵乘法，其中$(X_c,Y_c,Z_c,1{)}^T$是空间点P在相机坐标系的齐次坐标，$(x,y,1{)}^T$是图像点p在图像坐标系中的齐次坐标。

$$Z_c\cdot \left[\begin{array}{c}x\\ y\\ 1\end{array}\right]=\left[\begin{array}{cccc}f& 0& 0& 0\\ 0& f& 0& 0\\ 0& 0& 1& 0\end{array}\right]\cdot \left[\begin{array}{c}{X}_c\\ Y_c\\ Z_c\\ 1\end{array}\right]$$

③ 图像->像素$xy>>uv$

  设图像坐标系中心在像素坐标系的表达为$u_0,v_0$，图像上的投影点是$p(x,y)$

  $d_x$像素宽度，$d_y$像素高度分别表示每个像素在水平u和竖直v方向上的实际物理尺寸（单位mm）。

$$\begin{array}{rl}u& =\frac{x}{dx}+u_0\\ & \\ v& =\frac{y}{dy}+v_0\end{array}$$
  $\frac{x}{dx}$就表示在u方向占了多少像素，再加上中心坐标，就是实际的像素坐标。写成矩阵形式：

$$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}\frac{1}{dx}& o& u_0\\ 0& \frac{1}{dy}& v_0\\ 0& 0& 1\end{array}\right]\cdot \left[\begin{array}{c}x\\ y\\ 1\end{array}\right]$$

④ 世界->像素

  把上面几个变换结合起来
$$Z_c\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}\frac{1}{dx}& o& u_0\\ 0& \frac{1}{dy}& v_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}f& 0& 0& 0\\ 0& f& 0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{cc}R& t\\ \overrightarrow{O}& 1\end{array}\right]\left[\begin{array}{c}Xw\\ Yw\\ Zw\\ 1\end{array}\right]$$
  合并左边两个矩阵，即常见的内参矩阵$K_{3\ast 4}$，外参矩阵$T_{3\ast 4}$。其中，$f_x=\frac{f}{d_x},f_y=\frac{f}{d_y}$可以理解为焦距f分别在x,y轴方向等价的像素个数（每个像素的物理尺寸为dx×dy，单位为mm）
$$Z_c\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{cccc}{f}_x& 0& u_0& 0\\ 0& f_y& v_0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{cc}R& t\\ \overrightarrow{O}& 1\end{array}\right]\left[\begin{array}{c}Xw\\ Yw\\ Zw\\ 1\end{array}\right]$$

⑤ 问题

问题：

已知一款相机佳能80D及镜头参数如下：最高分辨率6000×4000，传感器尺寸22.3×14.9mm，焦距f=35mm，根据相机内参定义，估算相机的内参fx,fy,u0,v0

答案：

fx = f / dx = 35 / (22.3/6000) = 9417.040358744
fy = f / dy = 35 / (14.9/4000) = 9395.973154362
u0= 6000 / 2 = 3000
v0= 4000 / 2 = 2000

由于制造工艺等因素，相机像素坐标系中心未必与光轴经过的图像坐标系中心重合，即(u0,v0)并非是像素坐标系的中心，而是在中心附近某个位置，焦距及像素物理尺寸也非绝对精准，故而需要通过实际的内参标定过程，确定相机的内参矩阵。

参考链接

3.2 单目三角化

3.2.1 三角化推导

  某路标点L在若干个关键帧 k = 1,··· ,n 中看到，取齐次坐标$\mathbf{y}\in {\mathbb{R}}^4$，观测取归一化坐标$(x,y,1)$，这样可以忽略相机内参，只利用相机系和世界系坐标。
$$\lambda \left[\begin{array}{c}x\\ y\\ 1\end{array}\right]=\left[\begin{array}{cc}{R}_{cw}& t_{cw}\end{array}\right]\cdot \left[\begin{array}{c}Xw\\ Yw\\ Zw\\ 1\end{array}\right]$$
  利用投影矩阵P第三行就能代入矩阵上面两行，即一个特征点通过投影矩阵P可以提供两个约束方程！

  对于n个匹配特征点，$\mathbf{D}\in {\mathbb{R}}^{2n\times 4}$。
$$[\begin{array}{c}{x}_1{\mathbf{P}}_{1,3}^{\top }-{\mathbf{P}}_{1,1}^{\top }\\ y_1{\mathbf{P}}_{1,3}^{\top }-{\mathbf{P}}_{1,2}^{\top }\\ ⋮\\ x_n{\mathbf{P}}_{n,3}^{\top }-{\mathbf{P}}_{n,1}^{\top }\\ y_n{\mathbf{P}}_{n,3}^{\top }-{\mathbf{P}}_{n,2}^{\top }\end{array}]\left[\begin{array}{c}Xw\\ Yw\\ Zw\\ 1\end{array}\right]=0\to \mathbf{DL}=0$$

三角化数学推导具体推导参考链接，同ORB-SLAM。构建矩阵D的代码如下

int D_dim_row = 2 * (end_frame_id - start_frame_id);int D_dim_col = 4;Eigen::MatrixXd D = Eigen::MatrixXd::Zero(D_dim_row, D_dim_col);// 每一个特征点可以提供两行约束for(int i = start_frame_id; i < end_frame_id; i++){Eigen::MatrixXd P = Eigen::MatrixXd::Zero(3, 4);// 构建投影矩阵//  P.block<3,3>(0,0) = camera_pose[i].Rwc.transpose();P.block(0, 0, 3, 3) = camera_pose[i].Rwc.transpose();   // RcwP.block(0, 3, 3, 1) = -camera_pose[i].Rwc.transpose() * camera_pose[i].twc;     // tcw// 归一化坐标点(u,v,1)----当前帧-----在Eigen库中，可以使用( )操作符或者[ ]操作符---或uv.x() uv.ydouble x = camera_pose[i].uv[0];double y = camera_pose[i].uv[1];// 代入公式----每一个特征点可以提供两行约束D.block(2 * (i - start_frame_id), 0, 1 , 4) = x * P.row(2) - P.row(0);  D.block(2 * (i - start_frame_id) + 1, 0, 1 , 4) = y * P.row(2) - P.row(1);}

3.2.2 三角化求解

  在观测次于大于等于两次时，很可能 D 满秩，无零空间，这样就只有0解，但这是不对的。所以，寻求最小二乘解来替代：
$$\underset{\mathbf{y}}{\min }{\Vert \mathbf{DL}\Vert }_2^2,$$
  然后，利用SVD分解进行求解（关于SVD，可以参考博客，这里就是要知道SVD分解中的D和V都是可以由被分解矩阵与其转置的乘积的特征向量组成，所以下面最小奇异值对应最小特征向量！在第4部分给出代码测试，还有不要和下面的D混淆）
$${\mathbf{D}}^{\top }\mathbf{D}=\sum _{i=1}^4{\sigma }_i^2{\mathbf{u}}_i{\mathbf{u}}_j^{\top }$$
  最后，右奇异矩阵的最后一列就是最终的解。至于原有，曾经在ORB-SLAM2中推导过

还看到一个利用拉格朗日算子推理的

代码实现

    Eigen::JacobiSVD<Eigen::MatrixXd> svd(D.transpose()*D, Eigen::ComputeThinU | Eigen::ComputeThinV);// 调用svd对象的singularValues()成员函数，矩阵D的奇异值Eigen::Vector4d delta = svd.singularValues();// 取最小奇异值对应的右奇异向量----注意上面是对D^T*D进行了SVD分解，相当于对一个对称矩阵进行特征分解，两边UV都是一样的，所以这里取U也可以。但如果是对D进行SVD分解，只能是取V！Eigen::Vector4d u4 = svd.matrixU().col(3);

3.2.3 求解注意点

① 判断该解有效性的条件${\sigma }_4\ll {\sigma }_3$

    if(delta(3) * 1e-3 > delta(2)){   // 一般来讲奇异值都是从小到大，只要视差合理，这种极端情况不会出现std::cout << "The parallax is not enough" << std::endl;return -1;}

② ${\sigma }_4！=0$ 且深度>0

    if(u4(3) != 0 && u4(2) / u4(3) > 0){P_est = u4.head<3>() / u4(3);}

③ Rescale

  在某此场合（比如我们使用 UTM 坐标的平移），D 的数值大小差异明显，导致解在数值上不稳定。此时对 D 做一个缩放
$$\mathrm{Dy}=\underset{\tilde{\mathrm{D}}}{\underbrace{\mathrm{DS}}}\underset{\tilde{\mathrm{y}}}{\underbrace{{\mathrm{S}}^{-1}\mathrm{y}}}=0$$
  其中 S 为一个对角阵，取 D 最大元素之逆即可。

3.2.4 测量值噪声

  这里仿真的位姿以及地图点都是没有误差的，实际中路标点投影到相机帧中不可能是没有误差的，所以这里可以加一个测量噪声，来验证算法的有效性！

double x = Pc.x();
double y = Pc.y();
double z = Pc.z();double x_noise = x/z + noise(generator);
double y_noise = y/z + noise(generator);// camera_pose[i].uv = Eigen::Vector2d(x/z,y/z);
camera_pose[i].uv = Eigen::Vector2d(x_noise,y_noise);

​

3.2.5 特征点数量对结果的影响

  固定噪声方差参数，将观测图像帧扩成多帧（如3, 4 ,5，6，7，8等），观察最小奇异值和第二小奇异值之间的比例变化，并绘制比例值的变化曲线。

  这个就更简单了，把那个end_frame_id改了。

4 SVD分解

A = D*S*V^T，例子来源维基百科

验证了A^T*A的特征向量组成了A奇异值分解中的矩阵V

同理A*A^T的特征向量组成了A奇异值分解中的矩阵U

代码，其余理论可参考博客