[从零写VIO|第七节]——VINS-Mono代码精简版代码详解——初始化2视觉初始化(内容|代码)

接上一篇——外参标定的代码和内容的讲解，计算出旋转外参数$q_{bc}$(对应代码中的ric)后，开始其他参数的初始化；
视觉初始化的过程是至关重要的，如果在刚开始不能给出很好的位姿态估计，那么也就不能对IMU的参数进行精确的标定。当然，当相机在弱纹理或者高动态场景下工作时，预期的SLAM算法能够根据IMU的数据补偿视觉不确定性带来的精度损失。

我们利用SfM确定各个pose和特征点的相对于$c_0$(对应代码中的$l$帧)帧的位置关系。如下图所示，类似于基于图像的三维重建，可以用三角化和PnP把这一串的$c_k$帧相对于基础帧的位姿和特征点位置确定下来(特征点是伪深度)，在加上外参数$q_{bc}和p_{bc}$，一系列$b_k$帧的位姿(相对于$c_0$帧)也确定下来。注意，这里把$c_0$帧作为基础帧，实际上，$c_0$帧旋转一下使$g_{c0}和g_w$方向一致时获得的坐标系就是vins的世界坐标系，也就是先验。

代码在estimator.cpp中的Estimator::initialStructure()。

1. 滑动窗口中的视觉SFM

以下代码将estimator.cpp中的Estimator::initialStructure()按照实现功能进行分块介绍。

1.1 通过标准差判断IMU的运动情况

这一部分的思想是通过计算滑窗内所有帧的线加速度的标准差，判断IMU是否有充分运动激励，以判断是否进行初始化。all_image_frame这个数据结构包含了滑窗内所有帧的视觉和IMU信息，它是一个hash，以时间戳为索引。

相关代码解释见代码注释，代码如下：

//1.1 通过滑窗内所有帧的线加速度的标准差判断IMU的运动情况 check imu observibility    
// all_image_frame：包含滑窗内所有帧的视觉和IMU信息，它是一个hash，以时间戳为索引。    
{        
    map<double, ImageFrame>::iterator frame_it;        
    Vector3d sum_g;        
    for (frame_it = all_image_frame.begin(), frame_it++; frame_it != all_image_frame.end(); frame_it++)        
    {            
        double dt = frame_it->second.pre_integration->sum_dt;            
        //计算每一帧图像对应的加速度            
        Vector3d tmp_g = frame_it->second.pre_integration->delta_v / dt;            //图像的加速度之累加和            
        sum_g += tmp_g;        
    }        
    Vector3d aver_g;        
    //计算平均加速度        
    aver_g = sum_g * 1.0 / ((int)all_image_frame.size() - 1);        
    double var = 0;
            
    for (frame_it = all_image_frame.begin(), frame_it++; frame_it != all_image_frame.end(); frame_it++)        
    {            
        double dt = frame_it->second.pre_integration->sum_dt;            
        Vector3d tmp_g = frame_it->second.pre_integration->delta_v / dt;            
        //加速度减去平均加速度的差值的平方的累加            
        var += (tmp_g - aver_g).transpose() * (tmp_g - aver_g);            
        //cout << "frame g " << tmp_g.transpose() << endl;        
    }        
    var = sqrt(var / ((int)all_image_frame.size() - 1));        
    //ROS_WARN("IMU variation %f!", var);        
    if (var < 0.25)        
    {            
        // ROS_INFO("IMU excitation not enouth!");            
        //return false;        
    }    
}

1.2 全局SFM——将全局feature当中的特征存储到sfm_f中

(1)初始化变量

// global sfm    
Quaterniond Q[frame_count + 1]; // 旋转    
Vector3d T[frame_count + 1]; // 平移    
map<int, Vector3d> sfm_tracked_points; // map类型，存储后面SFM重建出来的特征点的坐标    
vector<SFMFeature> sfm_f; // SFMFeature的数据结构

其中，SFMFeature的数据结构如下：

struct SFMFeature{    
    bool state; //三角化状态，是否进行了三角化    
    int id; //特征索引    
    vector<pair<int,Vector2d>> observation; //所有观测到该特征点的 图像帧ID 和 特征点在这个图像帧的归一化坐标   
    double position[3]; //位置坐标    
    double depth; // 深度
};

【补充】
f_manager.feature存储的是一个list，list feature 中的FeaturePerId表示针对每个有索引的特征对应的属性信息，其数据结构为：

class FeaturePerId
{
    public:  
        const int feature_id; //索引  
        int start_frame; //首次被观测到时，该帧的索引  
        vector<FeaturePerFrame> feature_per_frame; //所有观测到该特征的图像帧的信息，包括图像坐标、特征点的跟踪速度、空间坐标等属性
          
        int used_num; //该特征出现的次数  
        bool is_outlier; //是否是外点  
        bool is_margin;  
        double estimated_depth; //逆深度  

        int solve_flag; // 0 haven't solve yet; 1 solve succ; 2 solve fail;
  
        Vector3d gt_p;
  
        FeaturePerId(int _feature_id, int _start_frame)      : 
             feature_id(_feature_id), start_frame(_start_frame),        
             used_num(0), estimated_depth(-1.0), solve_flag(0)  
        {
          }
  
        int endFrame();};

(2) 更新sfm_f
接下来对于每个特征，从其imu_j = it_per_id.start_frame - 1被观测到的首帧(前一帧)开始对每帧的信息：观测到该feature的帧id，归一化坐标点赋值给一个SFMFeature类型的变量，然后在存储到sfm_f中，即sfm_f存储的是每个特征对应的所有的被观测数据。代码如下：

