深蓝学院《从零开始手写VIO》作业6

1.证明式(15)

wiki百科奇异值分解
奇异值分解理解

2.完成三角化代码

    /* your code begin */
    int row = (end_frame_id - start_frame_id) * 2;

    Eigen::MatrixXd D(Eigen::MatrixXd::Zero(row, 4));
    for (int i = start_frame_id; i < end_frame_id; ++i) {
        Eigen::Matrix<double, 3, 4> Pi;
        Pi.block<3,3>(0,0) = camera_pose[i].Rwc.transpose();
        Pi.block<3,1>(0,3) = - camera_pose[i].Rwc.transpose() * camera_pose[i].twc;
        Eigen::Matrix<double, 1, 4> Pi1 = Pi.block<1,4>(0,0);
        Eigen::Matrix<double, 1, 4> Pi2 = Pi.block<1,4>(1,0);
        Eigen::Matrix<double, 1, 4> Pi3 = Pi.block<1,4>(2,0);

        D.block<1,4>((i-start_frame_id)*2,0) = camera_pose[i].uv.x() * Pi3 - Pi1;
        D.block<1,4>((i-start_frame_id)*2+1,0) = camera_pose[i].uv.y() * Pi3 - Pi2;
    }

    Eigen::JacobiSVD<Eigen::MatrixXd> svd(D.transpose() * D, Eigen::ComputeThinU | Eigen::ComputeThinV);

    Eigen::MatrixXd U = svd.matrixU();
    Eigen::MatrixXd V = svd.matrixV();
    Eigen::MatrixXd A = svd.singularValues();

    std::cout <<"U: \n"<< U <<std::endl;
    std::cout <<"V: \n"<< V <<std::endl;
    std::cout <<"A: \n"<< A <<std::endl;

    P_est = V.block<3,1>(0,3) / V(3,3);
    /* your code end */

3.加上不同噪声

在观测数据上加上不同大小噪声

        std::normal_distribution<double> noise_pdf(0., NoiseArray[j]);  // 2pixel / focal
        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();

            x += noise_pdf(generator);
            y += noise_pdf(generator);

            camera_pose[i].uv = Eigen::Vector2d(x/z,y/z);
        }

判断该解有效性的条件:σ 4 ≪ σ 3 。若该条件成立,认为三角化有效,否则认为三角化无效。

上图中所有σ4/σ3（最小奇异值和第二小奇异值之间的比例）最大也不过2e-5满足条件，三角化都有效。随着噪声标准差的增大，σ4/σ3波动较大但整体趋势是增大的说明三角化估计的结果不确定度也越来越大。

4.改变观测图像帧数

设置观测噪声为0，单纯比较不同帧数的影响

当帧数为1时结果如下：

frame: 1
ground truth: 
  -2.9477 -0.330799   8.43792
your result: 
  -3.10636 -0.0329462    8.14095
scale: 0.320057

此时误差较大，其他结果基本与ground truth一致：

-------------------------------------------------
frame: 2
ground truth: 
  -2.9477 -0.330799   8.43792
your result: 
  -2.9477 -0.330799   8.43792
scale: 7.76232e-13
-------------------------------------------------
frame: 3
ground truth: 
  -2.9477 -0.330799   8.43792
your result: 
  -2.9477 -0.330799   8.43792
scale: 3.22598e-13

.
.
.

-------------------------------------------------
frame: 19
ground truth: 
  -2.9477 -0.330799   8.43792
your result: 
  -2.9477 -0.330799   8.43792
scale: 1.23565e-15

画图时省略帧数为1时的点

随着观测帧数的增加，σ4/σ3整体呈减小趋势，中间略有波动

最终代码如下：

