VINS-FUSION代码超详细注释（VIO部分）/VIO入门(2)

0 前情回顾

上一个博客讲到了主程序rosNodeTest.cpp。在程序最后，会进入sync_process线程进行处理。本篇博客接着进行讲解。

本次工作

我首先一步步的把代码全部注释了，十分的详细，对于C++和OpenCV的一些操作也进行了详细的注释，对于刚入门的同学应该还是有帮助的。之后我将代码开源，并写了相应的博客进行讲解。

开源程序:

https://github.com/kuankuan-yue/VINS-FUSION-leanrning.git

相应博客：

VINS-FUSION代码超详细注释（VIO部分）/VIO入门(1)
VINS-FUSION代码超详细注释（VIO部分）/VIO入门(2)
VINS-FUSION代码超详细注释（VIO部分）/VIO入门(3)
VINS-FUSION代码超详细注释（VIO部分）/VIO入门(4)

1 sync_process

本程序的作用，判断是否双目，双目的话判断时间是否同步，之后讲图像image（单目），或者image0和image1通过inputImage输入到estimator中。

inputImage

// 给Estimator输入图像
// 其实是给featureTracker.trackImage输入图像，之后返回图像特征featureFrame。填充featureBuf
// 之后执行processMeasurements
void Estimator::inputImage(double t, const cv::Mat &_img, const cv::Mat &_img1)

首先设置参数，并开启processMeasurements线程

setParameter();

然后追踪图像上的特征。trackImage，之后会进行详解,其中得到了featureFrame

if(_img1.empty())
    featureFrame = featureTracker.trackImage(t, _img);// 追踪单目
else
    featureFrame = featureTracker.trackImage(t, _img, _img1);// 追踪双目

然后，getTrackImage对特征到跟踪的图像进行一些处理。并把追踪的图片imgTrack发布出去.

   if (SHOW_TRACK)//这个应该是展示轨迹 
    {
        cv::Mat imgTrack = featureTracker.getTrackImage();
        pubTrackImage(imgTrack, t);
    }

然后，填充featureBuf
最后执行processMeasurements，之后会进行详细讲解

2、trackImage

得到featureFrame

// 对图片进行一系列操作，返回特征点featureFrame。
// 其中还包含了：图像处理、区域mask、检测特征点、计算像素速度等
map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> FeatureTracker::trackImage(double _cur_time, const cv::Mat &_img, const cv::Mat &_img1)

2.1 图像处理

可以添加图像处理的部分，比如直方图均衡等等方法。

   {
        cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));//createCLAHE 直方图均衡
        clahe->apply(cur_img, cur_img);
        if(!rightImg.empty())
            clahe->apply(rightImg, rightImg);
    }

2.2 hasPrediction

会对上一阵的点进行预测。但是具体是什么作用还不是很清楚

2.3 if(SHOW_TRACK)

drawTrack 画出追踪情况，就是在图像上的特征点位置出画圈圈，如果是双目的话就连线。

//在imTrack图像上画出特征点
void FeatureTracker::drawTrack(const cv::Mat &imLeft, const cv::Mat &imRight, 
                               vector<int> &curLeftIds,
                               vector<cv::Point2f> &curLeftPts, 
                               vector<cv::Point2f> &curRightPts,
                               map<int, cv::Point2f> &prevLeftPtsMap)

2.4 setMask

在已跟踪到角点的位置上，将mask对应位置上设为0,
意为在cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.01, MIN_DIST, mask);
进行操作时在该点不再重复进行角点检测，这样可以使角点分布更加均匀
具体详情见开源的注释代码。

// 把追踪到的点进行标记
// 设置遮挡部分（鱼眼相机）
// 对检测到的特征点按追踪到的次数排序
// 在mask图像中将追踪到点的地方设置为0，否则为255，目的是为了下面做特征点检测的时候可以选择没有特征点的区域进行检测。
// 在同一区域内，追踪到次数最多的点会被保留，其他的点会被删除
void FeatureTracker::setMask()

2.5 goodFeaturesToTrack

如果当前图像的特征点cur_pts数目小于规定的最大特征点数目MAX_CNT，则进行提取。
提取使用的cv::goodFeaturesToTrack。将点保存到n_pts