for (auto &it_per_id : f_manager.feature) // 遍历滑窗中出现的所有特征点    
 {        
     int imu_j = it_per_id.start_frame - 1; // 帧的id        
     SFMFeature tmp_feature;        
     tmp_feature.state = false; // 三角化状态        
     tmp_feature.id = it_per_id.feature_id; //特征点id        

     //遍历每一个能观察到该feature的frame        
     for (auto &it_per_frame : it_per_id.feature_per_frame)         
     {            
         imu_j++;            
         Vector3d pts_j = it_per_frame.point; //当前特征在编号为imu_j++的帧的归一化坐标            
         //把当前特征在当前帧的坐标和当前帧的编号配对            
         tmp_feature.observation.push_back(make_pair(imu_j, Eigen::Vector2d{pts_j.x(), pts_j.y()}));        
      }        
      sfm_f.push_back(tmp_feature);    
}

1.3 对帧间的相对位姿进行求解

这里指的是求解最新帧和滑动窗口中第一个满足条件的帧之间的位姿关系。eg.在滑窗(0-9)中找到第一个满足要求的帧(第l帧)，它与最新一帧(frame_count=10)有足够的共视点和平行度，并求出这两帧之间的相对位置变化关系。
（1）定义容器

//  1.3 求解最新帧和滑动窗口中第一个满足条件的帧之间的位姿关系
// (1) 定义容器    
Matrix3d relative_R;    
Vector3d relative_T;    
int l; //滑窗中满足与最新帧视差关系的那一帧的帧号

(2)两帧之间的视差判断,并得到两帧之间的相对位姿变化关系

// (2)两帧之间的视差判断,并得到两帧之间的相对位姿变化关系
if (!relativePose(relative_R, relative_T, l))    
{        
    cout << "Not enough features or parallax; Move device around" << endl;          
    return false;    
}

relativePose(relative_R, relative_T, l)函数解析如下：

具体流程：
a.计算滑窗内的每一帧(0-9)与最新一帧(10)之间的视差，直到找出第一个满足要求的帧，作为我们的第l帧；
b.计算l帧与最新一帧的相对位姿关系

具体地，对于每个特征，如果[i，WINDOW]区间被包含在其[start_frame,endFrame()]中，那么获取i和WINDOW两帧分别对应的观测点坐标，存储在corres中。

得到两帧之间的特征对应corres之后，如果其size大于20，计算其平均位移，如果具有足够的视差，那么就执行m_estimator.solveRelativeRT(corres, relative_R, relative_T)求解当前帧和最新一帧之间的相对位姿，然后把当前帧的索引赋值给变量$l$，这里的第$l$帧是从第一帧开始到滑动窗口中第一个满足与最新一帧的平均视差足够大的帧，会作为参考帧到下面的全局sfm使用。

得到的Rt，即relative_R和relative_T是最新一帧WINDOW_SIZE相对于$l$帧坐标系的位姿估计。

具体实现过程如下：

bool Estimator::relativePose(Matrix3d &relative_R, Vector3d &relative_T, int &l)
{    
    // find previous frame which contians enough correspondance and parallex with newest frame    
    //滑窗内的所有帧都和最新一帧进行视差比较    
    for (int i = 0; i < WINDOW_SIZE; i++)    
    {        
        vector<pair<Vector3d, Vector3d>> corres;        
        corres = f_manager.getCorresponding(i, WINDOW_SIZE); // 每一个特征it在两个帧i和WINDOW_SIZE中对应的point点对         
        if (corres.size() > 20) //归一化坐标point(x,y,不需要z)        
        {            
            double sum_parallax = 0;            
            double average_parallax; //计算平均视差            
            for (int j = 0; j < int(corres.size()); j++)            
            {                
                //第j个对应点在第i帧和最新一帧的(x,y)                
                Vector2d pts_0(corres[j].first(0), corres[j].first(1));                
                Vector2d pts_1(corres[j].second(0), corres[j].second(1));                

                double parallax = (pts_0 - pts_1).norm();                
                sum_parallax = sum_parallax + parallax;            
            }            
            //计算平均视差            
            average_parallax = 1.0 * sum_parallax / int(corres.size());            
            //判断是否满足初始化条件：视差>30和内点数满足要求(大于12)
            //solveRelativeRT()通过基础矩阵计算当前帧与第l帧之间的R和T,并判断内点数目是否足够
            //同时返回窗口最新一帧（当前帧）到第l帧（参考帧）的relative_R，relative_T           
            if (average_parallax * 460 > 30 && m_estimator.solveRelativeRT(corres, relative_R, relative_T))            
            {                
                l = i; // l中存放的是窗口中从最前边开始遍历和最新一帧满足视差和RT估计的第一个帧的id               
                //ROS_DEBUG("average_parallax %f choose l %d and newest frame to triangulate the whole structure", average_parallax * 460, l);                
                return true;            
            }        
        }    
    }    
    return false;
}

【补充】

1. f_manager.getCorresponding的函数在我的上一篇博客的2.1节可见，主要功能是获取i和WINDOW两帧分别对应的观测点坐标，存储在corres中。
2. solveRelativeRT为最关键函数，代码很简单，就是把对应的点传进入，然后套cv的公式，求出来R和T。相关原理可见我的另一篇博客。

bool MotionEstimator::solveRelativeRT(const vector<pair<Vector3d, Vector3d>> &corres, Matrix3d &Rotation, Vector3d &Translation)
{    
    if (corres.size() >= 15)    
    {        
        vector<cv::Point2f> ll, rr;        
        for (int i = 0; i < int(corres.size()); i++)        
        {            
            ll.push_back(cv::Point2f(corres[i].first(0), corres[i].first(1)));            
            rr.push_back(cv::Point2f(corres[i].second(0), corres[i].second(1)));        }        
            cv::Mat mask;            
            //因为这里的ll,rr是归一化坐标，所以得到的是本质矩阵        
            cv::Mat E = cv::findFundamentalMat(ll, rr, cv::FM_RANSAC, 0.3 / 460, 0.99, mask);        
            cv::Mat cameraMatrix = (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1);        
            cv::Mat rot, trans;        
            int inlier_cnt = cv::recoverPose(E, ll, rr, cameraMatrix, rot, trans, mask);        
            //cout << "inlier_cnt " << inlier_cnt << endl

            Eigen::Matrix3d R;                                     
            Eigen::Vector3d T;        
            for (int i = 0; i < 3; i++)        
            {               
                T(i) = trans.at<double>(i, 0);            
                for (int j = 0; j < 3; j++)                
                    R(i, j) = rot.at<double>(i, j);        
             }        
             Rotation = R.transpose();        
             Translation = -R.transpose() * T;        
             if(inlier_cnt > 12)            
                 return true;        
             else            
                 return false;    
     }    
     return false；
}

