从零手写VIO(六)

---

对从零手写VIO的第六次作业进行总结

---

三角化证明

三角化问题

证明y=u4为最优解

回顾我们的原始问题如下:
求解
$$D_{2n\times 4}{y}_{4\times 1}=0$$

对于齐次线性方程组$Dy=0$。当D行数大于列数时，就是一个超定方程,本质上求解得到的是一个最小二乘解
在||y||=1的约束下，其最小二乘解为矩阵$D^{T}D$最小特征值所对应的特征向量，下面给出证明
$$\underset{\mathbf{y}}{\min }\Vert Dy{\Vert }^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$$
其中
$$\begin{array}{c}v={\sum }_{j=1,j\ne i}^4{k}_j{u}_j\\ k_i,k_j\in R\end{array}$$

容易知道，$u_i$和$v$正交。
将$y$代入到$y^T{D}^{T}Dy$得到
$$\begin{gathered} \underset{\mathbf{y}}{\min }{\Vert D_{\mathbf{y}}\Vert }^2={\left({\mathbf{k}}_{\mathbf{i}}{u}_{\mathbf{i}}+\mathbf{v}\right)}^{\mathbf{T}}{\mathbf{D}}^{\mathbf{T}}\mathbf{D}\left({\mathbf{k}}_{\mathbf{i}}{u}_{\mathbf{i}}+v\right) \\ ={\mathbf{k}}_{\mathbf{i}}^2{u}_i^{\mathbf{T}}{\mathbf{D}}^{\mathbf{T}}{\mathbf{D}}_{{\mathbf{u}}_{\mathbf{i}}}+{\mathbf{v}}^{\mathbf{T}}{\mathbf{D}}^{\mathbf{T}}\mathbf{D}\mathbf{v}+{\mathbf{k}}_{\mathbf{i}}{u}_i^{\mathbf{T}}{\mathbf{D}}^{\mathbf{T}}\mathbf{D}\mathbf{v}+{\mathbf{k}}_{\mathbf{i}}{\mathbf{v}}^{\mathbf{T}}{\mathbf{D}}^{\mathbf{T}}\mathbf{D}{\mathbf{u}}_{\mathbf{i}} \end{gathered}$$
由于$u_i$和$v$正交，所以后两项为0 ; 并且 $D{u}_i={\sigma }_i{u}_i,$ 带入得到
$$\begin{gathered} \underset{x}{\min }\Vert Dy{\Vert }^2={\left(k_i{u}_i+v\right)}^T{D}^{T}D\left(k_i{u}_i+v\right)= \\ {k}_i^2{u}_i^T{D}^{T}D{u}_i+v^T{D}^{T}Dv=k_i^2{\sigma }_i^2{\Vert u_i\Vert }^2+v^T{D}^{T}Dv>=k_i^2{u}_i^T{D}^{T}D{u}_i+v^T{D}^{T}Dv=k_i^2{\sigma }_i^2{\Vert u_i\Vert }^2 \end{gathered}$$
当且仅当$v=0$的时候等号成立
如果想取得最小值，则${\sigma }_i=\sigma 4$，也就是取得最小奇异值的时候，目标函数取得最小值，此时
$$y=k_4{u}_4+v=k_4{u}_4$$
由于$||y||=1$，所以$k_4=1$，所以
$$y=u_4$$

三角化程序

思路

• 步骤
  step1：通过$P_{cw}$将世界坐标点转化到归一化相机坐标点,然后构造D矩阵
  step2：缩放D,scale取D绝对值元素最大的逆,然后$D^{T}D$
  step3：判断第四个奇异值是否远小于第三个(1e-2,1e-4) ,若是,则三角化有效,$y=u_4$,并且恢复scale,并且除以第四个元素(u4/u4[3]).head(3)得到非齐次坐标.

• 分析
  

代码

