手写vio——第一章习题

第一章习题
一、
1、视觉与IMU融合后的优势？
从传感器特性来看，IMU的优势在于可以高频的感知到运动的状态变化信息角速度和加速度，进而可以通过积分计算出状态值。其劣势则在于受零偏、温漂、制造精度的影响，只能在短时间内进行积分，时间一长，估计的状态与真值差别巨大。
而相机的优势在于不会有漂移的问题，静止不动得到的图像数据不会发生变动，而其劣势在于其频率较低，且在高速运动时，对于卷帘式快门，得到的是模糊的图像，或者运动幅度过大，由于是低频采样，导致图像无重合或重合度低，或者是重复纹理的地方如走廊上，没有有效特征，就没有办法提取对应的特征来进行运动估计了，这个时候将会丢失所跟踪的状态。
从估计的数据来看，相机得到的图像可以根据图像内容的变化进行相机与环境之间的姿态变化估计，由于没有绝对数值参考，估计的平移量是缺乏尺度的。但两帧图像之间的变化可以是相机运动造成的，也可以是环境变化造成的，这个是纯相机的弱点；而IMU由于得到的是物理量，可以直接进行位移、角度的估计，且可以明确自己是否在运动。

从上面的分析可以看到，对于相机的高速运动或运动过大，可以由IMU提供对应时刻的相对运动估计来保持跟踪，而对于IMU估计值会发散的问题，则可以由图像的估计来进行纠正，就像是加了一个gps传感器一样。
所以，相机与IMU的融合，可以带来的优势包括：克服弱势场景，保证姿态跟踪的连续性，保证姿态估计的精度。

2、有哪些常见的视觉+IMU融合方案？有没有工业界应用的例子？
常见方案：
MSCKF
ROVIO
OKVIS
VIORB
VINS-MONO VINS-FUSION
ICE-BA

工业界应用例子：
苹果:ARKit
Google:ARCore
百度:DuMix AR
微软:HoloLens

3、在学术界，VIO研究有哪些进展？有没有将学习方法应用到VIO的例子？
新进展:
[1] Usenko V , Demmel N , Schubert D , et al. Visual-Inertial Mapping with Non-Linear Factor Recovery[J]. 2019.

[2] Shao W , Vijayarangan S , Li C , et al. Stereo Visual Inertial LiDAR Simultaneous Localization and Mapping[J]. 2019.

[3] Karnik Ram, Chaitanya Kharyal, Sudarshan S. Harithas, K. Madhava Krishna,et al. RP-VIO: Robust Plane-based Visual-Inertial Odometry for Dynamic Environments.2021

[4] Jeff Delaune, David S. Bayard, Roland Brockers,et al.Range-Visual-Inertial Odometry: Scale Observability Without Excitation . 2021

[5] Shaozu Cao, Xiuyuan Lu, Shaojie Shen,et al.GVINS: Tightly Coupled GNSS-Visual-Inertial Fusion for Smooth and Consistent State Estimation .2021

基于学习的方法:
[1] Shamwell, E. Jared, et al. “Unsupervised Deep Visual-Inertial Odometry with Online Error Correction for RGB-D Imagery.” IEEE transactions on pattern analysis and machine intelligence (2019).

[2] Chen, Changhao, et al. “Selective Sensor Fusion for Neural Visual-Inertial Odometry.” arXiv preprint arXiv:1903.01534 (2019).

[3] Yasin Almalioglu, Mehmet Turan, Alp Eren Sari, Muhamad Risqi U. Saputra, et al. SelfVIO: Self-Supervised Deep Monocular Visual-Inertial Odometry and Depth Estimation .2019

[4] Xingxing Zuo, Nathaniel Merrill, Wei Li, Yong Liu, Marc Pollefeys, Guoquan Huang. CodeVIO: Visual-Inertial Odometry with Learned Optimizable Dense Depth .2020

[5] Koji Minoda, Fabian Schilling, Valentin Wüest, Dario Floreano, Takehisa Yairi et al.VIODE: A Simulated Dataset to Address the Challenges of Visual-Inertial Odometry in Dynamic Environments .2021

二、

#include <iostream>
#include <vector>
#include <cmath>
#include <assert.h>

using namespace std;


class VIO_Matrix3x3;
class VIO_Quater;
class VIO_Vector3d;


double vec_multi_vec(std::vector<double> a,std::vector<double> b,int size){
    assert(size>0);
    assert(a.size()==size);
    assert(b.size()==size);

    double sum=0;

    for(int i=0;i<size;++i){
        sum+=a[i]*b[i];
    }

    return sum;

}