/** 
*  Mat cv::findFundamentalMat(  返回通过RANSAC算法求解两幅图像之间的本质矩阵E 
*      InputArray  points1,             第一幅图像点的数组 
*      InputArray  points2,            第二幅图像点的数组 
*      int     method = FM_RANSAC,     RANSAC 算法 
*      double  param1 = 3.,            点到对极线的最大距离，超过这个值的点将被舍弃 
*      double  param2 = 0.99,          矩阵正确的可信度
*      OutputArray mask = noArray()    输出在计算过程中没有被舍弃的点 
*  )  
*/  
/** 
*  int cv::recoverPose (   通过本质矩阵得到Rt，返回通过手性校验的内点个数 
*      InputArray  E,              本质矩阵 
*      InputArray  points1,        第一幅图像点的数组 
*      InputArray  points2,        第二幅图像点的数组 
*      InputArray  cameraMatrix,   相机内参 
*      OutputArray     R,          第一帧坐标系到第二帧坐标系的旋转矩阵 
*      OutputArray     t,          第一帧坐标系到第二帧坐标系的平移向量 
*      InputOutputArray    mask = noArray()  在findFundamentalMat()中没有被舍弃的点 
*  )   
*/ 

1.4 调用sfm.construct()函数进行三维重建

construct()：对窗口中每个图像帧求解sfm问题;如果SFM重建失败，将边缘化标志设置为MARGIN_OLD，并返回false，初始化失败。对窗口中每个图像帧求解sfm问题，传入了frame_count + 1，l, relative_R, relative_T, sfm_f;得到所有图像帧相对于参考帧$l$的旋转四元数Q、平移向量T和特征点坐标sfm_tracked_points。

// 1.4 对窗口中每个图像帧求解sfm问题    
GlobalSFM sfm;    
if (!sfm.construct(frame_count + 1, Q, T, l,                       
                     relative_R, relative_T,                       
                     sfm_f, sfm_tracked_points))    
{   
    // SFM重建失败        
    cout << "global SFM failed!" << endl;        
    marginalization_flag = MARGIN_OLD; // 边缘化标志设置        
    return false; // 初始化失败    
}

下面是对sfm.construct()的解读：
(1) 初始化操作
把第$l$帧作为参考坐标系，获得最新一帧在参考坐标系下的位姿

