VINS-FUSION 研究日志 （3）进入 estimator.cpp Part A

继续碎碎念梳理VINS-FUSION
上一篇博文梳理了整个程序的入口，其实只是做了传感器参数的读取与配置，传感器测量数据的订阅 两件事情。

estimator.cpp 中有一个持续运行的线程，是在 void Estimator::setParameter() 中最后一行代码启动的processThread = std::thread(&Estimator::processMeasurements, this);
而 void Estimator::setParameter() 是在 rosNodeTest.cpp 的main函数中执行的

与 estimator.cpp 主要数据联系在于

void Estimator::inputIMU(double t, const Vector3d &linearAcceleration, const Vector3d &angularVelocity)
void Estimator::inputImage(double t, const cv::Mat &_img, const cv::Mat &_img1)

inputIMU 中把数据存入下面的队列中，然后简单的积分一下，把最新的位姿信息发布出去

    queue<pair<double, Eigen::Vector3d>> accBuf;
    queue<pair<double, Eigen::Vector3d>> gyrBuf;

inputImage 函数进入后，先对 inputImageCnt 自增 （inputImageCnt 在构造函数中被初始化为0），然后定义一个map数据 map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> featureFrame;
进而执行 featureFrame = featureTracker.trackImage(t, _img, _img1);
并将 featureFrame 存放进 featureBuf 中； (inputImageCnt % 2 == 0) 意思是每两帧存放一次
其中 featureBuf 也是一个队列：queue<pair<double, map<int, vector<pair<int, Eigen::Matrix<double, 7, 1> > > > > > featureBuf;

FeatureTracker::trackImage(double _cur_time, const cv::Mat &_img, const cv::Mat &_img1) 是一个很长的函数
前六行代码做了基本的赋值内容

    TicToc t_r; // 用于时间统计
    cur_time = _cur_time; // 当前时间记录
    cur_img = _img; // 左目图像
    row = cur_img.rows; // 图像尺寸：高
    col = cur_img.cols; // 图像尺寸：宽
    cv::Mat rightImg = _img1; // 右目图像

cur_pts.clear(); // 清空左目的特征点，其定义为：vector<cv::Point2f>

然后判断上一次留下来的特征点的个数：

if (prev_pts.size() > 0)

如果个数大于0，则对左目图像进行光流特征追踪 （对上一次留下来的特征点进行跟踪）
这里有一个关于hasPrediction的判断，似乎程序中没有使用到这个功能，故将所有关于这个变量的内容都删除
然后进行光流追踪，FLOW_BACK 在参数读取的时候已经将其设置为1，所以在后面的特征点追踪时，也要进行一次反向筛查
同时根据 判据 (status[i] && reverse_status[i] && distance(prev_pts[i], reverse_pts[i]) <= 0.5) 进行 特征点的筛选

track_cnt 表示 光流追踪成功,特征点被成功跟踪的次数就加1

    for (auto &n : track_cnt)
        n++;

setMask() 函数是设置一个掩膜，避免特征点的稠密化

然后重新提取特征点，对特征点的数目进行补充
下面对 mask 的判断在程序中没有起到实质性的作用，故删除

            if(mask.empty())
                cout << "mask is empty " << endl;
            if (mask.type() != CV_8UC1)
                cout << "mask type wrong " << endl;

使用 cv::goodFeaturesToTrack(cur_img, n_pts, MAX_CNT - cur_pts.size(), 0.01, MIN_DIST, mask); 提取新的特征点，注意mask的作用

addPoints() 函数中，遍历新提取的特征点，并将其将入的 cur_pts 中，赋予 ids 编号，由于是新提取到的特征点，所以 track_cnt 是1

对左目特征点进行畸变校正

    cur_un_pts = undistortedPts(cur_pts, m_camera[0]);

计算左目特征点的运动速度

pts_velocity = ptsVelocity(ids, cur_un_pts, cur_un_pts_map, prev_un_pts_map);

到此，左目的工作做完了，现在开始右目的工作

首先判断右目是否有图像，stereo_cam 这个变量有点多余，我们将其彻底删除
右目的工作其实是利用光流，追踪左目的特征点