/* goodFeaturesToTrack
_image：8位或32位浮点型输入图像，单通道
_corners：保存检测出的角点
maxCorners：角点数目最大值，如果实际检测的角点超过此值，则只返回前maxCorners个强角点
qualityLevel：角点的品质因子
minDistance：对于初选出的角点而言，如果在其周围minDistance范围内存在其他更强角点，则将此角点删除
_mask：指定感兴趣区，如不需在整幅图上寻找角点，则用此参数指定ROI
blockSize：计算协方差矩阵时的窗口大小
useHarrisDetector：指示是否使用Harris角点检测，如不指定，则计算shi-tomasi角点
harrisK：Harris角点检测需要的k值 */
cv::goodFeaturesToTrack(cur_img, n_pts, MAX_CNT - cur_pts.size(), 0.01, MIN_DIST, mask);
// mask 这里肯定是指定感兴趣区，如不需在整幅图上寻找角点，则用此参数指定ROI

之后将n_pts保存到cur_pts之中

2.3.1 undistortedPts

将像素座标系下的座标，转换为归一化相机座标系下的座标 即un_pts为归一化相机座标系下的座标。

// 将像素座标系下的座标，转换为归一化相机座标系下的座标 即un_pts为归一化相机座标系下的座标。
vector<cv::Point2f> FeatureTracker::undistortedPts(vector<cv::Point2f> &pts, camodocal::CameraPtr cam)

cam->liftProjective(a, b);
这个函数是对鱼眼相机模型的标定及去畸变过程

/**
 * \brief Lifts a point from the image plane to its projective ray
 * \param p image coordinates
 * \param P coordinates of the projective ray
 * 这个函数是对鱼眼相机模型的标定及去畸变过程
 */
void
PinholeCamera::liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const

2.3.2 ptsVelocity

计算当前帧相对于前一帧 特征点沿x,y方向的像素移动速度

// 其为当前帧相对于前一帧 特征点沿x,y方向的像素移动速度
vector<cv::Point2f> FeatureTracker::ptsVelocity(vector<int> &ids, vector<cv::Point2f> &pts, 
    map<int, cv::Point2f> &cur_id_pts, map<int, cv::Point2f> &prev_id_pts)

2.4 如果是双目

如果是双目相机，那么在右目上追踪左目的特征点。使用的函数是calcOpticalFlowPyrLK

/*光流跟踪是在左右两幅图像之间进行cur left ---- cur right
prevImg	第一幅8位输入图像 或 由buildOpticalFlowPyramid()构造的金字塔。
nextImg	第二幅与preImg大小和类型相同的输入图像或金字塔。
prevPts	光流法需要找到的二维点的vector。点坐标必须是单精度浮点数。
nextPts	可以作为输入，也可以作为输出。包含输入特征在第二幅图像中计算出的新位置的二维点（单精度浮点坐标）的输出vector。当使用OPTFLOW_USE_INITIAL_FLOW 标志时，nextPts的vector必须与input的大小相同。
status	输出状态vector(类型：unsigned chars)。如果找到了对应特征的流，则将向量的每个元素设置为1；否则，置0。
err	误差输出vector。vector的每个元素被设置为对应特征的误差，可以在flags参数中设置误差度量的类型；如果没有找到流，则未定义误差（使用status参数来查找此类情况）。
winSize	每级金字塔的搜索窗口大小。
maxLevel	基于最大金字塔层次数。如果设置为0，则不使用金字塔（单级）；如果设置为1，则使用两个级别，等等。如果金字塔被传递到input，那么算法使用的级别与金字塔同级别但不大于MaxLevel。
criteria	指定迭代搜索算法的终止准则（在指定的最大迭代次数标准值（criteria.maxCount）之后，或者当搜索窗口移动小于criteria.epsilon。）
flags 操作标志，可选参数：
OPTFLOW_USE_INITIAL_FLOW：使用初始估计，存储在nextPts中；如果未设置标志，则将prevPts复制到nextPts并被视为初始估计。
OPTFLOW_LK_GET_MIN_EIGENVALS：使用最小本征值作为误差度量（见minEigThreshold描述）；如果未设置标志，则将原始周围的一小部分和移动的点之间的 L1 距离除以窗口中的像素数，作为误差度量。
minEigThreshold	
算法所计算的光流方程的2x2标准矩阵的最小本征值（该矩阵称为[Bouguet00]中的空间梯度矩阵）÷ 窗口中的像素数。如果该值小于MinEigThreshold，则过滤掉相应的特征，相应的流也不进行处理。因此可以移除不好的点并提升性能。 */
cv::calcOpticalFlowPyrLK(cur_img, rightImg, cur_pts, cur_right_pts, status, err, cv::Size(21, 21), 3);

if(FLOW_BACK)
如果这个打开，就想前边的左右目图像的位置换一下，在进行一次特征跟踪，目的是反向跟踪，得到左右目都匹配到的点

 cv::calcOpticalFlowPyrLK(rightImg, cur_img, cur_right_pts, reverseLeftPts, statusRightLeft, err, cv::Size(21, 21), 3);

