【深蓝学院】手写VIO第6章--视觉前端--作业(SVD分解部分复习)

0. 题目

T1.

奇异值分解需要补，看这篇博客，讲的很好。
总结一下，关于奇异值分解(Singular Value Decomposition，SVD )有以下内容摘抄自该博客，关于SDV分解的部分应该是摘自李航《统计学习方法里面的》：

1. 特征值分解

设 A 为 n 阶方阵，若存在数 λ 和非零向量 x，使得
$$Ax=\lambda x(x\ne 0)$$
则称$\lambda$是A的一个特征值。
求出了矩阵 A 的 n 个特征值${\lambda }_1\le {\lambda }_2\le {\lambda }_3...\le {\lambda }_n$，这n个特征值对应的特征向量分别为$p_1,p_2,p_3...p_n$，如果这n个特征向量线性无关，则矩阵A即可以用下式的特征分解表示
$$A=P\Lambda P^{-1}$$ 其中$P=\{p_1,p_2,p_3...p_n\}$是这 n 个特征向量张开成的 n*n 矩阵，$\Lambda$是以这n个特征值为主对角线的n*n维对角阵。
一般我们会把$P$的 n 个特征向量标准化，此时，这 n 个特征向量为标准正交基，满足$P\ast P^T=I$ ，即$P^T=P^{-1}$，这样特征分解表达式可以写成：$$A=P\Lambda P^T$$
注意，要进行特征分解，矩阵 A 必须为方阵。如果A不是方阵，即行和列不相同时，我们还可以对矩阵进行分解吗？答案是可以，此时我们的SVD登场了。

2. SVD分解

SVD 定义：矩阵的奇异值分解是指，将一个非零的 m × n 实矩阵 A，表示为以下三个实矩阵乘积形式的运算 (SVD 可以更一般地定义在复数矩阵上，但本文不涉及)，即进行矩阵的因子分解：
$$A=U\Sigma V^T$$
其中 U 是 m 阶正交矩阵，V 是 n 阶正交矩阵，Σ 是由降序排列的非负的对角线元素组成的 m × n 矩形对角矩阵。满足：

$A=U\Sigma V^T$为矩阵$A$的SVD分解，${\sigma }_i$为$A$的奇异值，$U$的列项向量称为$A$的左奇异向量，V的列向量称为$A$的右奇异向量。

奇异值分解不要求矩阵 A 是方阵，事实上矩阵的奇异值分解可以看做是方阵的对角化的推广。

2.1 SVD 基本定理

若 A 为一 m × n 实矩阵 ，则 A 的奇异值分解存在
$$A=U\Sigma V^T$$

其中 U 是 m 阶正交矩阵，V 是 n 阶正交矩阵，Σ 是 m × n 矩形对角矩阵，其对角线元素非负，且按降序排列。

这个定理表达的意思就是矩阵的奇异值分解是一定存在的 (但不唯一)，这里就不具体证明了。

2.2 主要性质

1. 设矩阵A的AVD分解为$A=U\Sigma V^T$，则以下关系成立：
   $$\begin{array}{rl}& A^{T}A=(U\Sigma V^T{)}^T(U\Sigma V^T)=V({\Sigma }^T\Sigma )V^T\\ & A{A}^T=(U\Sigma V^T)(U\Sigma V^T{)}^T=U({\Sigma }^T\Sigma )U^T\end{array}$$
   也就是说，矩阵 $A^{T}A$ 和 $A{A}^T$ 的特征分解存在，且可以由矩阵 $A$ 的奇异值分解的矩阵表示。$V$ 的列向量是 $A^{T}A$ 的特征向量，$U$ 的列向量是 $A{A}^T$ 的特征向量，$\Sigma$ 的奇异值是 $A^{T}A$ 和 $A{A}^T$ 的特征值的平方根。
2. 在矩阵 A 的奇异值分解中，奇异值、左奇异向量和右奇异向量之间存在对应关系：
   $$\begin{gathered} AV=U\Sigma \\ A{v}_j={\sigma }_j{u}_j,j=1,2,...,n \end{gathered}$$
   类似的，奇异值、右奇异向量和左奇异向量之间存在对应关系：
   $$\begin{gathered} A^{T}U=V{\Sigma }^T \\ {A}^T{u}_j={\sigma }_j{v}_j,j=1,2,...,n \\ {A}^T{u}_j=0,j=n+1,n+2,...,m \end{gathered}$$