接下来的 drawTrack 函数是一个封装的绘图函数，删除关于右图是否为空的判断
并将其转化为3通道RGB，方便后面在图上绘制彩色的标记，分别绘制左右两幅图片的特征点

next, 将当前数据赋值给 prev 的变量

    prev_img = cur_img;
    prev_pts = cur_pts;
    prev_un_pts = cur_un_pts;
    prev_un_pts_map = cur_un_pts_map;
    prev_time = cur_time;

给 prevLeftPtsMap 变量赋值

定义一个变量 map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> featureFrame;，然后对其进行赋值
分别对 左目 和 右目 的特征数据进行赋值

featureFrame 是一个 map 数据，赋值完毕之后便将其作为函数返回值返回

estimator.cpp 中发现一个冗余的函数，这里就直接删除了

void Estimator::inputFeature(double t, const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &featureFrame)

现在进入 void Estimator::processMeasurements() 函数
注意回忆 这个线程是在 void Estimator::setParameter() 中开启的
processMeasurements 函数中是一个 大的 while 循环
条件
while(1) 循环中，feature 是一个 pair, pair 中 是 (时间，特征ID，相机ID，7维特征向量)
然后准备存放acc 和 gyr 的vector空间，vector中的每个元素都绑定了时间戳

vector<pair<double, Eigen::Vector3d>> accVector, gyrVector;

首先判断 if(!featureBuf.empty()) 是否为空，如果不为空，则将其中最老的feature挑出来，并将其时间戳赋值给 curTime

之后，通过 一个 while(1) 实现对 imu 数据的等待：
while(1) 中的 if 判断条件：((!USE_IMU || IMUAvailable(feature.first + td)))
第一个条件：!USE_IMU USE_IMU需要设定为true，才有意义；如果USE_IMU是false，则!USE_IMU为true，那么整个条件都是true，接下来会直接进入 break 语句，跳过IMU的等待（因为IMU压根没有启用）
第二个条件：IMUAvailable(feature.first + td)) 这个函数的参数是 最旧特征的时间戳，函数的原型很短，意思是首先 accBuf 不能为空，且 最旧特征的时间戳必须小于等于最新的IMU的时间戳（这里用acc的信息代替了IMU的信息）

bool Estimator::IMUAvailable(double t)
{
    if(!accBuf.empty() && t <= accBuf.back().first)
        return true;
    else
        return false;
}

满足了上面两个条件，此时则不需要等待IMU了；否则需要继续等待IMU数据的更新，直到 最旧特征的时间戳小于等于最新的IMU的时间戳

接下来的函数是 getIMUInterval

if(USE_IMU)
                getIMUInterval(prevTime, curTime, accVector, gyrVector);

这个函数的参数中 prevTime 是上一次最旧特征的时间，curTime 是当前最旧特征的时间，accVector 和 gyrVector 是空的。函数内部的逻辑梳理：

bool Estimator::getIMUInterval(double t0, double t1, vector<pair<double, Eigen::Vector3d>> &accVector, 
                                vector<pair<double, Eigen::Vector3d>> &gyrVector)
{
    if(accBuf.empty())
    {
        printf("not receive imu\n");
        return false;
    }
    //printf("get imu from %f %f\n", t0, t1);
    //printf("imu fornt time %f   imu end time %f\n", accBuf.front().first, accBuf.back().first);
    if(t1 <= accBuf.back().first)
    {
        while (accBuf.front().first <= t0)
        {
            accBuf.pop();
            gyrBuf.pop();
        }
        while (accBuf.front().first < t1)
        {
            accVector.push_back(accBuf.front());
            accBuf.pop();
            gyrVector.push_back(gyrBuf.front());
            gyrBuf.pop();
        }
        accVector.push_back(accBuf.front());
        gyrVector.push_back(gyrBuf.front());
    }
    else
    {
        printf("wait for imu\n");
        return false;
    }
    return true;
}

函数原型如上，首先判断 accBuf 中是否有数据，这个判断感觉是多余的，因为如果 accBuf 是空的，会在上一步中一直处于等待过程，因此考虑将其删除掉。
然后是一个时间判断：if(t1 <= accBuf.back().first) ，意思是 当前特征的时间t1（当前最旧的特征）小于等于最新的IMU数据时间戳，才执行下一步
整个流程如下图所示，椭圆代表IMU，方框代表图像帧，小于t0的IMU（黄色）需要被直接删除
t0与t1之间的IMU需要被保留，并填充进 accVector 和 gyrVector 中