#include <iostream>
#include <vector>
#include <random>  
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>
#include <fstream>
using namespace std;

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;    // 这帧图像观测到的特征坐标
};
int main()
{

    int poseNums = 20;
    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

    ///存csv文件
    std::ofstream csv_data;
    csv_data.open("data.csv", ios_base::out);

    vector<double> NoiseArray;// {0, 1e-3, 2e-3, 3e-3, 4e-3, 5e-3, 6e-3, 7e-3, 8e-3, 9e-3, 10e-3}
    vector<int> FrameArray;

    for (int k = 0; k < 50; ++k) {
        NoiseArray.push_back(k*0.2e-3);
    }

    for (int k = 2; k < poseNums; ++k) {
        FrameArray.push_back(k);
    }

    for (int j = 0; j < FrameArray.size(); ++j) {
        int start_frame_id = poseNums - FrameArray[j];
        int end_frame_id = poseNums;
//        std::normal_distribution<double> noise_pdf(0., NoiseArray[j]);  // 2pixel / focal

        std::cout <<"-------------------------------------------------"<<std::endl;
        std::cout <<"frame: "<< FrameArray[j] <<std::endl;
//        std::cout <<"noise: "<< NoiseArray[j] <<std::endl;

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

//            x += noise_pdf(generator);
//            y += noise_pdf(generator);

            camera_pose[i].uv = Eigen::Vector2d(x/z,y/z);
        }

        /// TODO::homework; 请完成三角化估计深度的代码
        // 遍历所有的观测数据，并三角化
        Eigen::Vector3d P_est;           // 结果保存到这个变量
        P_est.setZero();
        /* your code begin */
        int row = (end_frame_id - start_frame_id) * 2;

        Eigen::MatrixXd D(Eigen::MatrixXd::Zero(row, 4));
        for (int i = start_frame_id; i < end_frame_id; ++i) {
            Eigen::Matrix<double, 3, 4> Pi;
            Pi.block<3,3>(0,0) = camera_pose[i].Rwc.transpose();
            Pi.block<3,1>(0,3) = - camera_pose[i].Rwc.transpose() * camera_pose[i].twc;
            Eigen::Matrix<double, 1, 4> Pi1 = Pi.block<1,4>(0,0);
            Eigen::Matrix<double, 1, 4> Pi2 = Pi.block<1,4>(1,0);
            Eigen::Matrix<double, 1, 4> Pi3 = Pi.block<1,4>(2,0);

            D.block<1,4>((i-start_frame_id)*2,0) = camera_pose[i].uv.x() * Pi3 - Pi1;
            D.block<1,4>((i-start_frame_id)*2+1,0) = camera_pose[i].uv.y() * Pi3 - Pi2;
        }

        Eigen::JacobiSVD<Eigen::MatrixXd> svd(D.transpose() * D, Eigen::ComputeThinU | Eigen::ComputeThinV);

        Eigen::MatrixXd U = svd.matrixU();
        Eigen::MatrixXd V = svd.matrixV();
        Eigen::MatrixXd A = svd.singularValues();

//        std::cout <<"U: \n"<< U <<std::endl;
//        std::cout <<"V: \n"<< V <<std::endl;
//        std::cout <<"A: \n"<< A <<std::endl;

        P_est = V.block<3,1>(0,3) / V(3,3);
        /* your code end */

        std::cout <<"ground truth: \n"<< Pw.transpose() <<std::endl;
        std::cout <<"your result: \n"<< P_est.transpose() <<std::endl;

        std::cout <<"scale: "<< A(3)/A(2) <<std::endl;

//        csv_data << NoiseArray[j] << "," << A(2) << "," << A(3) << "," << A(3)/A(2) << endl;
        csv_data << FrameArray[j] << "," << A(2) << "," << A(3) << "," << A(3)/A(2) << endl;
    }
    
    // TODO:: 请如课程讲解中提到的判断三角化结果好坏的方式，绘制奇异值比值变化曲线
    return 0;
}

Python绘图：

import numpy as np;
import matplotlib
import matplotlib.pyplot as plt
from matplotlib.pyplot import MultipleLocator #导入此类，设置坐标轴间隔
import csv

Noise = []
Scale = []

Framenum = []
Scale2 = []

#用reder读取csv文件
with open('/home/jevin/code/vio-homework/hw6/datanoise50.csv','r') as csvFile:
    reader = csv.reader(csvFile)
    for line in reader:
        print line
        numbers_float = map(float, line) #转化为浮点数  
        Noise.append( numbers_float[0] )
        Scale.append( numbers_float[3] )

with open('/home/jevin/code/vio-homework/hw6/data.csv','r') as csvFile:
    reader = csv.reader(csvFile)
    for line in reader:
        print line
        numbers_float = map(float, line) #转化为浮点数  
        Framenum.append( int(numbers_float[0]) )
        Scale2.append( numbers_float[3] )

plt.figure(1)  
plt.plot(Noise, Scale, marker='o')
plt.xlabel("Noise")
plt.ylabel("sigma4/sigma3")

plt.figure(2)  
x_major_locator=MultipleLocator(1)#设置横坐标为整数（间隔为1）
ax=plt.gca()
ax.xaxis.set_major_locator(x_major_locator)
plt.plot(Framenum, Scale2, marker='o')
plt.xlabel("Frame Number")
plt.ylabel("sigma4/sigma3")

plt.show()