3. 矩阵 $A$ 的奇异值分解中，奇异值 ${\sigma }_1,{\sigma }_2,...{\sigma }_n$是唯一的，而矩阵 $U$ 和 $V$ 不是唯一的。
4. 矩阵 $A$ 和 $\Sigma$ 的秩相等，等于正奇异值${\sigma }_i$ 的个数 $r$ (包含重复的奇异值)。

上述的公式$A^{T}A$与Dan Simon的《Optimal State Estimation Kalman, H∞, and Nonlinear Approaches》P6—P7中的内容（公式（1.16））相结合，非常容易得出课件中的公式（15）。

3. 解题

用上面的知识铺垫可以来解决T1

助教答案


同时参考了博客，但是其证明过程感觉有问题，所以主要参考了助教的方法。

方法3的拉格朗日算子方法需要了解拉格朗日对偶，作为拓展：

可以参考这篇博客里面使用的Langrange算子法来证明的，我觉得更易理解，目标函数实际上就是特征值大小，故取最小特征值能够使目标函数最小。

由于SVD分解出来的$V^T$右正交于$A^{T}A$，所以其特征向量是$A^{T}A$的特征向量，由于特征值降序排列，特征值越小，目标函数越小，所以最小的特征值是$\Lambda$的最右列的，对应的特征向量就是$V^T$最右列的向量使得目标函数最小，即为我们所求的解。

以上就是求$min||Dy|{|}_2^2$的最小二乘解的方法，求出y之后就可以求出观测点的深度值，即完成三角化。

T2.

代码:

typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> MatXX;
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 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);//实际上是Rp+t，拆开来看就是Rwc^T * Pw +(-Rwc * twc)= Rcw * Pw + tcw

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

        camera_pose[i].uv = Eigen::Vector2d(x/z,y/z);//因为camera内参为1，1，所以内参可以忽略
    }
    
    /// TODO::homework; 请完成三角化估计深度的代码
    // 遍历所有的观测数据，并三角化
    Eigen::Vector3d P_est;           // 结果保存到这个变量
    P_est.setZero();
    /* your code begin */
    //1.构建D
    int D_size = end_frame_id - start_frame_id;
    MatXX D(MatXX::Zero( 2 * D_size, 4));//D维度为2n*4
    for(int i=start_frame_id; i<end_frame_id; ++i) {
        //构建投影矩阵Pk
        MatXX Pi(MatXX::Zero(3,4));
        Eigen::Matrix3d Rcw = camera_pose[i].Rwc.transpose();
        Pi.block(0,0,3,3) = Rcw;
        Pi.block(0,3,3,1) = -Rcw * camera_pose[i].twc;//tcw，变换矩阵求逆
        cout << "i = " <<i <<",   Pi_block: \n" << Pi <<endl;
        //构建2*4的矩阵快
        MatXX tmp_mat = camera_pose[i].uv * Pi.block(2,0,1,4) - Pi.block(0,0,2,4);
        D.block((i-start_frame_id) * 2, 0, 2, 4) = camera_pose[i].uv * Pi.block(2,0,1,4) - Pi.block(0,0,2,4);
    }
    cout << "the whole D mat, size: " << D.size() << "\nMat is:\n" << D <<endl;
    //2.对D进行rescale
    MatrixXd::Index maxRow, maxCol;
    double max = D.maxCoeff(&maxRow,&maxCol);
//    max = 1;//取消scale
    cout << "max element of D is: " << max <<endl;
    printf("maxRow: %ld, maxCol: %ld\n", maxRow, maxCol);
    D /= max;
    //3.对D^TD进行SVD（参数有ComputeThinU | ComputeThinV 和 ComputeFullU | ComputeFullV 这个位置不传参就代表你只想计算特征值，不关注左右特征向量(UV矩阵)，
    // 传参就代表你想计算出左右特征向量，而full就是告诉函数计算出来的UV方阵，也就是Matrix3d，计算出来的就是3*3的方阵，thin只在矩阵维度不知道时使用，即n*p的矩阵D，不知道n和p谁更小，假设m=min(n,p),
    // 那么计算结果： U：n*m, V:p*m, 其所代表的特征向量均不是对应实际的\sigma中的特征值的)
    JacobiSVD<MatrixXd> svd(D.transpose() * D, ComputeThinU | ComputeThinV);//D^T*D 进行SVD分解
    cout << "Its singular values are:\n" << svd.singularValues() << endl;
    cout << "Its left singular vectors are the columns of the thin U matrix:\n" << svd.matrixU() << endl;
    cout << "Its right singular vectors are the columns of the thin V matrix:\n" << svd.matrixV() << endl;
    //4.判断解的有效性(\sigma_4 / \sigma_3 < 1e-2 ?)
    double judge_value = std::abs(svd.singularValues()(3) / svd.singularValues()(2));
    if(judge_value < 1e-2) {
        Eigen::Vector4d u4 = max * svd.matrixU().rightCols(1);
        cout << "this Triangulation is valid, judge_value:" << judge_value << endl << "u4 is: \n" <<  u4 << endl;//最后一列（为什么是U不是V？）
        //5.对triangulation的结果(4维)进行归一化(最后一维变为1)
        P_est = (u4/u4(3)).head(3);
    }
    /* your code end */
    
    std::cout <<"ground truth: \n"<< Pw.transpose() <<std::endl;
    std::cout <<"your result: \n"<< P_est.transpose() <<std::endl;
    // TODO:: 请如课程讲解中提到的判断三角化结果好坏的方式，绘制奇异值比值变化曲线

    return 0;
}