上面执行完之后，执行featureBuf.pop();，相当于是把 t0时刻的 feature 给删掉了

initFirstIMUPose(accVector); 根据加速度计测量值计算 姿态角
然后计算IMU之间的时间差 dt
processIMU 是对 IMU 数据进行处理，函数原型是

void processIMU(double t, double dt, const Vector3d &linear_acceleration, const Vector3d &angular_velocity);

但是似乎函数的第一个参数并没有用到，所以我将第一个参数删除了

首先 判断 是否是第一个 IMU 数据，如果是第一个数据，则 给 acc_0 和 gyr_0 赋值
然后 判断 pre_integrations[frame_count] 是否存在，如果不存在，则新建一个
IntegrationBase 的构造函数的成员列表为：{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};，其实这个构造函数只是对 IMU 的测量噪声进行了数学描述

接着对 frame_count 的计数进行了判断，如果是第一个（frame_count == 0）,则跳过
否则，对使用 IMU 的数据对 pre_integrations 进行赋值操作
(疑问：frame_count = 0时的预积分没有计算？？)
pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity); 这句话是对IMU的数据进行了一个预积分

tmp_pre_integration->push_back(dt, linear_acceleration, angular_velocity); // 这个 要留到后面再回头看
同时，将 dt 和 IMU 的测量值存放进 下面的 全局 buf 中

dt_buf[frame_count].push_back(dt);
linear_acceleration_buf[frame_count].push_back(linear_acceleration);
angular_velocity_buf[frame_count].push_back(angular_velocity);

紧接着就是 时间， 加速度， 角速度 向量的赋值，然后计算了下
Rs[j], Ps[j], Vs[j]; // 这个也得 留到后面再回头看

最后，更新了一下 acc_0 和 gyr_0 的值，方便下一次进行中值积分

现在再回到 processMeasurements 函数中，接着看 processImage 函数，这个函数也很长
void Estimator::processImage(const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, const double header)
函数的参数是两个，第一个是 map 数据类型，第二个是时间
进入函数，首先判断视差，根据视差判断 边缘化 哪一个
边缘化最老帧，还是 边缘化 次新帧，并设置标志位： marginalization_flag

然后对 Headers 向量进行赋值，其实就是把时间存在里 Headers 里面
实例化 ImageFrame imageframe(image, header); 对象，根据 ImageFrame 的构造函数可以看出，默认是非关键帧
把所有实例化的 imageframe 放进 all_image_frame 中，all_image_frame 是一个 map
tmp_pre_integration 是一个IntegrationBase类型的指针，然后使用 acc_0, gyr_0 对其进行重新初始化
imageframe.pre_integration = tmp_pre_integration; 实际上只是把指针赋值给指针
注意：在 processIMU 中 ，当 frame_count = 0时，是不会对tmp_pre_integration进行操作的，因此此时 tmp_pre_integration 还没有被初始化

然后判断是否需要进行外参标定，外参标定先跳过，默认为 当前的 外参已经足够好

然后判断当前求解状态是否为 solver_flag == INITIAL，如果是则进行初始化操作
如果不是，则进行基于滑动窗口的优化操作

初始化操作包括3个：
(1) 单目+IMU
(2) 双目+IMU
(3) 单目

本次主要研究 双目 + IMU 的

if(STEREO && USE_IMU)
{
     f_manager.initFramePoseByPnP(frame_count, Ps, Rs, tic, ric);
     f_manager.triangulate(frame_count, Ps, Rs, tic, ric);
     if (frame_count == WINDOW_SIZE)
     {
         map<double, ImageFrame>::iterator frame_it;
         int i = 0;
         for (frame_it = all_image_frame.begin(); frame_it != all_image_frame.end(); frame_it++)
         {
             frame_it->second.R = Rs[i];
             frame_it->second.T = Ps[i];
             i++;
         }
         solveGyroscopeBias(all_image_frame, Bgs);
         for (int i = 0; i <= WINDOW_SIZE; i++)
         {
             pre_integrations[i]->repropagate(Vector3d::Zero(), Bgs[i]);
         }
         solver_flag = NON_LINEAR;
         optimization();
         slideWindow();
         ROS_INFO("Initialization finish!");
     }
}

