深藍學院《從零開始手寫VIO》作業五

• 深藍學院《從零開始手寫VIO》作業六
• • 1. 證明題
  • 2. 代碼題

深藍學院《從零開始手寫VIO》作業六

1. 證明題

證明$Dy=0$的最優解$y$等于$D^{T}D$的最小奇異值對應的奇異值向量

矩陣$D^{T}D$的奇異值分解如下：
$${\mathbf{D}}^{?}\mathbf{D}=\sum _{i=1}^4{\sigma }_i^2{\mathbf{u}}_i{\mathbf{u}}_j^{?}$$
回顧我們的原始問題如下：
求解$D_{2n\times 4}{y}_{4\times 1}=0$，這是一個超定方程，本質上求解得到的是一個最小二乘解，使用如下表示：
$$\underset{y}{\min }||Dy|{|}^2=(Dy{)}^T(Dy)=y^T{D}^{T}Dy$$
其中$||y||=1$

$y$可以由$D^{T}D$的奇異值向量線性組合得到，也就是可以表示成如下形式
$$y=\sum _{i=1}^4{k}_i{u}_i=k_i{u}_i+v$$
其中$$v=\sum _{j=1,j=i}^4{k}_j{u}_j$$
$$k_i,k_j\in R$$
容易知道$u_i$和$v$正交。
將$y$代入$y^T{D}^{T}Dy$得到
$$\underset{y}{\min }||Dy|{|}^2=(k_i{u}_i+v{)}^T{D}^{T}D(k_i{u}_i+v)=k_i^2{u}_i^T{D}^{T}D{u}_i+v^T{D}^{T}Dv+k_i{u}_i^T{D}^{T}Dv+k_i{v}^T{D}^{T}D{u}_i$$

由于$u_i$和$v$正交，所以后兩項為0；并且$D{u}_i={\sigma }_i{u}_i$，帶入得到
$$\underset{y}{\min }||Dy|{|}^2=(k_i{u}_i+v{)}^T{D}^{T}D(k_i{u}_i+v)=k_i^2{u}_i^T{D}^{T}D{u}_i+v^T{D}^{T}Dv=k_i^2{\sigma }_i^2||u_i|{|}^2+v^T{D}^{T}Dv>=k_i^2{\sigma }_i^2||u_i|{|}^2$$
當且僅當$v=0$的時候等號成立。
如果想取得最小值，則${\sigma }_i={\sigma }_4$,也就是取得最小奇異值的時候，目標函數取得最小值，此時
$$y=k_4{u}_4+v=k_4{u}_4$$
由于$||y||=1$,所以$k_4=1$,所以
$$y=u_4$$
證明完畢。

2. 代碼題

代碼如下：

/// TODO::homework; 請完成三角化估計深度的代碼// 遍歷所有的觀測數據，并三角化Eigen::Vector3d P_est; // 結果保存到這個變量P_est.setZero();/* your code begin */Eigen::MatrixXd matD = ConstructMatrixD(camera_pose);Eigen::BDCSVD<Eigen::MatrixXd> bcdsvd(matD.transpose()*matD, Eigen::ComputeFullV | Eigen::ComputeFullU);auto singular_values = bcdsvd.singularValues();auto matU = bcdsvd.matrixU();auto matV = bcdsvd.matrixV();// result 1auto tmp_y = matU.rightCols(1);auto real_y = tmp_y/tmp_y(3);// result2double s = matD.maxCoeff();Eigen::Matrix4d scale_mat = Eigen::Matrix4d::Identity()/s;auto matD_new = matD*scale_mat;Eigen::BDCSVD<Eigen::MatrixXd> bcdsvd_new(matD_new.transpose()*matD_new, Eigen::ComputeFullV | Eigen::ComputeFullU);auto matU_new = bcdsvd_new.matrixU();auto tmp_y_new = scale_mat*matU_new.rightCols(1); //結果一樣auto real_y_new = tmp_y_new/tmp_y_new(3);/* your code end */

結果如下：

ground truth: -2.9477 -0.330799 8.43792 [normal solve] your result1: -2.9477 -0.330799 8.43792 1 [using Scale solve] your result2: -2.9477 -0.330799 8.43792 1

總結

以上是生活随笔為你收集整理的深蓝学院《从零开始手写VIO》作业六的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。