class VIO_Vector3d{
private:
    double data_[3];

public:
    
    VIO_Vector3d(std::vector<double> ds){
        assert(ds.size()==3);
        data_[0]=ds[0];
        data_[1]=ds[1];
        data_[2]=ds[2];
    };

    VIO_Vector3d Norm(){
        double sq_sum=data_[0]*data_[0]+data_[1]*data_[1]+data_[2]*data_[2];
        sq_sum=sqrtf(sq_sum);

        double n1=data_[0]/sq_sum;
        double n2=data_[1]/sq_sum;
        double n3=data_[2]/sq_sum;

        std::vector<double> ds={n1,n2,n3};

        VIO_Vector3d res(ds);

        return res;
    }

    double GetTheta(){
        double sq_sum=data_[0]*data_[0]+data_[1]*data_[1]+data_[2]*data_[2];
        sq_sum=sqrtf(sq_sum);

        return sq_sum;
    }

    double operator[](int i){
        assert(i<3);
        return data_[i];
    }

    VIO_Matrix3x3 ToMatrix();

};

class VIO_Quater{
    private:
    double w=0;
    double x=0,y=0,z=0;

    public:
    VIO_Quater(){};

    VIO_Quater(double tw,double tx,double ty,double tz):w(tw),x(tx),y(ty),z(tz){};

    VIO_Matrix3x3 ToRotateMatrix();

    VIO_Quater MultiOther(VIO_Quater other){
        double sw=w*other.w-x*other.x-y*other.y-z*other.z;
        double sx=w*other.x+x*other.w+y*other.z-z*other.y;
        double sy=w*other.y-x*other.z+y*other.w+z*other.x;
        double sz=w*other.z+x*other.y-y*other.x+z*other.w;

        VIO_Quater res(sw,sx,sy,sz);
        return res;

    }

    void Print(){
        std::cout<<"[w,x,y,z]=["<<w<<","<<x<<","<<y<<","<<z<<"]\n";
    }

    VIO_Quater Normalize(){
        double m=w*w+x*x+y*y+z*z;
        assert(m>0);
        m=sqrtf(m);

        VIO_Quater res(w/m,x/m,y/m,z/m);
        return res;

    }

};


class VIO_Matrix3x3{
private:
    double data[3][3]={0};

public:
    VIO_Matrix3x3(){};
    VIO_Matrix3x3(std::vector<double> d){
        assert(d.size()==9);
        int idx=0;
        for(int i=0;i<3;++i){
            for(int j=0;j<3;++j){
                data[i][j]=d[idx++];
            }
        }
    }
    void Print(){
        for(int i=0;i<3;++i){
            for(int j=0;j<3;++j){
                printf("%0.9f\t",data[i][j]);
            }
            std::cout<<endl;
        }
    }

    VIO_Quater ToQuater(){
        double w=sqrtf(this->Trace()+1)/2;
        double x=(data[2][1]-data[1][2])/(4*w);
        double y=(data[0][2]-data[2][0])/(4*w);
        double z=(data[1][0]-data[0][1])/(4*w);

        VIO_Quater res(w,x,y,z);
        return res;

    }

    double Trace(){
        double res=data[0][0]+data[1][1]+data[2][2];
        return res;
    }
    std::vector<double> GetRow(int idx){
        std::vector<double> res;
        for(int i=0;i<3;++i){
            res.push_back(data[idx][i]);
        }
        return res;
    }
    std::vector<double> GetCol(int idx){
        std::vector<double> res;
        for(int i=0;i<3;++i){
            res.push_back(data[i][idx]);
        }
        return res;
    }

    VIO_Matrix3x3 MultiOther(VIO_Matrix3x3 other){
        std::vector<double> res;
        for(int i=0;i<3;++i){
            std::vector<double> left=this->GetRow(i);
            for(int j=0;j<3;++j){
                std::vector<double> right=other.GetCol(j);
                double ele=vec_multi_vec(left,right,3);
                res.push_back(ele);
            }
        }

        VIO_Matrix3x3 fin(res);
        return fin;
    }

    VIO_Matrix3x3 Add(VIO_Matrix3x3 other){
        std::vector<double> dts;
        for(int i=0;i<3;++i){
            for(int j=0;j<3;++j){
                data[i][j]+=other.data[i][j];
                dts.push_back(data[i][j]);
            }
        }
        VIO_Matrix3x3 res(dts);
        return res;
        
    }

