从零开始手写VIO第六章作业（含公式推导依据及编程思路）

前言·与同主题博文的不同

延续该系列博文的风格，本博文站在前人优秀博文的基础上，尽量补充同主题博文所没有涉及到的关键点。之前同主题优秀博文已经讲解非常清楚的部分，本博文中将直接给出链接，对于之前博文没有涉及到的点，本博文会重点阐述。欢迎有兴趣的朋友讨论交流。
对于第六章，所补充的关键点包含以下几个方面：第一题给出了公式推导中应用到的一些定理和矩阵运算技巧，并说明了课件中的公式（15）是如何得到的，第二题结合课件中的理论给出了编程思路，第三题参考第五章代码给出了噪声参数设置的标准。

1.证明式（15）

题目如下图：
公式（15）如下图：

1.1对公式（15）的说明

小编看了同主题的优秀博文，虽然给出了SVD的详细介绍，但是对于如何由SVD定义得到公式（15）却没有任何提及。
为了便于说明这里先给出李航的《统计学习方法第2版》P271—P272中引出的SVD：

下面小编结合李航的《统计学习方法第2版》P280—P281中的内容来说明公式（15）是如何得到的。该书中的矩阵A也就是课件中的矩阵D。课件中的公式（15）最原始的表达式是下图中的公式（15.20），容易知道其中Σ^TΣ是σ_i²所构成的对角线矩阵。

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

另外，下面1.2中的推导过程也用到了上述李航的《统计学习方法第2版》P280—P281公式（15.22），即矩阵 A 的右奇异向量和奇异值、左奇异向量的关系。

1.2推导过程

请结合1.1中的内容参考博文从零手写VIO(六)中的相关内容。

2.请依据本节课公式，完成特征点三角化代码，并通过仿真测试

2.1理论依据

先给出课件中对应的理论内容，然后整理为对应的编程思路。
课件中对应的理论内容在P26—P29，如下：




从后往前整理分析，最后求解的目标是P29中的y，所以需要对D^TD做SVD分解（公式15），那么首先需要构造出D。公式（13）给出了矩阵D的具体表达式，最后结合公式（10-12）和P26中的具体内容可知，需要知道观测量和投影矩阵就可以求出D。
需要注意的是，这里没有涉及P29中的Rescale和y投影正负号判断。

2.2代码及仿真结果

下面是原代码中相机pose的结构体：

struct Pose
{
    Pose(Eigen::Matrix3d R, Eigen::Vector3d t):Rwc(R),qwc(R),twc(t) {};
    Eigen::Matrix3d Rwc;
    Eigen::Quaterniond qwc;
    Eigen::Vector3d twc;

    Eigen::Vector2d uv;    // 这帧图像观测到的特征坐标
};

这里已知的是Rwc和twc，而2.1中所用的的pose是Rcw和tcw，所以还需要做一步转换，转换公式参考《视觉SLAM十四讲》P46公式（3-13），如下图：

具体代码如下：

    /* your code begin */
    Eigen::Matrix<double, Eigen::Dynamic, 4> D(2 * (poseNums - start_frame_id), 4);
    Eigen::RowVector4d P_1 = Eigen::RowVector4d::Zero(4);
    Eigen::RowVector4d P_2 = Eigen::RowVector4d::Zero(4);
    Eigen::RowVector4d P_3 = Eigen::RowVector4d::Zero(4);
    int k=0;
    for (int i = start_frame_id; i < end_frame_id; ++i) { 
	Eigen::Matrix3d Rcw = camera_pose[i].Rwc.transpose();
	Eigen::Vector3d tcw = -Rcw*camera_pose[i].twc;
	P_1 << Rcw.block<1, 3>(0, 0), tcw.x();
	P_2 << Rcw.block<1, 3>(1, 0), tcw.y();
	P_3 << Rcw.block<1, 3>(2, 0), tcw.z();
	D.block<1, 4>(k, 0) = camera_pose[i].uv.x()*P_3-P_1;
	D.block<1, 4>(k+1, 0) = camera_pose[i].uv.y()*P_3-P_2;
	k+=2;
    }
    Eigen::MatrixXd DTD = D.transpose() * D;
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(DTD, Eigen::ComputeThinU | Eigen::ComputeThinV);
    Eigen::MatrixXd U = svd.matrixU();
    P_est = U.block<3, 1>(0, 3) / U(3, 3);//取齐次坐标的前三维，并同时除以第四维坐标
    Eigen::Vector4d Singular_values=svd.singularValues();
    std::cout<<"Singular values:\n"<<Singular_values<<std::endl;
    std::cout<<"sigma4/sigma3:\n"<<Singular_values[3]/Singular_values[2]<<std::endl;
    /* your code end */

另外，关于函数.block的说明如下：

.block(i, j, p, q)    //起点(i, j)，块大小(p, q)，构建一个动态尺寸的block
.block<p, q>(i, j)  // 构建一个固定尺寸的block

仿真结果如下图：

可以看出，σ4/σ3（最小奇异值和第二小奇异值之间的比例）小于10^-4，三角化有效，在不加任何噪声的情况下三角化求得的空间点 P_est 与原空间点 Pw 完全相同。

3.请对测量值加上不同噪声（增大测量噪声方差），观察最小奇异值和第二小奇异值之间的比例变化，并绘制比例值的变化曲线

3.1噪声参数设置

首先需要做的事情是选择合理的噪声参数，下面是第五章中老师所给代码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));
    }
}

关于第二行，小编的理解如下，1. / 1000.表示2个像素误差对应到归一化平面的误差参数。小编以此作为噪声参数设定的标准。

3.2代码及仿真结果

参考3.1中的代码，修改代码如下：

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);

    std::normal_distribution<double> noise_pdf(0., ???. / 2000.);

    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));
}

只需要修改std::normal_distribution noise_pdf(0., ???. / 2000.);中???部分。下面是仿真结果，噪声方差依次对应1—10个像素误差。

曲线图如下：

上图中所有σ4/σ3（最小奇异值和第二小奇异值之间的比例）在10^-4量级，三角化都有效，随着方差的增大，σ4/σ3也在不断增大，说明三角化误差越来越大。

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

通过修改代码中的变量start_frame_id来设置观测帧数。噪声参数固定为10个像素误差（10. / 2000.），观测帧数设置为1—10，仿真结果如下，其中观测帧数为1时，无法完成三角化，σ4/σ3为0.48，三角化无效。

曲线图如下（舍弃观测帧数为1时的情况，从观测帧数为2时开始绘制）：

从图中可以看出，所有σ4/σ3（最小奇异值和第二小奇异值之间的比例）都在10^-4量级，三角化都有效，随着观测帧数的增加，σ4/σ3整体呈减小趋势，中间略有波动。