ORB-SLAM3：初始化

参考：https://blog.csdn.net/weixin_46363611/article/details/113445503?spm=1001.2101.3001.6650.2&utm_medium=distribute.pc_relevant.none-task-blog-2%7Edefault%7ECTRLIST%7Edefault-2.nonecase&depth_1-utm_source=distribute.pc_relevant.none-task-blog-2%7Edefault%7ECTRLIST%7Edefault-2.nonecase
ORB-SLAM3的初始化分为三步，纯视觉的初始化、纯IMU的初始化、VI联合优化。
（1）纯视觉的初始化：使用单目ORB-SLAM的视觉初始化并在一段短时间内（比如两秒）使用BA进行纯视觉MAP估计。同时，计算相邻关键帧之间的IMU预积分和协方差。
（2）纯IMU的初始化：把IMU轨迹和视觉轨迹对齐，并得到尺度、关键帧的速度、重力向量和IMU偏差。我觉得这里和vins-mono的初始化差不太多，vins-mono的初始化是松耦合，先用三角化和PnP得到滑动窗口中pose和landmark，然后在和IMU轨迹对齐。orb-slam3是在得到视觉轨迹的时候也用了优化，然后还是和IMU轨迹对齐，这么看来应该也是个松耦合。但是orb3又加了一步

ORB3是这样的流程：
从main函数开始，先把图像路径和imu数据读进来。然后初始化SLAM对象，初始化所有线程并准备处理frame。假设IMU测量先开始，先找到第一张图片时间戳对应的IMU数据。设图片的数量为n，则下面的内容循环n次，也就是每读取一张图片，就读取与前一张图片中间的所有IMU数据（如果是第一张图片，ni==0，不会读取）。然后执行SLAM.TrackMonocular()函数。
TrackMonocular函数中，主要包括GrabImuData和GrabImageMonocular。
GrabImuData就只是把之前读取的IMU数据放到mlQueueImuData队列中，以供预积分等处理。
GrabImageMonocular函数首先处理图片颜色格式，创建当前帧mCurrentFrame。并执行
Track函数。接下来所有内容都在这个函数中完成。
首先把上一个关键帧的imuBias设置到当前帧，然后预积分。如果没有完成初始化，执行初始化函数MonocularInitialization（单目imu情况，双目imu和rgbd执行StereoInitialization）。
由于现阶段我只关心初始化的问题，后面暂且不看，直接进入初始化函数。
以单目imu为例，MonocularInitialization中：
首先找一个关键点很多（大于100）的图片作为参考帧，用这个帧作为初始帧，并用这个帧创建初始化器Initializer。如果使用惯导，则新建一个预积分对象Preintegrated，用bias和calib进行初始化，主要包括预积分变量的创建和初始化。

Preintegrated::Preintegrated(const Bias &b_, const Calib &calib)
{
    Nga = calib.Cov.clone();
    NgaWalk = calib.CovWalk.clone();
    Initialize(b_);
}

如果已经有了初始化器，用当前帧和初始帧进行orb特征点的匹配，执行ReconstructWithTwoViews函数得到两帧之间的变换关系Tcw，并设置在当前帧中。这个过程目前也不是重点，继续看后面的CreateInitialMapMonocular函数。
CreateInitialMapMonocular函数中，先以初始帧和当前帧作为两个关键帧。注意，关键帧Keyframe类中有很多数据，其中包括了预积分对象的指针Preintegrated和imu的Calib。把初始帧的预积分对象指针指向null，因为初始帧没有前一帧，没有预积分数据。然后分别为两个关键帧计算BoW，并把这两个关键帧加到地图中去。然后做了一步全局BA，后面有需要再展开。

// Bundle Adjustment
    Verbose::PrintMess("New Map created with " + to_string(mpAtlas->MapPointsInMap()) + " points", Verbose::VERBOSITY_QUIET);
    Optimizer::GlobalBundleAdjustemnt(mpAtlas->GetCurrentMap(),20);

参考：
https://www.cnblogs.com/CV-life/p/10286037.html
http://zhaoxuhui.top/blog/2018/04/10/g2o&bundle_adjustment.html#2g2o%E5%BA%93%E7%AE%80%E4%BB%8B%E4%B8%8E%E7%BC%96%E8%AF%91%E5%AE%89%E8%A3%85
https://blog.csdn.net/m0_37874102/article/details/114291951?spm=1001.2014.3001.5501
g2o的编程是从底层到顶层的搭建：

1、定义顶点
一般来说，我们使用图优化的情况各式各样，需要自行定义顶点，继承g2o::BaseVertex类，例如orbslam3中的位姿顶点：

// Optimizable parameters are IMU pose
class VertexPose : public g2o::BaseVertex<6,ImuCamPose>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    VertexPose(){}
    VertexPose(KeyFrame* pKF){
        setEstimate(ImuCamPose(pKF));
    }
    VertexPose(Frame* pF){
        setEstimate(ImuCamPose(pF));
    }


    virtual bool read(std::istream& is);
    virtual bool write(std::ostream& os) const;

    virtual void setToOriginImpl() {
        }

    virtual void oplusImpl(const double* update_){
        _estimate.Update(update_);
        updateCache();
    }
};

除了定义了该顶点的构造函数之外，还需要重写基类的虚函数，read，write，setToOriginImpl，和oplusImpl。
read和write用于读写操作，一般不做操作。
setToOriginImpl用于初始化顶点，给顶点一个初值。
oplusImpl最关键，用于更新顶点，比如上面的例子中，使用Update进行更新，这个函数在优化变量ImuCamPose中有定义：

void ImuCamPose::Update(const double *pu)
{
    Eigen::Vector3d ur, ut;
    ur << pu[0], pu[1], pu[2];
    ut << pu[3], pu[4], pu[5];

    // Update body pose
    twb += Rwb*ut;
    Rwb = Rwb*ExpSO3(ur);

    // Normalize rotation after 5 updates
    its++;
    if(its>=3)
    {
        NormalizeRotation(Rwb);
        its=0;
    }

    // Update camera poses
    const Eigen::Matrix3d Rbw = Rwb.transpose();
    const Eigen::Vector3d tbw = -Rbw*twb;

    for(int i=0; i<pCamera.size(); i++)
    {
        Rcw[i] = Rcb[i]*Rbw;
        tcw[i] = Rcb[i]*tbw+tcb[i];
    }

}