之后undistortedPts ptsVelocity

2.6 制作featureFrame

map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> featureFrame;
// 数据格式为feature_id camera_id（0或1） xyz_uv_velocity（空间坐标，像素坐标和像素速度）

其中，camera_id = 0为左目上的点，camera_id = 1，为右目上的点。

3 processMeasurements

这是处理全部量测的线程,IMU的预积分,特征点的处理等等都在这里进行.

3.1 对imu的处理

3.1.1 判断IMU数据是否可用

if ((!USE_IMU  || IMUAvailable(feature.first + td)))//如果不用imu或者

其中

// 判断输入的时间t时候的imu是否可用
bool Estimator::IMUAvailable(double t)

3.1.2 获得accVector和gyrVector

对imu的时间进行判断，讲队列里的imu数据放入到accVector和gyrVector中，

// 对imu的时间进行判断，讲队列里的imu数据放入到accVector和gyrVector中，完成之后返回true
bool Estimator::getIMUInterval(double t0, double t1, vector<pair<double, Eigen::Vector3d>> &accVector, 
                                vector<pair<double, Eigen::Vector3d>> &gyrVector)

3.1.3 初始化IMU的姿态

initFirstIMUPose,其实很简单,就是求一个姿态角,然后把航向角设为0

//初始第一个imu位姿
void Estimator::initFirstIMUPose(vector<pair<double, Eigen::Vector3d>> &accVector)

3.1.4 处理IMU数据,运行processIMU

/* 对imu计算预积分
传进来的是一个imu数据 得到预积分值pre_integrations 还有一个tmp_pre_integration */
void Estimator::processIMU(double t, double dt, const Vector3d &linear_acceleration, const Vector3d &angular_velocity)

其中frame_count是值窗内的第几帧图像

下边是新建一个预积分项目u

 pre_integrations[frame_count] = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};

预积分

pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity);
        // push_back进行了重载，的时候就已经进行了预积分

其中的push_back

    void push_back(double dt, const Eigen::Vector3d &acc, const Eigen::Vector3d &gyr)
    {
        dt_buf.push_back(dt);
        acc_buf.push_back(acc);
        gyr_buf.push_back(gyr);
        propagate(dt, acc, gyr);
    }

其中的propagate

    // IMU预积分传播方程 
    // 积分计算两个关键帧之间IMU测量的变化量
    // 同时维护更新预积分的Jacobian和Covariance,计算优化时必要的参数
    void propagate(double _dt, const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1)

其中的midPointIntegration.这里边就涉及到了IMU的传播方针和协方差矩阵.雅克比矩阵等等.哪里不懂可以VIO的理论知识.
【泡泡读者来稿】VINS 论文推导及代码解析（一）
【泡泡读者来稿】VINS 论文推导及代码解析（二）
【泡泡读者来稿】VINS 论文推导及代码解析（三）
【泡泡读者来稿】VINS 论文推导及代码解析（四）

// 中值积分递推Jacobian和Covariance
// _acc_0上次测量加速度 _acc_1本次测量加速度 delta_p上一次的位移 result_delta_p位置变化量计算结果 update_jacobian是否更新雅克比基本方法就涉及到了IMU的创博方针和器方差矩阵的窗哦sdf
void midPointIntegration(double _dt, 
                        const Eigen::Vector3d &_acc_0, const Eigen::Vector3d &_gyr_0,
                        const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1,
                        const Eigen::Vector3d &delta_p, const Eigen::Quaterniond &delta_q, const Eigen::Vector3d &delta_v,
                        const Eigen::Vector3d &linearized_ba, const Eigen::Vector3d &linearized_bg,
                        Eigen::Vector3d &result_delta_p, Eigen::Quaterniond &result_delta_q, Eigen::Vector3d &result_delta_v,
                        Eigen::Vector3d &result_linearized_ba, Eigen::Vector3d &result_linearized_bg, bool update_jacobian)

之后计算对应绝对坐标系下的位置等

    // Rs Ps Vs是frame_count这一个图像帧开始的预积分值,是在绝对坐标系下的.
    int j = frame_count;         
    Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g;//移除了偏执的加速度
    Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - Bgs[j];//移除了偏执的gyro
    Rs[j] *= Utility::deltaQ(un_gyr * dt).toRotationMatrix();
    Vector3d un_acc_1 = Rs[j] * (linear_acceleration - Bas[j]) - g;
    Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
    Ps[j] += dt * Vs[j] + 0.5 * dt * dt * un_acc;
    Vs[j] += dt * un_acc;

3.2 对图像的处理

请见下一博客