    VIO_Matrix3x3 sub(VIO_Matrix3x3 other){
        std::vector<double> dts;
        for(int i=0;i<3;++i){
            for(int j=0;j<3;++j){
                data[i][j]-=other.data[i][j];
                dts.push_back(data[i][j]);
            }
        }
        VIO_Matrix3x3 res(dts);
        return res;
        
    }


    VIO_Matrix3x3 MultiScale(double s){
        std::vector<double> dts;
         for(int i=0;i<3;++i){
            for(int j=0;j<3;++j){
                data[i][j]*=s;
                dts.push_back(data[i][j]);
            }
        }

        VIO_Matrix3x3 res(dts);
        return res;
    }

    std::vector<double> GetData(){
        std::vector<double> dts;
        for(int i=0;i<3;++i){
            for(int j=0;j<3;++j){
                dts.push_back(data[i][j]);
            }
        }

        return dts;
    }

};

VIO_Matrix3x3 VIO_Quater::ToRotateMatrix(){
        std::vector<double> datas;
        datas.push_back(1-2*y*y-2*z*z);
        datas.push_back(2*x*y-2*z*w);
        datas.push_back(2*x*z+2*y*w);

        datas.push_back(2*x*y+2*z*w);
        datas.push_back(1-2*x*x-2*z*z);
        datas.push_back(2*y*z-2*x*w);

        datas.push_back(2*x*z-2*y*w);
        datas.push_back(2*y*z+2*x*w);
        datas.push_back(1-2*x*x-2*y*y);

        VIO_Matrix3x3 res(datas);

        return res;
    }
VIO_Matrix3x3 vec_multi_hor_vec(std::vector<double> a,std::vector<double> b){
    
    assert(a.size()==3 && b.size()==3);
    std::vector<double> res;
    for(int i=0;i<3;++i){
        for(int j=0;j<3;++j){
            res.push_back(a[i]*b[j]);
        }
    }

    VIO_Matrix3x3 m(res);
    return m;

}

//旋转向量->旋转矩阵
VIO_Matrix3x3 rotate_vec_to_matrix(std::vector<double> rv){
    assert(rv.size()==3);

    double theta=rv[0]*rv[0]+rv[1]*rv[1]+rv[2]*rv[2];
    theta=sqrtf(theta);
    cout<<"theta="<<theta<<endl;

    assert(theta!=0);

    double axis_x=rv[0]/theta;
    double axis_y=rv[1]/theta;
    double axis_z=rv[2]/theta;

    cout<<"axis_x="<<axis_x
    <<",axis_y="<<axis_y
    <<",axis_z="<<axis_z
    <<endl;


    double cost=cos(theta);
    double cost_1=1-cost;
    double sint=sin(theta);

    VIO_Matrix3x3 I({1,0,0,0,1,0,0,0,1});
    VIO_Matrix3x3 A_sym({ 0,      -axis_z,     axis_y,
                  axis_z,  0,          -axis_x,
                  -axis_y, axis_x,     0});

    cout<<"A_sym=\n";
    A_sym.Print();

    std::vector<double> A={axis_x,axis_y,axis_z};
    std::vector<double> At=A;

    VIO_Matrix3x3 AxAt=vec_multi_hor_vec(A,At);


    //cost*I + (1-sint)a*at+sint*a^
    VIO_Matrix3x3 res=I.MultiScale(cost);
    res=res.Add(AxAt.MultiScale(1-cost));
    res=res.Add(A_sym.MultiScale(sint));
    

    return res;
    
}


VIO_Matrix3x3 VIO_Vector3d::ToMatrix(){

    std::vector<double> rs={data_[0],data_[1],data_[2]};
    return rotate_vec_to_matrix(rs);
}



int main(int argc,char** argv){
    cout<<"==================绕Z轴旋转90度=============\n";
    double init_theta=M_PI/2;
    
    double init_cost=cos(init_theta);
    double init_sint=sin(init_theta);

    VIO_Matrix3x3 init_R({init_cost,-init_sint,0,init_sint,init_cost,0,0,0,1});

    cout<<"初始旋转矩阵:\n";
    init_R.Print();
    cout<<endl;

    VIO_Quater init_quater=init_R.ToQuater();
    cout<<"初始四元数:\n";
    init_quater.Print();
    cout<<endl<<endl;


    VIO_Vector3d small_rot_vec({0.01,0.02,0.03});
    cout<<"旋转角度:"<<small_rot_vec.GetTheta()<<endl;
    VIO_Matrix3x3 small_rot=small_rot_vec.ToMatrix();
    cout<<"旋转矩阵:\n";
    small_rot.Print();
    cout<<endl;


    VIO_Matrix3x3 rot_update=init_R.MultiOther(small_rot);
    cout<<"更新后旋转矩阵:\n";
    rot_update.Print();
    cout<<endl;

    
    VIO_Quater small_quat({1,small_rot_vec[0]/2,small_rot_vec[1]/2,small_rot_vec[2]/2});
    cout<<"角速度转四元数:\n";
    small_quat.Print();
    cout<<endl;

    VIO_Quater quater_update=init_quater.MultiOther(small_quat);
    quater_update=quater_update.Normalize();
    cout<<"更新后四元数(归一化后):\n";
    quater_update.Print();
    cout<<endl;

    VIO_Matrix3x3 quater_update_rot=quater_update.ToRotateMatrix();
    cout<<"更新后四元数对于旋转矩阵:\n";
    quater_update_rot.Print();
    cout<<endl;

    VIO_Matrix3x3 diff_aft_update=quater_update_rot.sub(rot_update);
    cout<<"更新后差异:\n";
    diff_aft_update.Print();
    cout<<endl;


    return 0;
}