进入之后，先执行两个函数

f_manager.initFramePoseByPnP(frame_count, Ps, Rs, tic, ric);
f_manager.triangulate(frame_count, Ps, Rs, tic, ric);  

（发现第二个函数的第一个参数 frame_count 没有在函数中使用到，所以将该参数彻底删除）
首先研究 initFramePoseByPnP 函数
该程序只有在滑动窗口中有图像才进入，对应 if(frameCnt > 0)
首先定义了两个 变量 pts2D, pts3D; 分别用于存放世界坐标系下的3D点和其对应的相机归一化坐标系下的2D点；
然后遍历每个特征，判断每个特征点的深度是否为正 （第一次进入的话肯定都是负的，所以需要后面的 triangulate 函数进行三角化）

整个过程就是图像帧一帧一帧的进来，会对周围特征点进行观测,
VINS的策略是当滑动窗口不满时，一直进来图像，提取特征点，三角化，这些帧都是看作关键帧，直到满足滑窗帧数
下面对函数内部的 for 循环进行解读：

frameCnt：当前滑动窗口中的帧数；
it_per_id.start_frame：特征点被第一次观测到时，对应滑动窗口的帧号
feature_per_frame.size() ：表示观测到某个路标点 图像的个数
feature_per_frame[0].point： 某个路标点被第一张图像观测到的信息，所谓的第一张图片不一定是滑窗内的第一张
ric[0]：相机参考坐标系的点 到 对应的bk坐标系上点的转换 矩阵- Rbk_ck, 这个值在在线调节外参的时候就定了，不再变了！
Rb0_c0 = Rb1_c1 = Rb2_c2 ... Rbn_cn
tic[0]:  是两者的平移

int index = frameCnt - it_per_id.start_frame;
帮助理解的4个假设：
假设1 ： frameCnt = 1时， it_per_id.start_frame 也为1，则此时 index 为 0，
判断 it_per_id.feature_per_frame.size() >= index + 1，也就是说：观测到这个路标点图像的个数 应当 大于等于 1

假设2 ： frameCnt = 3时， it_per_id.start_frame 也为1，则此时 index 为 2，
判断 it_per_id.feature_per_frame.size() >= index + 1，也就是说：观测到这个路标点图像的个数 应当 大于等于 3

假设3 ： frameCnt = 3时， it_per_id.start_frame 也为2，则此时 index 为 1，
判断 it_per_id.feature_per_frame.size() >= index + 1，也就是说：观测到这个路标点图像的个数 应当 大于等于 2

假设4 ： frameCnt = 5时， it_per_id.start_frame 也为2，则此时 index 为 3，
判断 it_per_id.feature_per_frame.size() >= index + 1，也就是说：观测到这个路标点图像的个数 应当 大于等于 4

然后把符合条件的特征遍历一遍，分别将其转换为 世界坐标系中 和 相机坐标系。（转换前为图像坐标系）
最后执行一次 PnP 算法

然后看 triangulate 函数

triangulate(int frameCnt, Vector3d Ps[], Matrix3d Rs[], Vector3d tic[], Matrix3d ric[])

这个函数也非常长，逐步分析如下：
首先对所有特征进行遍历
每次遍历的时候，先判断 深度值是否正常
当深度值小于0的时候，才进行三角化；如果具有正常的深度值，不需要三角化，则直接跳过，对下一个特征进行处理

进入函数后，判断 是否是双目
如果是双目的话，则开始进行特征点三角化处理
分别计算左右目相机的位姿，然后根据左右目共同看到的特征点的坐标，计算其三维坐标，从而得到其深度

如果不是双目的话，则按照 单目处理，这里处理的跟上面基本类似，但是中间注意有 imu_i++，意思是根据相邻两帧的位姿进行三角化

其中有个关键函数 triangulatePoint ，后面有空再对其进行解读。

之后的程序好像没有啥用，直接删除了，最后删减后的 函数给出如下：