其中对svd的参数进行稍微解释：

1. 构造参数：MatrixXd: 计算的特征向量的维度（n维方阵就是Matrixnd，n*m非方阵就填MatrixXd，不确定）
2. 传参：ComputeThinU | ComputeThinV 和 ComputeFullU | ComputeFullV 这个位置不传参就代表你只想计算特征值，不关注左右特征向量(UV矩阵)，传参就代表你想计算出左右特征向量，而full就是告诉函数计算出来的UV方阵，也就是Matrixnd，计算出来的就是nn的方阵，thin只在矩阵维度不知道时使用，即np的矩阵D，不知道n和p谁更小，假设m=min(n,p), 那么计算结果： U：nm, V:pm, 其所代表的特征向量均不是对应实际的$\Sigma$中的特征值的

参考博客：

最终的结果，

1. $\frac{{\sigma }_4}{{\sigma }_3}<<1e-2$，满足有效性阈值，所以triangulation有效，
2. 进而改变max=1取消scale，发现结果仍然正确，说明D的数值偏小（按照课上来说几十万算是大的）时，对SVD的结果没太大影响。
   
   

提升T1.

Ch5作业中TestMonoBA.cpp给的观测添加噪声的方法

    // 随机数生成三维特征点
    std::default_random_engine generator;
    std::normal_distribution<double> noise_pdf(0., 1. / 1000.);  // 2pixel / focal
    for (int j = 0; j < featureNums; ++j) {
        std::uniform_real_distribution<double> xy_rand(-4, 4.0);
        std::uniform_real_distribution<double> z_rand(4., 8.);

        Eigen::Vector3d Pw(xy_rand(generator), xy_rand(generator), z_rand(generator));
        points.push_back(Pw);

        // 在每一帧上的观测量
        for (int i = 0; i < poseNums; ++i) {
            Eigen::Vector3d Pc = cameraPoses[i].Rwc.transpose() * (Pw - cameraPoses[i].twc);
            Pc = Pc / Pc.z();  // 归一化图像平面
            Pc[0] += noise_pdf(generator);
            Pc[1] += noise_pdf(generator);
            cameraPoses[i].featurePerId.insert(make_pair(j, Pc));
        }
    }

如下添加观测noise

    for(int j=1; j<50; ++j) {
        std::normal_distribution<double> noise_pdf(0., (double)j / 1000.);  // 2pixel / focal，修改var可改变噪声大小
        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);//实际上是Rp+t，拆开来看就是Rwc^T * Pw +(-Rwc * twc)= Rcw * Pw + tcw

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

            camera_pose[i].uv = Eigen::Vector2d(x/z + noise_pdf(generator),y/z + noise_pdf(generator));//因为camera内参为1，1，所以内参可以忽略
        }
    //else balabala
}

Gaussian distribution的参数1 / 1000表示2个pixel对应到归一化平面的误差参数，也是cov

不同参数对应的结果如下：

曲线如下，使用gnuplot绘制，执行以下指令生成绘制曲线图：

cat muban.conf | gnuplot

gnuplot配置文件muban.conf

set terminal gif small size 900,780
set output "var_judge.gif" #指定输出gif图片的文件名
set autoscale
#set xdata time
#set timefmt "%s"
#set format x "%S"
set title "sigma_4/sigma_3 under different noise var" #图片标题
set style data lines #显示网格
set xlabel "noise_var" #X轴标题
set ylabel "sigma_4/sigma_3" #Y轴标题
set grid #显示网格
plot "data.txt" using 1:4 title "sigma_4/sigma_3"

随着噪声varince的上升，比值逐渐增大，到5e-2时已经不valid了，证明噪声越大，Triangulation误差越大。

提升T2.

固定noise var为5/1000，改变end_point，从1帧到7帧，随着观测数量的增多，三角化的误差越来越小。

本章完。