int main()
{

    int poseNums = 10;
    double radius = 8;
    double fx = 1.;
    double fy = 1.;
    std::vector<Pose> camera_pose;
    for(int n = 0; n < poseNums; ++n ) {
        double theta = n * 2 * M_PI / ( poseNums * 4); // 1/4 圆弧
        // 绕 z轴 旋转
        Eigen::Matrix3d R;
        R = Eigen::AngleAxisd(theta, Eigen::Vector3d::UnitZ());
        Eigen::Vector3d t = Eigen::Vector3d(radius * cos(theta) - radius, radius * sin(theta), 1 * sin(2 * theta));
        camera_pose.push_back(Pose(R,t));
    }

    // 随机数生成 1 个 三维特征点
    std::default_random_engine generator;
    std::uniform_real_distribution<double> xy_rand(-4, 4.0);
    std::uniform_real_distribution<double> z_rand(8., 10.);
    double tx = xy_rand(generator);
    double ty = xy_rand(generator);
    double tz = z_rand(generator);

    Eigen::Vector3d Pw(tx, ty, tz);
    // 这个特征从第三帧相机开始被观测，i=3
    int const start_frame_id = 3;
    int end_frame_id = poseNums;
    for (int i = start_frame_id; i < end_frame_id; ++i) {
        Eigen::Matrix3d Rcw = camera_pose[i].Rwc.transpose();
        Eigen::Vector3d Pc = Rcw * (Pw - camera_pose[i].twc);   // 将世界坐标下的点转换到相机坐标系下

        double x = Pc.x();
        double y = Pc.y();
        double z = Pc.z();

        camera_pose[i].uv = Eigen::Vector2d(x/z,y/z);
    }
    
    /// TODO::homework; 请完成三角化估计深度的代码
    // 遍历所有的观测数据，并三角化
    Eigen::Vector3d P_est;           // 结果保存到这个变量
    P_est.setZero();
    /* your code begin */
     //step1:constuct D Dy=0,其中y为世界系下的空间点
    Eigen::Matrix<double, 2*(10 - start_frame_id), 4> D;    // D的维度为 2n×4
    for (int i = start_frame_id; i < end_frame_id; ++i) {
        // 得到Tcw
        Eigen::Matrix<double,3,4> Tcw;
        Eigen::Matrix3d Rcw = camera_pose[i].Rwc.transpose();
        Eigen::Vector3d tcw = -Rcw*camera_pose[i].twc;//注意是-:Pc=Pc0-tcw
        Tcw<< Rcw, tcw;

        //rows = mat.rows(2);  //  获取第3行
        Eigen::Vector2d uv = camera_pose[i].uv ;

        D.block(2*(i - start_frame_id), 0, 1,4) = uv[0]*Tcw.row(2) - Tcw.row(0);
        D.block(2*(i - start_frame_id)+1, 0, 1,4) = uv[1]*Tcw.row(2) - Tcw.row(1);

    }
    std::cout<<"D Matrix is :"<<D<<std::endl;

    //step2:recale D,找到D中最大元素
   // 对D进行缩放S，其中 S 为一个对角阵,取 D 最大元素之逆即可
    Eigen::MatrixXd::Index maxRow, maxCol;
    double max = D.maxCoeff(&maxRow,&maxCol);
    //注意是D的绝对值中最大元素的D.cwiseAbs().maxCoeff()
    std::cout << "Max = \n" << max <<"行:"<<maxRow<<"列"<<maxCol<< std::endl;

    Eigen::MatrixXd DtD((D/max).transpose()*(D/max));

    clock_t time_stt = clock();

    Eigen::JacobiSVD<Eigen::MatrixXd> svd_holder(DtD, Eigen::ComputeThinU | Eigen::ComputeThinV );

    clock_t time_stt1 = clock();


    std::cout<<"SVD分解，耗时:\n"<<(clock()-time_stt1)/(double) CLOCKS_PER_SEC<<"ms"<<std::endl; 

     // 构建SVD分解结果 Eigen::MatrixXd U = svd_holder.matrixU(); Eigen::MatrixXd V = svd_holder.matrixV(); 
     Eigen::MatrixXd S(svd_holder.singularValues()); 
     std::cout<<"singularValues :\n"<<S<<std::endl;

     //step3:判断分解是否有效,然后求解y
     if(std::abs(svd_holder.singularValues()[3]/svd_holder.singularValues()[2]) < 1e-2){

        Eigen::Vector4d U4 = (max*svd_holder.matrixU().rightCols(1));//最后一列
        P_est = (U4/U4[3]).head(3); // U4是一个4*1的向量，最后以为归一化后，取前三维为y

     }
     else{

        std::cout<<"这次求解无效"<<std::endl;
     }
     
    /* your code end */
    
    std::cout <<"ground truth: \n"<< Pw.transpose() <<std::endl;
    std::cout <<"your result: \n"<< P_est.transpose() <<std::endl;
    return 0;
}

参考：手写VIO学习总结（五）