bool GlobalSFM::construct(int frame_num, Quaterniond* q, Vector3d* T, int l,              
                          const Matrix3d relative_R, const Vector3d relative_T,              
                          vector<SFMFeature> &sfm_f, map<int, Vector3d> &sfm_tracked_points)
{    
    // 初始化操作    
    feature_num = sfm_f.size();    
    //cout << "set 0 and " << l << " as known " << endl;    
    // have relative_r relative_t    
    // intial two view    
    q[l].w() = 1; // 参考帧的四元数    
    q[l].x() = 0;    
    q[l].y() = 0;    
    q[l].z() = 0;
    
    //1、这里把第l帧看作参考坐标系，    
    // 根据当前帧到第l帧的relative_R，relative_T，得到当前帧在参考坐标系下的位姿，    
    // 之后的pose[i]表示第l帧到第i帧的变换矩阵[R|T]    
    T[l].setZero();
    q[frame_num - 1] = q[l] * Quaterniond(relative_R); //frame_num-1表示当前帧    
    T[frame_num - 1] = relative_T;

(2)构造容器，存储滑窗内第$l$帧相对于其它各个帧的位姿

//rotate to cam frame    
Matrix3d c_Rotation[frame_num];    
Vector3d c_Translation[frame_num];    
Quaterniond c_Quat[frame_num];    
double c_rotation[frame_num][4];    
double c_translation[frame_num][3];    
Eigen::Matrix<double, 3, 4> Pose[frame_num];

【注意】
这两类容器为坐标系变换矩阵，存储的都是相对运动。
大写的容器对应的是$l$帧旋转到各个帧。
小写的容器是用于全局BA时使用的，也同样是$l$帧旋转到各个帧。
之所以在这两个地方要保存这种相反的旋转，是因为三角化求深度的时候需要这个相反旋转的矩阵。

(3)对于第l帧和最新一帧，它们的相对运动是已知的，可以直接放入容器

// 3. 对于第l帧和最新一帧，它们的相对运动是已知的，可以直接放入容器    
// 第l帧    
c_Quat[l] = q[l].inverse();     
c_Rotation[l] = c_Quat[l].toRotationMatrix(); //四元数转为旋转矩阵    
c_Translation[l] = -1 * (c_Rotation[l] * T[l]);    

// 将第l帧的旋转和平移量存入到Pose当中，为变换矩阵    
Pose[l].block<3, 3>(0, 0) = c_Rotation[l];    
Pose[l].block<3, 1>(0, 3) = c_Translation[l];
    
// 最新帧(当前帧)    
c_Quat[frame_num - 1] = q[frame_num - 1].inverse();    
c_Rotation[frame_num - 1] = c_Quat[frame_num - 1].toRotationMatrix();    
c_Translation[frame_num - 1] = -1 * (c_Rotation[frame_num - 1] * T[frame_num - 1]);    

// 将最新帧的旋转和平移量存入到Pose当中，为变换矩阵      
Pose[frame_num - 1].block<3, 3>(0, 0) = c_Rotation[frame_num - 1];    
Pose[frame_num - 1].block<3, 1>(0, 3) = c_Translation[frame_num - 1];

【补充】

1. block的使用

① matrix.block(i,j, p, q) : 表示返回从矩阵(i, j)开始，每行取p个元素，每列取q个元素所组成的临时新矩阵对象，原矩阵的元素不变；
② matrix.block(i, j) :可理解为一个p行q列的子矩阵，该定义表示从原矩阵中第(i, j)开始，获取一个p行q列的子矩阵， 返回该子矩阵组成的临时矩阵对象，原矩阵的元素不变；

【注意】
这里i有一个取相反旋转的操作，因为三角化的时候需要这个相反的旋转。

至此，计算出了滑动窗口中第l帧的位姿Pose[l]和最新帧的位姿Pose[frame_num-1]，下面就是三角化处理和PnP操作，整体代码如下：

//1: trangulate between l ----- frame_num - 1    
//2: solve pnp l + 1; trangulate l + 1 ------- frame_num - 1;     
for (int i = l; i < frame_num - 1 ; i++)    
{        
    // solve pnp        
    // 对于在sliding window里在第l帧之后的每一帧，分别都和前一帧用PnP求l帧的位姿        
    if (i > l)        
    {            
        Matrix3d R_initial = c_Rotation[i - 1];            
        Vector3d P_initial = c_Translation[i - 1];            
        if(!solveFrameByPnP(R_initial, P_initial, i, sfm_f))                
            return false;            
        c_Rotation[i] = R_initial;            
        c_Translation[i] = P_initial;            
        c_Quat[i] = c_Rotation[i];            

        Pose[i].block<3, 3>(0, 0) = c_Rotation[i];            
        Pose[i].block<3, 1>(0, 3) = c_Translation[i];        
    }
    // triangulate point based on the solve pnp result        
    triangulateTwoFrames(i, Pose[i], frame_num - 1, Pose[frame_num - 1], sfm_f);    
}

(4)三角化l帧和最新帧，获得他们的共视点在l帧上的空间坐标

triangulateTwoFrames(l, Pose[l], frame_num - 1, Pose[frame_num - 1], sfm_f);

此函数代码如下：

void GlobalSFM::triangulateTwoFrames(int frame0, Eigen::Matrix<double, 3, 4> &Pose0,                                      
                                     int frame1, Eigen::Matrix<double, 3, 4> &Pose1,                                     
                                     vector<SFMFeature> &sfm_f)
{    
    assert(frame0 != frame1);    
    for (int j = 0; j < feature_num; j++)    
    {        
        if (sfm_f[j].state == true) // 已进行三角化，则跳过            
            continue;        
    
        bool has_0 = false, has_1 = false;        
        Vector2d point0;        Vector2d point1;        
        //observation：对应的归一化平面观测：所有观测到该特征点的 图像帧ID 和 特征点在这个图像帧的归一化坐标        
        for (int k = 0; k < (int)sfm_f[j].observation.size(); k++)        
        {            
            if (sfm_f[j].observation[k].first == frame0)            
            {                
                point0 = sfm_f[j].observation[k].second; // 特征点j在帧0的归一化坐标                
                has_0 = true;            
            }            
            if (sfm_f[j].observation[k].first == frame1)            
            {                
                point1 = sfm_f[j].observation[k].second; // 特征点j在帧1的归一化坐标                
                has_1 = true;            
            }        
        }        
        if (has_0 && has_1)        
        {            
            Vector3d point_3d;            
            //根据他们的位姿和归一化坐标，输出在参考系l下的的空间坐标            
            triangulatePoint(Pose0, Pose1, point0, point1, point_3d);            
            sfm_f[j].state = true; // 已经进行三角化              
            sfm_f[j].position[0] = point_3d(0);            
            sfm_f[j].position[1] = point_3d(1);            
            sfm_f[j].position[2] = point_3d(2);            
            //cout << "trangulated : " << frame1 << "  3d point : "  << j << "  " << point_3d.transpose() << endl;        
        }                                
    }
}

这个函数的核心是triangulatePoint(Pose0, Pose1, point0, point1, point_3d)。

首先他把sfm_f的特征点取出来，一个个地检查看看这个特征点是不是被第2帧都观测到了，如果被观测到了，再执行三角化操作。

triangulatePoint()的代码如下：

void GlobalSFM::triangulatePoint(Eigen::Matrix<double, 3, 4> &Pose0, Eigen::Matrix<double, 3, 4> &Pose1,                        
                                 Vector2d &point0, Vector2d &point1, Vector3d &point_3d)
{    
    Matrix4d design_matrix = Matrix4d::Zero();    
    
    design_matrix.row(0) = point0[0] * Pose0.row(2) - Pose0.row(0);    
    design_matrix.row(1) = point0[1] * Pose0.row(2) - Pose0.row(1);    
    design_matrix.row(2) = point1[0] * Pose1.row(2) - Pose1.row(0);    
    design_matrix.row(3) = point1[1] * Pose1.row(2) - Pose1.row(1);    
    Vector4d triangulated_point;    
    
    triangulated_point =             
    design_matrix.jacobiSvd(Eigen::ComputeFullV).matrixV().rightCols<1>();    
    
    point_3d(0) = triangulated_point(0) / triangulated_point(3);    
    point_3d(1) = triangulated_point(1) / triangulated_point(3);    
    point_3d(2) = triangulated_point(2) / triangulated_point(3);}

【补充】
三角化求深度的方法是博客中的第二种。



【注意】

1. 由于得到矩阵的秩大于未知量的数，所以上式方程没有非零解，使用SVD求最小二乘解，最小奇异值对应的向量即为所需的解。
2. 在这里，T指的是l帧到各个帧的转换(因为所有位姿和特征点目前都是在l帧表示的)

(5)对于在sliding window里在第l帧之后的每一帧，分别都和前一帧用PnP求它的位姿，得到位姿后再和最新一帧三角化得到它们共视点的3D坐标
回顾一下整体流程的代码：

//1: trangulate between l ----- frame_num - 1    
//2: solve pnp l + 1; trangulate l + 1 ------- frame_num - 1;     
for (int i = l; i < frame_num - 1 ; i++)    
{        
    // solve pnp        
    // 对于在sliding window里在第l帧之后的每一帧，分别都和前一帧用PnP求l帧的位姿        
    if (i > l)        
    {            
        Matrix3d R_initial = c_Rotation[i - 1];            
        Vector3d P_initial = c_Translation[i - 1];                        
        if(!solveFrameByPnP(R_initial, P_initial, i, sfm_f))                
            return false;            
        c_Rotation[i] = R_initial;            
        c_Translation[i] = P_initial;            
        c_Quat[i] = c_Rotation[i];            
        Pose[i].block<3, 3>(0, 0) = c_Rotation[i];            
        Pose[i].block<3, 1>(0, 3) = c_Translation[i];        
    }
    // triangulate point based on the solve pnp result        
    //  (4)三角化l帧和最新帧，获得他们的共视点在l帧上的空间坐标        
    triangulateTwoFrames(i, Pose[i], frame_num - 1, Pose[frame_num - 1], sfm_f);    
}

pnp相关函数为solveFrameByPnP(R_initial, P_initial, i, sfm_f)：

执行PnP操作。首先对于每个特征，检查其所有的被观测信息，找到特征在当前帧上的观测信息，将该观测赋值给pts_2_vector，而其三维坐标存储在pts_3_vector。如果满足条件的特征数小于15,则认为无法进行PnP操作。

一般来讲，求位姿，2D-2D对极几何只是在第一次使用，也就是没有3D特征点坐标的时候使用，一旦有了特征点，之后都会用3D-2D的方式求位姿。然后会进入PnP求新位姿，然后三角化求新3D坐标的循环中。

solveFrameByPnP()代码逻辑可以分成4部分：

a．第一次筛选：把滑窗的所有特征点中，那些没有3D坐标(没有进行三角化)的点pass掉。

bool GlobalSFM::solveFrameByPnP(Matrix3d &R_initial, Vector3d &P_initial, int i,                                
                                vector<SFMFeature> &sfm_f)
{    
    vector<cv::Point2f> pts_2_vector; // 特征在当前帧上的观测信息存储于此    
    vector<cv::Point3f> pts_3_vector; // 特征的三维坐标存储于此    
    for (int j = 0; j < feature_num; j++)    
    {        
        // 要把待求帧i上所有特征点的归一化坐标和3D坐标(l帧坐标系上)都找出来        
        if (sfm_f[j].state != true) 
            //这个特征点没有被三角化为空间点，跳过这个点的PnP            
            continue;

b．求解在待求帧i(上一帧$l+1$)上出现过的特征的归一化坐标和在参考系l的3D空间坐标，分别放在pts_2_vector和pts_3_vector容器中。

因为是对当前帧$l$和上一帧$l+1$进行PnP，所以这些有3D坐标的特征点，不仅得在当前帧被观测到，还得在上一帧被观测到。

// b．因为是对当前帧和上一帧进行PnP，所以这些有3D坐标的特征点，不仅得在当前帧被观测到，还得在上一帧被观测到。        
Vector2d point2d;        
// observation：对应的归一化平面观测；所有观测到该特征点的 图像帧ID 和 特征点在这个图像帧的归一化坐标        
for (int k = 0; k < (int)sfm_f[j].observation.size(); k++)        
{            
    if (sfm_f[j].observation[k].first == i) // 出现在第i帧的特征点j            
    {                
        Vector2d img_pts = sfm_f[j].observation[k].second; // 特征点在图像帧i上的归一化坐标                
        cv::Point2f pts_2(img_pts(0), img_pts(1));                
        pts_2_vector.push_back(pts_2);
                        
        cv::Point3f pts_3(sfm_f[j].position[0], sfm_f[j].position[1], sfm_f[j].position[2]);                
        pts_3_vector.push_back(pts_3); //把在待求帧i上出现过的特征在参考系l的空间坐标放到容器中                
        break; //因为一个特征在帧i上只会出现一次，一旦找到了就没有必要再继续找了            
    }        
}

c.如果这些有3D坐标的特征点，并且在当前帧和上一帧都出现了，数量却少于15，那么整个初始化全部失败。因为它的是层层往上传递。

if (int(pts_2_vector.size()) < 15)    
{        
    printf("unstable features tracking, please slowly move you device!\n");        
    if (int(pts_2_vector.size()) < 10)            
        return false; // 初始化失败    
}

d．套用openCV的公式，进行PnP求解。

cv::Mat r, rvec, t, D, tmp_r; // rvec, t：输出的旋转/平移向量。使坐标点从当前坐标系旋转到第l帧
cv::eigen2cv(R_initial, tmp_r); //转换成solvePnP能处理的格式    
cv::Rodrigues(tmp_r, rvec);    
cv::eigen2cv(P_initial, t);    
cv::Mat K = (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1);    
bool pnp_succ;    
pnp_succ = cv::solvePnP(pts_3_vector, pts_2_vector, K, D, rvec, t, 1); //得到了第i帧(l之后的帧)到第l帧的旋转平移    
if(!pnp_succ)    
{        
    return false;    
}    
cv::Rodrigues(rvec, r);    
//cout << "r " << endl << r << endl;    
MatrixXd R_pnp;    
cv::cv2eigen(r, R_pnp); // 转换成原有格式    
MatrixXd T_pnp;    
cv::cv2eigen(t, T_pnp);    
R_initial = R_pnp; // 覆盖原先的旋转平移    
P_initial = T_pnp;    
return true;

【补充】

1. rvec,t是将pts_3_vector从其所在的l帧相机坐标系旋转到pts_2_vector所在的坐标系。也就是当前帧(第l帧之后的帧)在l帧相机坐标系下的位姿——>最新帧与l帧之间的位姿关系；这是pnp求解后的输出值。
2. opencv中的pnp函数解析

(6)从第l+1帧到滑窗的最后的每一帧再与第l帧进行三角化补充3D坐标

回到construct()函数，上一步求出了$l$帧后面的每一帧的位姿，也求出了它们相对于最后一帧的共视点的3D坐标，但是这是不够的，现在继续补充3D坐标，——和第$l$帧进行三角化。

//6: triangulate l-----l+1 l+2 ... frame_num -2    
for (int i = l + 1; i < frame_num - 1; i++)        
    triangulateTwoFrames(l, Pose[l], i, Pose[i], sfm_f);

(7)对于在sliding window里在第$l$帧之前的每一帧，分别都和后一帧用PnP求它的位姿，得到位姿后再和第$l$帧三角化得到它们共视点的3D坐标

$l$帧之后的帧都处理好之后，现在需要处理之前的帧。这个过程和(5)完全一样。

//7: solve pnp l-1; triangulate l-1 ----- l    
//             l-2              l-2 ----- l    
for (int i = l - 1; i >= 0; i--)    
{        
    //solve pnp        
    Matrix3d R_initial = c_Rotation[i + 1];        
    Vector3d P_initial = c_Translation[i + 1];        
    if(!solveFrameByPnP(R_initial, P_initial, i, sfm_f))            
        return false;        
    c_Rotation[i] = R_initial;        
    c_Translation[i] = P_initial;        
    c_Quat[i] = c_Rotation[i];        
    Pose[i].block<3, 3>(0, 0) = c_Rotation[i];        
    Pose[i].block<3, 1>(0, 3) = c_Translation[i];        
    //triangulate        
    triangulateTwoFrames(i, Pose[i], l, Pose[l], sfm_f);    
}

(8) 三角化其他未恢复的特征点

代码中用到的函数前文已经介绍，这里不在赘述 。到这里，我们就得到了滑动窗口中所有图像帧的位姿以及特征点的3D坐标。

//8: triangulate all other points    
for (int j = 0; j < feature_num; j++)    
{        
    if (sfm_f[j].state == true)            
        continue;        
    if ((int)sfm_f[j].observation.size() >= 2)        
    {            
        Vector2d point0, point1;            
        int frame_0 = sfm_f[j].observation[0].first;//第j个特征在滑窗第一次被观测到的帧id            
        point0 = sfm_f[j].observation[0].second;//第j个特征在滑窗第一次被观测到的帧的归一化坐标            
        int frame_1 = sfm_f[j].observation.back().first; // 最后被观测到的帧id            
        point1 = sfm_f[j].observation.back().second;            
        Vector3d point_3d;//在帧l下的空间坐标            
        triangulatePoint(Pose[frame_0], Pose[frame_1], point0, point1, point_3d);
                    
        sfm_f[j].state = true;            
        sfm_f[j].position[0] = point_3d(0);            
        sfm_f[j].position[1] = point_3d(1);            
        sfm_f[j].position[2] = point_3d(2);            
        //cout << "trangulated : " << frame_0 << " " << frame_1 << "  3d point : "  << j << "  " << point_3d.transpose() << endl;       
    }           
}

(9)采用ceres进行全局BA

a.声明problem

因为这里的四元数是4维的，但是自由度是3维的，因此需要引入LocalParameterization。

//9. full BA    
ceres::Problem problem;    
ceres::LocalParameterization* local_parameterization = new ceres::QuaternionParameterization();

【补充】

1. 对ceres::QuaternionParameterization()的认识：
   如果变量的维度大于其自由度。这会导致变量各个量之间存在约束，如果直接对这些量求导、优化，那么这就是一个有约束的优化，实现困难。对于 SLAM 问题，广泛遇到的 Manifold 是旋转，旋转仅需使用 3 个量（so(3)）即可表达，但是实际应用中因为涉及到万向锁的问题，在更高维度表达旋转。四元数就是在维度 4 表达 3 个自由度的三维空间的旋转。
   $还是比较晕，理解不了，以后回过头看...$，继续代码的分析…

b.加入待优化量：全局位姿

通过代码可以看出，仅位姿被优化，特征点的3D坐标没有被优化。

for (int i = 0; i < frame_num; i++)    
{        
    //double array for ceres        
    c_translation[i][0] = c_Translation[i].x();        
    c_translation[i][1] = c_Translation[i].y();        
    c_translation[i][2] = c_Translation[i].z();        
    c_rotation[i][0] = c_Quat[i].w();        
    c_rotation[i][1] = c_Quat[i].x();        
    c_rotation[i][2] = c_Quat[i].y();        
    c_rotation[i][3] = c_Quat[i].z();        
    problem.AddParameterBlock(c_rotation[i], 4, local_parameterization);//value,size        
    problem.AddParameterBlock(c_translation[i], 3);

c.固定先验值

因为$l$帧是参考系，最新帧的平移也是先验，如果不固定住，原本可观的量会变的不可观。

if (i == l)        
{            
    problem.SetParameterBlockConstant(c_rotation[i]);        
}
        
if (i == l || i == frame_num - 1)        
{            
    problem.SetParameterBlockConstant(c_translation[i]);        
}   

d.加入残差块——重投影误差

这部分代码还没有加损失函数，最小化重投影误差，需要2D-3D信息。

// d.加入残差块——最小化重投影误差    
for (int i = 0; i < feature_num; i++)    
{        
   if (sfm_f[i].state != true) // 没有进行三角化则跳出            
       continue;        
   for (int j = 0; j < int(sfm_f[i].observation.size()); j++)        
   {            
       int l = sfm_f[i].observation[j].first;             
       ceres::CostFunction* cost_function = ReprojectionError3D::Create(                                                  
                                            sfm_f[i].observation[j].second.x(),                                                
                                            sfm_f[i].observation[j].second.y());
            
       problem.AddResidualBlock(cost_function, NULL, c_rotation[l], c_translation[l],                                     
                                sfm_f[i].position);          
    }
}

e.shur消元求解

// e.shur消元求解    
ceres::Solver::Options options;    
options.linear_solver_type = ceres::DENSE_SCHUR;    
//options.minimizer_progress_to_stdout = true;    
options.max_solver_time_in_seconds = 0.2;    
ceres::Solver::Summary summary;    
ceres::Solve(options, &problem, &summary);    
//std::cout << summary.BriefReport() << "\n";    
if (summary.termination_type == ceres::CONVERGENCE || summary.final_cost < 5e-03)    
{        
    //cout << "vision only BA converge" << endl;    
}    
else    
{        
    //cout << "vision only BA not converge " << endl;        
    return false;    
}

【补充】
shur消元有2大作用，一个是在最小二乘中利用H矩阵稀疏的性质进行加速求解，另一个是在sliding window时求解marg掉老帧后的先验信息矩阵。这块是shur消元的第一个用法。

d．返回特征点l系下3D坐标和优化后的全局位姿

// d．返回特征点l系下3D坐标和优化后的全局位姿    
for (int i = 0; i < frame_num; i++)    
{        
    q[i].w() = c_rotation[i][0];         
    q[i].x() = c_rotation[i][1];         
    q[i].y() = c_rotation[i][2];         
    q[i].z() = c_rotation[i][3];         
    q[i] = q[i].inverse();        
    //cout << "final  q" << " i " << i <<"  " <
}    
for (int i = 0; i < frame_num; i++)    
{
    T[i] = -1 * (q[i] * Vector3d(c_translation[i][0], c_translation[i][1], c_translation[i][2]));        
    //cout << "final  t" << " i " << i <<"  " << T[i](0) <<"  "<< T[i](1) <<"  "<< T[i](2) << endl;    
}    
for (int i = 0; i < (int)sfm_f.size(); i++)    
{        
    if(sfm_f[i].state)            
        sfm_tracked_points[sfm_f[i].id] = Vector3d(sfm_f[i].position[0], sfm_f[i].position[1], sfm_f[i].position[2]);    
}    
return true;
}

优化完成后，我们想要的是各帧在帧$l$系下的位姿(也就是各帧到$l$帧的旋转平移)，所以需要inverse操作，然后把特征点在帧$l$系下的3D坐标传递出来。

至此，construct()函数全部完成!!!

1.5 给滑窗外的图像帧提供初始的RT估计，然后solvePnP进行优化

现在，我们回到Estimator::initialStructure()看一下代码流程：

// 1.4 对窗口中每个图像帧求解sfm问题    
GlobalSFM sfm;    
if (!sfm.construct(frame_count + 1, Q, T, l,                          
                   relative_R, relative_T,                       
                   sfm_f, sfm_tracked_points))    
{   
    // SFM重建失败        
    cout << "global SFM failed!" << endl;        
    marginalization_flag = MARGIN_OLD; // 边缘化标志设置        
    return false; // 初始化失败    
}
// 至此，construct()函数全部完成，对窗口中每个图像帧求解了sfm问题，得到R,T，三维坐标
//solve pnp for all frame   
map<double, ImageFrame>::iterator frame_it;    
map<int, Vector3d>::iterator it;    
frame_it = all_image_frame.begin();
//遍历所有的图像帧    
for (int i = 0; frame_it != all_image_frame.end(); frame_it++)    
{        
    // provide initial guess        
    cv::Mat r, rvec, t, D, tmp_r;
    // 对当前图像帧的操作
    ....
}
if (visualInitialAlign())        
    return true;    
else    
{        
    cout << "misalign visual structure with IMU" << endl;        
    return false;    
}

根据代码，在之前已经分析过的流程里，求出了滑窗内所有帧和对应特征点在l帧下的状态，现在要求出所有帧(也就是滑窗外)和对应特征点在l帧下的状态。

然后接下来对所有帧进行PNP操作。因为前面只得到了滑动窗口内所有关键帧的位姿，但由于并不是第一次视觉初始化就能成功，此时图像帧数目有可能会超过滑动窗口的大小(根据图像帧的视差判断是否为关键帧，然后选择滑窗的策略)，此时要对那些不被包括在滑动窗口内的图像帧位姿进行求解。

另，visualInitialAlign()用来进行视觉惯性联合优化，在下一篇博客开始介绍。

先看一下ImageFrame的数据结构：
可以看到，它不仅包括了图像信息，还包括了对应的IMU的位姿信息和IMU预积分信息。

class ImageFrame{
public:    
    ImageFrame(){};    
    ImageFrame(const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &_points, double _t) : t{_t}, is_key_frame{false}    
    {        
        points = _points;    
    };    
    map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> points;    
    double t;    
    Matrix3d R;    
    Vector3d T;    
    IntegrationBase *pre_integration;    
    bool is_key_frame;
};

a.边界判断：对于滑窗内的帧，把它们设为关键帧，并获得它们对应的IMU坐标系到l系的旋转平移

代码如下：

//  1. 滑动窗口内的操作        
// 把滑动窗口内的帧设为关键帧，并获得它们对应的IMU坐标系到l系的旋转平移        
if ((frame_it->first) == Headers[i].stamp.toSec())        
{            
    frame_it->second.is_key_frame = true; // 设置为关键帧            
    // 得到相对于l帧相机坐标系下的body坐标系的姿态
    // 各帧相对于参考帧的旋转和相机-IMU之间的旋转做乘积，就将坐标系转换到了IMU坐标系下            
    frame_it->second.R = Q[i].toRotationMatrix() * RIC[0].transpose(); //  RIC：q_bc——>q_{cl,bi}
    //各帧相对于参考帧的平移            
    frame_it->second.T = T[i];            
    i++;            
    continue;        
}
// 边界判断：如果当前帧的时间戳大于滑窗内第i帧的时间戳，那么i++
if ((frame_it->first) > Headers[i].stamp.toSec())        
{            
    i++;        
}

【补充】

1. Headers，Q和T，它们的size都是WINDOW_SIZE+1；它们存储的信息都是滑窗内的，尤其是Q和T，它们都是当前相机帧到$l$相机帧系的旋转和平移。
2. 这里得到的是这些帧第一次获得它们对应的IMU的位姿信息的位置，即bk->l帧的旋转。

frame_it->second.R = Q[i].toRotationMatrix() * RIC[0].transpose(); //  RIC：q_bc——>q_{cl,bi}            

其中，RIC是之前估计出的旋转外参数$q_{bc}$,$Q[i]$是指$q_{c_l,c_i}$，运算即可得到$q_{c_l,b_i}$，就是bk->l帧的旋转。

b.对滑窗外的所有帧，求出它们对应的IMU坐标系到l帧的旋转平移

相关代码如下：

//遍历所有的图像帧    
for (int i = 0; frame_it != all_image_frame.end(); frame_it++)    
{
     // a.边界判断：对于滑窗内的帧，把它们设为关键帧，并获得它们对应的IMU坐标系到l系的旋转平移
     ....
    // b.对滑窗外的所有帧，求出它们对应的IMU坐标系到l帧的旋转平移        
    // 这里的 Q和 T是图像帧的位姿        
    Matrix3d R_inital = (Q[i].inverse()).toRotationMatrix();        
    Vector3d P_inital = -R_inital * T[i];        
    cv::eigen2cv(R_inital, tmp_r); // 变换矩阵形式 eigen:R_inital;cv::Mat :tmp_r        
    cv::Rodrigues(tmp_r, rvec); // 罗德里格斯公式将旋转向量转换成旋转矩阵        
    cv::eigen2cv(P_inital, t);
   
    frame_it->second.is_key_frame = false;        
    vector<cv::Point3f> pts_3_vector;        
    vector<cv::Point2f> pts_2_vector;        
    for (auto &id_pts : frame_it->second.points) // 帧上的所有特征点        
    {            
         int feature_id = id_pts.first; // 特征点id            
         for (auto &i_p : id_pts.second)            
         {                
             // 找到sfm中id_pts对应的特征点                
             it = sfm_tracked_points.find(feature_id); // sfm_tracked_points存储着SFM重建出来的特征点的坐标                
             if (it != sfm_tracked_points.end())                
             {                    
                 Vector3d world_pts = it->second; // id_pts对应的3d点坐标                    
                 cv::Point3f pts_3(world_pts(0), world_pts(1), world_pts(2));                    
                 pts_3_vector.push_back(pts_3);                    
                 Vector2d img_pts = i_p.second.head<2>();                    
                 cv::Point2f pts_2(img_pts(0), img_pts(1)); // id_pts对应的归一化坐标                    
                 pts_2_vector.push_back(pts_2);                
             }            
         }        
    }        
    cv::Mat K = (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1);          
    //保证特征点数大于5        
    if (pts_3_vector.size() < 6)        
    {            
         cout << "Not enough points for solve pnp pts_3_vector size " << pts_3_vector.size() << endl;            
         return false;        
    }        
    if (!cv::solvePnP(pts_3_vector, pts_2_vector, K, D, rvec, t, 1)) // 输出帧间的变换rvec, t        
    {            
        cout << " solve pnp fail!" << endl;            
        return false;        
    }        
    cv::Rodrigues(rvec, r);        
    MatrixXd R_pnp, tmp_R_pnp;        
    cv::cv2eigen(r, tmp_R_pnp);        
    R_pnp = tmp_R_pnp.transpose();        
    MatrixXd T_pnp;        
    cv::cv2eigen(t, T_pnp);        
    T_pnp = R_pnp * (-T_pnp);        
    // !!! 传入的是bk->l的旋转平移        
    frame_it->second.R = R_pnp * RIC[0].transpose();        
    frame_it->second.T = T_pnp;    
    }

这部分和前面的思路一样，最关键的是最后两行，表明我们传入的是bk->l的旋转平移。

【补充】

1. OpenCV矩阵与Eigen矩阵相互转换
   在opencv中矩阵都是使用cv::Mat表示，但是在pcl中是使用Eigen::Matrix4d表示的，因此在矩阵计算时需要转换。

#include 
#include 

void eigen2cv(const Eigen::Matrix<_Tp, _rows, _cols, _options, _maxRows, _maxCols>& src, Mat& dst)
void cv2eigen(const Mat& src, Eigen::Matrix<_Tp, _rows, _cols, _options, _maxRows, _maxCols>& dst)

2. cv::Rodrigues()函数
   罗德里格斯公式将三维旋转向量转化为三维旋转空间矩阵

void Rodrigues( const CvMat* src，CvMat* dst，CvMat* jacobian=0 );

参数说明：
src:输入的旋转矩阵/向量，表示其旋转的角度，用向量长度表示。
dst:输出的旋转矩阵/向量

1.6. 视觉惯性联合优化visualInitialAlign()

visualInitialAlign()这部分内容也比较多，下边会先对视觉IMU对齐进行解析。
基本上，初始化的理论部分都在VisualIMUAlignment函数里，我们将在这部分实现视觉惯性对齐求解。

陀螺仪偏置，尺度，重力加速度和速度的相关计算也在这个函数中。

后面的内容在我的下一篇博客中介绍——指路