三、
1、
$$\begin{gathered} \frac{dln{(R^{-1}p)}^{\vee }}{dR} \\ =\underset{\phi \to 0}{\lim }\frac{[Rexp({\phi }^{\wedge }){]}^{-1}p-R^{-1}p}{\phi } \\ =\underset{\phi \to 0}{\lim }\frac{exp({\phi }^{\wedge }{)}^{-1}{R}^{-1}p-R^{-1}p}{\phi } \\ =\underset{\phi \to 0}{\lim }\frac{(I+{\phi }^{\wedge }{)}^{-1}{R}^{-1}p-R^{-1}p}{\phi } \\ =\underset{\phi \to 0}{\lim }\frac{(I+{\phi }^{\wedge }{)}^T{R}^{-1}p-R^{-1}p}{\phi } \\ =\underset{\phi \to 0}{\lim }\frac{(I+({\phi }^{\wedge }{)}^T)R^{-1}p-R^{-1}p}{\phi } \\ =\underset{\phi \to 0}{\lim }\frac{(I-({\phi }^{\wedge }))R^{-1}p-R^{-1}p}{\phi } \\ =\underset{\phi \to 0}{\lim }\frac{-{\phi }^{\wedge }{R}^{-1}p}{\phi } \\ =\underset{\phi \to 0}{\lim }\frac{(R^{-1}p{)}^{\wedge }\phi }{\phi } \\ =(R^{-1}p{)}^{\wedge } \end{gathered}$$

2、
$$\begin{gathered} \frac{dln(R_1{R}_2^{-1}{)}^{\vee }}{R_2} \\ =\underset{\phi \to 0}{\lim }\frac{ln(R_1(R_{2}exp(\phi {)}^{\wedge }{)}^{-1}{)}^{\vee }-ln(R_1{R}_2^{-1}{)}^{\vee }}{\phi } \\ =\underset{\phi \to 0}{\lim }\frac{ln(R_1(exp({\phi }^{\wedge }){)}^{-1}{R}_2^{-1}{)}^{\vee }-ln(R_1{R}_2^{-1}{)}^{\vee }}{\phi } \\ =\underset{\phi \to 0}{\lim }\frac{ln(R_1(exp(-{\phi }^{\wedge }))R_2^{-1}{)}^{\vee }-ln(R_1{R}_2^{-1}{)}^{\vee }}{\phi } \\ =\underset{\phi \to 0}{\lim }\frac{ln(R_1{R}_2^T{R}_2(exp(-{\phi }^{\wedge }))R_2^{-1}{)}^{\vee }-ln(R_1{R}_2^{-1}{)}^{\vee }}{\phi } \\ =\underset{\phi \to 0}{\lim }\frac{ln(R_1{R}_2^T{R}_2(exp(-{\phi }^{\wedge }))R_2^T{)}^{\vee }-ln(R_1{R}_2^{-1}{)}^{\vee }}{\phi } \\ =\underset{\phi \to 0}{\lim }\frac{ln(R_1{R}_2^T(exp(-R_2{\phi }^{\wedge })){)}^{\vee }-ln(R_1{R}_2^{-1}{)}^{\vee }}{\phi } \\ =\underset{\phi \to 0}{\lim }\frac{ln(R_1{R}_2^T{)}^{\vee }+J_r^{-1}(ln(R_1{R}_2^{-1}{)}^{\vee })(-R_2\phi )-ln(R_1{R}_2^{-1}{)}^{\vee }}{\phi } \\ =-J_r^{-1}(ln(R_1{R}_2^{-1}{)}^{\vee })(R_2) \end{gathered}$$