void FeatureManager::triangulate(Vector3d Ps[], Matrix3d Rs[], Vector3d tic[], Matrix3d ric[])
{
    for (auto &it_per_id : feature)
    {
        // 意思是 当深度值小于0的时候，才进行三角化；如果具有正常的深度值，则直接跳过
        if (it_per_id.estimated_depth > 0)
            continue;

        if(STEREO && it_per_id.feature_per_frame[0].is_stereo)
        {
            int imu_i = it_per_id.start_frame;
            Eigen::Matrix<double, 3, 4> leftPose;
            Eigen::Vector3d t0 = Ps[imu_i] + Rs[imu_i] * tic[0];
            Eigen::Matrix3d R0 = Rs[imu_i] * ric[0];
            leftPose.leftCols<3>() = R0.transpose();
            leftPose.rightCols<1>() = -R0.transpose() * t0;
            //cout << "left pose " << leftPose << endl;

            Eigen::Matrix<double, 3, 4> rightPose;
            Eigen::Vector3d t1 = Ps[imu_i] + Rs[imu_i] * tic[1];
            Eigen::Matrix3d R1 = Rs[imu_i] * ric[1];
            rightPose.leftCols<3>() = R1.transpose();
            rightPose.rightCols<1>() = -R1.transpose() * t1;
            //cout << "right pose " << rightPose << endl;

            Eigen::Vector2d point0, point1;
            Eigen::Vector3d point3d;
            point0 = it_per_id.feature_per_frame[0].point.head(2);
            point1 = it_per_id.feature_per_frame[0].pointRight.head(2);
            //cout << "point0 " << point0.transpose() << endl;
            //cout << "point1 " << point1.transpose() << endl;

            triangulatePoint(leftPose, rightPose, point0, point1, point3d);
            Eigen::Vector3d localPoint;
            localPoint = leftPose.leftCols<3>() * point3d + leftPose.rightCols<1>();
            double depth = localPoint.z();
            if (depth > 0)
                it_per_id.estimated_depth = depth;
            else
                it_per_id.estimated_depth = INIT_DEPTH;
            /*
            Vector3d ptsGt = pts_gt[it_per_id.feature_id];
            printf("stereo %d pts: %f %f %f gt: %f %f %f \n",it_per_id.feature_id, point3d.x(), point3d.y(), point3d.z(),
                                                            ptsGt.x(), ptsGt.y(), ptsGt.z());
            */
            continue;
        }
        // else if(it_per_id.feature_per_frame.size() > 1)
        else if(it_per_id.feature_per_frame.size() >= 4)
        {
            int imu_i = it_per_id.start_frame;
            Eigen::Matrix<double, 3, 4> leftPose;
            Eigen::Vector3d t0 = Ps[imu_i] + Rs[imu_i] * tic[0];
            Eigen::Matrix3d R0 = Rs[imu_i] * ric[0];
            leftPose.leftCols<3>() = R0.transpose();
            leftPose.rightCols<1>() = -R0.transpose() * t0;

            imu_i++;
            Eigen::Matrix<double, 3, 4> rightPose;
            Eigen::Vector3d t1 = Ps[imu_i] + Rs[imu_i] * tic[0];
            Eigen::Matrix3d R1 = Rs[imu_i] * ric[0];
            rightPose.leftCols<3>() = R1.transpose();
            rightPose.rightCols<1>() = -R1.transpose() * t1;

            Eigen::Vector2d point0, point1;
            Eigen::Vector3d point3d;
            point0 = it_per_id.feature_per_frame[0].point.head(2);
            point1 = it_per_id.feature_per_frame[1].point.head(2);
            triangulatePoint(leftPose, rightPose, point0, point1, point3d);
            Eigen::Vector3d localPoint;
            localPoint = leftPose.leftCols<3>() * point3d + leftPose.rightCols<1>();
            double depth = localPoint.z();
            if (depth > 0)
                it_per_id.estimated_depth = depth;
            else
                it_per_id.estimated_depth = INIT_DEPTH;
            /*
            Vector3d ptsGt = pts_gt[it_per_id.feature_id];
            printf("motion  %d pts: %f %f %f gt: %f %f %f \n",it_per_id.feature_id, point3d.x(), point3d.y(), point3d.z(),
                                                            ptsGt.x(), ptsGt.y(), ptsGt.z());
            */
            continue;
        }
    }
}