【VSLAM系列】四：Vins-Mono源码学习笔记

VINS-Mono源码

工程化技巧：

滑动窗口的优化方式–>控制计算量同时实现优于滤波方法的里程计

高效的去畸变操作–>实时性优于opencv且精度不会下降的去畸变

不同实时性要求的处理方法–>后端实时性要求高于回环

优点：

套件价格、功耗、尺寸优势明显

快速鲁棒的单目IMU初始化过程

紧耦合的后端优化，在优化VIO位姿的同时还兼顾外参标定，零偏估计以及传感器延时估计

回环检测功能，便于构建全局一致性更好的位姿和地图

VINS中的问题：

尺度依赖于加速度计，一旦退化就成了单目VO

单目VIO鲁棒性不足

只能在纹理比较丰富的 场景下运行

静止时抖动，需要一定的运动激励

虽然会有地图的构建，不过是特征点的稀疏地图

1.光流追踪节点

坐标系定义，右手系，camera坐标系Z轴朝前，IMU坐标系X轴朝前

从零学习VINS-Mono/Fusion源代码
https://blog.csdn.net/slender_1031/article/details/127548106

feature_tracker功能包， 对应节点feature_tracker，节点接收话题/cam0/image_row，节点发布话题feature_tracker/feature_img（发给后端的）、 feature、restart

光流追踪节点只进行特征点追踪和提取，不进行相机位姿估计

1.1 ros节点处理

1）节点初始化

   //声明节点并读取配置参数，处理之后发布消息
    ros::init(argc, argv, "feature_tracker");//ros节点初始化
    ros::NodeHandle n("~");//声明一个句柄。~代表这个节点命名空间
    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);//设置ros log级别

2）读取配置文件

    readParameters(n);//读取配置文件

3）接收到消息进入回调函数

回调函数：判断当前帧是否要发送给后端 -> cv::bridge把图像转化为cv::Mat -> 开始光流追踪 -> 更新特征点的ID -> 给后端发布数据（如果需要发布）

发布消息

pub_img = n.advertise("feature", 1000);//发出去三个话题，实际发出去话题的名称是/feature_tracker/feature
pub_match = n.advertise("feature_img",1000);
pub_restart = n.advertise("restart",1000);[]()

1.2 前端光流追踪

前端要控制向后端发送图像数据的频率，维持在参数FREQ=10HZ左右

if (round(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time)) <= FREQ)
    //发送次数 / 当前帧与第一帧时间差 < 10hz
    {       
        PUB_THIS_FRAME = true;
        //....
    }

ros图片消息转为cv::Mat cv_bridge

cv_bridge::CvImageConstPtr ptr;//把ros::message转为cv::Mat
ptr = cv_bridge::toCvCopy(img_msg,sensor_msgs::image_encodings::MONO8);
cv::Mat show_img = ptr->image;//获取cv::Mat

1.2.1 图像均衡化处理

图片太亮或者太暗，提取特征点比较难，所以先均衡化，提升对比度，方便提取角点。调用cv::clahe函数

 if (EQUALIZE)
    {       
        cv::Ptr clahe = cv::createCLAHE(3.0, cv::Size(8, 8));
        clahe->apply(_img, img);//调用opencv函数
        }

1.2.2 光流追踪及剔除外点

cv::calcOpticalFlowPyrLK

void cv::calcOpticalFlowPyrLK (
InputArray prevImg,//以该图片为基准流追踪
InputArray nextImg,//用来获得追踪结果的图片
InputArray prevPts,//以该数组存储的特征点为基准就行流追踪，单位单精度浮点数
InputOutputArray nextPts, //获得追踪点的结果,存储在数组内
OutputArray status, //输出状态标记，如果追踪到了结果，则对应同角标的状态向量位置元素为1，否则为0
OutputArray err,//输出错误标记; 向量的每个元素都设置为相应特征的错误，错误度量的类型可以在flags参数中设置; 如果未找到流，则对错误不进行定义（使用status参数查找此类情况）
Size winSize = Size(21, 21),//每个金字塔的搜索窗口的winSize大小
int maxLevel = 3,//金字塔层数，图像层级总数为该值＋1
TermCriteria criteria = TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, 0.01), //指定迭代搜索算法的终止条件的参数
int flags = 0, //判定追踪结果错误的度量

追踪到的特征点外点剔除

 //Step1 通过cv函数追踪的status状态来剔除外点
cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, cv::Size(21, 21), 3);//opencv光流追踪函数
//Step2 通过图像边界剔除外点
if (status[i] && !inBorder(forw_pts[i]))status[i] = 0;//如果点在边界上把状态置为0
//Step3 通过对极几何约束剔除外点
rejectWithF();

计算本质矩阵剔除外点

//opencv接口计算本质矩阵，通过status剔除外点，如果两个点之间的距离大于门限F_THRESHOLD即认为是外点
cv::findFundamentalMat(un_cur_pts, un_forw_pts, cv::FM_RANSAC, F_THRESHOLD, 0.99, status);

特征点均匀化 setmask() 函数

//以特征点为中心，以 MIN_DIST为半径画圆，此时这个圆内的mask矩阵的值被修改为-1，那么下一次在这个圆内的特征点就存不进去了 
for (auto &it : cnt_pts_id)
    {
    if (mask.at(it.second.first) == 255)//初始值为255
        {
            //存点
            cv::circle(mask, it.second.first, MIN_DIST, 0, -1);
        }
    }

1.2.3 提取新的特征点

只有需要发布信息给后端时候才会提取特征点

 cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.01, MIN_DIST, mask);

void cv::goodFeaturesToTrack(
		cv::InputArray image, // 输入图像（CV_8UC1 CV_32FC1）
		cv::OutputArray corners, // 输出角点vector
		int maxCorners, // 最大角点数目
		double qualityLevel, // 质量水平系数（小于1.0的正数，一般在0.01-0.1之间）
		double minDistance, // 最小距离，小于此距离的点忽略
		cv::InputArray mask = noArray(), // mask=0的点忽略
		int blockSize = 3, // 使用的邻域数
		bool useHarrisDetector = false, // 用于指定角点检测的方法，如果是true则使用Harris角点检测，false则使用Shi Tomasi算法。
		double k = 0.04 // Harris角点检测时使用，默认0.04
	);

//将新提取到的特征点添加到当前帧的特征点集中
void FeatureTracker::addPoints();

1.2.4 像素点去畸变并计算速度

cv::undistortPoints(cur_pts, un_pts, K, cv::Mat());//cv函数去畸变，但是使用的是自定义的去畸变的函数
//计算速度
double v_x = (cur_un_pts[i].x - it->second.x) / dt;//当前帧的x位置- 上一帧的x位置/ dt
double v_y = (cur_un_pts[i].y - it->second.y) / dt;
pts_velocity.push_back(cv::Point2f(v_x, v_y));

离中间点越远的点，畸变程度越高，vins-mono使用递归的方法去除畸变

去畸变是由畸变后的点x’ y’计算畸变前的点x y ，使用八次迭代逼近原点

 for (int i = 1; i < n; ++i)//递归八次
            {
                distortion(Eigen::Vector2d(mx_u, my_u), d_u);
                mx_u = mx_d - d_u(0);
                my_u = my_d - d_u(1);
            }

2. 位姿估计节点

vins_estimater功能能包，接收feature_tracker节点发出的特征点数据和imu数据发出imu预积分结果数据imu_propagete，关键帧位姿keyframe_pose

2.1 ros节点处理

读取配置文件参数VinsMono_ws/src/vins_estimator/src/parameters.cpp，其中配置文件的路径在launch文件中给出

2.2 回调函数

2.2.1 IMU回调

接收imu话题发来的数据，imu消息存进buffer（是一个queue数据类型），同时按照IMU频率预测位姿并发送，这样使得里程计频率和IMU频率相当，发送的话题是vins_estimator/odometry

void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg)
{
    if (imu_msg->header.stamp.toSec() <= last_imu_t)//判断消息时间戳是否正确
    {
        ROS_WARN("imu message in disorder!");
        return;
    }
    last_imu_t = imu_msg->header.stamp.toSec();

    m_buf.lock();//上锁
    imu_buf.push(imu_msg);//存imu数据
    m_buf.unlock();//解锁
    con.notify_one();//条件变量通知等待的线程

    last_imu_t = imu_msg->header.stamp.toSec();//存储当前IMU数据的时间戳

    {
        std::lock_guard<std::mutex> lg(m_state);//`lock_guard`在构造时自动锁定互斥量, 而在退出作用域时会析构自动解锁, 
        predict(imu_msg);//预测IMU位姿
        std_msgs::Header header = imu_msg->header;
        header.frame_id = "world";
        if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR)//表示初始化已经完成
            pubLatestOdometry(tmp_P, tmp_Q, tmp_V, header);//把预测的结果pub出去
    }
}

2.2.2 图像特征点回调函数

接收光流追踪节点发送来的数据/feature_tracker/feature
回调函数中只把特征点消息数据存入了buffer中，未进行其他操作

    //接收光流追踪节点发送的数据并进入回调
    ros::Subscriber sub_image = n.subscribe("/feature_tracker/feature", 2000, feature_callback);

2.2.4 检测是否需要重启

接收光流追踪节点发送来的/feature_tracker/restart话题

    //是否需要重启
    ros::Subscriber sub_restart = n.subscribe("/feature_tracker/restart", 2000, restart_callback);

2.2.5 回环检测

2.3 位姿估计线程

 std::thread measurement_process{process};

1）从ros消息中取IMU和image数据对，一个image数据+多个imu数据

2） 处理imu数据（imu预积分见3）

for()//在node.cpp里面循环取IMU数据时候调用该函数
estimator.processIMU(dt, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));//处理IMU数据
//对IMU数据进行处理，包括更新滑动窗口内某个帧的预积分量，和提供优化使用的初始值

3）处理图像数据

void Estimator::processImage(const map>>> &image, const std_msgs::Header &header)

/VinsMono_ws/src/vins_estimator/src/feature_manager.cpp 中进行特征点管理

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-ajoDVltB-1686300235093)(/media/jeet/My Passport/SLAM学习资料/学习之路/已学VIO/文档图片/特征点管理.jpg)]

关键帧选择在/VinsMono_ws/src/vins_estimator/src/feature_manager.cpp addFeatureCheckParallax函数中进行
视差计算 在compensatedParallax2函数中进行

关键帧选择策略，第1 、2帧为关键帧，该帧从以前的帧中追踪到的特征点总数小于阈值（20）为关键帧，倒数第二帧与倒数第三针的共视点的距离大于阈值（10个像素）则倒数第二帧为关键帧

**4）初始化 **见4

3. IMU预积分

3.1 IMU的卡尔曼滤波

加速度传感器测量与惯性有关的加速度，包括旋转、重力和线性加速度，对测量数据进行积分可以得到线性速度，二次积分可以得到线位移，但积分产生的漂移误差将随时间累积而无限制地增长导致积分后得到的数据不准确。加速度传感器通过三角函数运算可获得倾角值，但是输出信号容易受噪声污染。当加速度传感器垂直于俯仰角安装时，通过反正切函数运算可以作为倾角仪使用进行360°的全方位测量，并且不会产生线性化误差。

陀螺仪测量瞬时旋转角速度，由于温度变化、摩擦力、不稳定力矩等因素，会产生漂移误差。对陀螺仪测量数据进行积分可以得到与垂直方向相关的倾角信息，动态响应快，但漂移误差将随时间累积而无限增长。陀螺仪具有足够的带宽，动态性能好，静态输出受漂移误差影响较大。

3.2IMU预积分

vins_estimator功能包 factor文件夹，接收话题/imu0，发布话题imu_propagate

intergration_bash.h 文件进行IMU预积分值计算以及雅克比和协方差矩阵的传递
每一次调用propagate函数后计算出相对于第k帧位姿下的p v q 和更新之后的雅克比与协方差矩阵（midPointIntegration函数实现）

void propagate(double _dt, const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1)
    {
        dt = _dt;
        acc_1 = _acc_1;
        gyr_1 = _gyr_1;
        Vector3d result_delta_p;
        Quaterniond result_delta_q;
        Vector3d result_delta_v;
        Vector3d result_linearized_ba;
        Vector3d result_linearized_bg;

        midPointIntegration(_dt, acc_0, gyr_0, _acc_1, _gyr_1, delta_p, delta_q, delta_v,                            linearized_ba, linearized_bg,                            result_delta_p, result_delta_q, result_delta_v,                            result_linearized_ba, result_linearized_bg, 1);       
        //更新单步积分后的值
        delta_p = result_delta_p;
        delta_q = result_delta_q;
        delta_v = result_delta_v;
        linearized_ba = result_linearized_ba;
        linearized_bg = result_linearized_bg;
        delta_q.normalize();
        sum_dt += dt;
        acc_0 = acc_1;
        gyr_0 = gyr_1;  
     
    }

4. vio初始化

/VinsMono_ws/src/vins_estimator/src/initial功能包进行初始化

4.1 外参标定

initial_ex_rotation.cpp CalibrationExRotation函数

bool InitialEXRotation::CalibrationExRotation(vector> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result)//外参标定函数

Matrix3d solveRelativeR(const vector> &corres);//相机对极几何求解旋转

//测试对极几何的四组解
 double testTriangulation(const vector &l,
                             const vector &r,
                             cv::Mat_ R, cv::Mat_ t);
    void decomposeE //本质矩阵分解

4.2 纯视觉SFM

/VinsMono_ws/src/vins_estimator/src/estimator.cpp 文件Estimator::initialStructure()函数

VinsMono_ws/src/vins_estimator/src/initial/initial_sfm.cpp

整体思路
1）在关键帧滑动窗口内找一个枢纽帧
2）求解枢纽帧与最后一帧的位姿变换
3）三角化枢纽帧与最后一帧的路标点，然后pnp求解枢纽帧与最后一帧之间所有帧的位姿与路标点三角化
4）考虑到有些特征点不能被最后一帧看到，因此，固定枢纽帧，遍历枢纽帧到最后一帧进行特征点三角化
5）处理第0帧到枢纽帧的位姿与路标点三角化
6）求解出了所有帧的位姿，把所有的路标点三角化
7）BA优化关键帧所有的路标点与相机位姿

8）通过关键帧的位姿和三角化出来的路标点将其他所有帧的位姿都求解出来

	cv::eigen2cv(R_initial, tmp_r);//eigen矩阵转为cv::Mat

PnP求解位姿

bool GlobalSFM::solveFrameByPnP(Eigen::Matrix3d &R_initial, Eigen::Vector3d &P_initial, int i, std::vector &sfm_f)
//pnp求解（枢纽帧与最后一帧之间）帧的位姿

/*参数:
R_initial – 入参，上一帧的旋转矩阵，出参，这一帧的旋转矩阵，入参是为了给slovePnP一个初值
P_initial – 入参，上一帧的平移向量，出参，这一帧的平移向量
i – 求解帧的索引
sfm_f – 所有的特征点信息

返回:
true 求解成功 false*/

路标点三角化

void GlobalSFM::triangulateTwoFrames(int frame0, Eigen::Matrix<double, 3, 4> &Pose0, int frame1, Eigen::Matrix<double, 3, 4> &Pose1, std::vector<SFMFeature> &sfm_f)
/*根据两帧索引和对应的位姿，对共视路标点进行三角化

参数:
frame0 – 前一帧的索引
Pose0 – 前一帧的位姿
frame1 – 后一帧的索引
Pose1 – 后一帧的位姿
sfm_f – 路标点*/

4.3 视觉惯性对齐

VinsMono_ws/src/vins_estimator/src/initial/initial_aligment.cpp文件

4.3.1 陀螺仪零偏估计

这里的零偏是固定参数，不是滑窗中的随机游走

/**
 * @brief 求解陀螺仪零偏，同时利用求出来的零偏对预积分值进行重新传播
 * 
 * @param all_image_frame 所有的图像帧
 * @param Bgs 出参
 */
void solveGyroscopeBias(map &all_image_frame, Vector3d* Bgs)
{
            A += tmp_A.transpose() * tmp_A;//AT * A * x = AT * B
        b += tmp_A.transpose() * tmp_b;

    }
    delta_bg = A.ldlt().solve(b);//使用eigen的ldlt分解求解x

使用Eigen库的LDLT分解求解线性方程组

A << 1, 0.5, 0.5, 0.5, 1, 0.5, 0.5, 0.5, 1;//给A赋值
	b << 1, -2, 3;//给b赋值
	Vector3d x = A.ldlt().solve(b);//求解Ax=b

4.3.2 求解尺度和重力方向和速度

bool LinearAlignment(std::map &all_image_frame, Eigen::Vector3d &g, Eigen::VectorXd &x)
/*视觉惯性对齐， 求解个帧的速度，枢纽帧的重力方向，以及尺度 优化的变量X = [v1,v2, v3...,gc0, s];

参数:
all_image_frame
g – 出参，枢纽帧的重力方向
x - 尺度

返回:
true false*/

重力方向较正

    c = a.cross(b);//求a叉乘b

5 滑动窗口优化

5.1 IMU残差构建

VinsMono_ws/src/vins_estimator/src/factor/imu_factor.h

/// @brief 使用解析求导，需要自定义的参数继承SizedCostFunction<残差的维度, 约束参数块的维度, 约束的参数块2的维数, 两帧的位姿所以是7979>
class IMUFactor : public ceres::SizedCostFunction<15, 7, 9, 7, 9>{
     /**
     * @brief 使用ceres解析求导，必须重载Evaluate函数
     * 
     * @param[in] parameters 这是一个二维数组，每个参数块都是一个double数组，而一个观测会对多个参数块形成约束
     * @param[in] residuals 残差的计算结果，是一个一维数组，残差就是该观测和约束的状态量通过某种关系形成残差
     * @param[in] jacobians 残差对参数块的雅克比矩阵，这也是一个二维数组，对任意一个参数块的雅克比都是一个一维数组
     * @return true 
     * @return false 
     */
    virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
    {
}

5.2 视觉重投影残差构建

ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);
                problem.AddResidualBlock(f, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index]);

//构造函数，传入点在两帧下的归一化相机坐标系位姿
ProjectionFactor::ProjectionFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j) : pts_i(_pts_i), pts_j(_pts_j)
{
#ifdef UNIT_SPHERE_ERROR
    Eigen::Vector3d b1, b2;
    Eigen::Vector3d a = pts_j.normalized();
    Eigen::Vector3d tmp(0, 0, 1);
    if(a == tmp)
        tmp << 1, 0, 0;
    b1 = (tmp - a * (a.transpose() * tmp)).normalized();
    b2 = a.cross(b1);
    tangent_base.block<1, 3>(0, 0) = b1.transpose();
    tangent_base.block<1, 3>(1, 0) = b2.transpose();
#endif
};

//重载Evaluate函数，二维数组parameters包含两帧的相机位姿以及外参，特征点的逆深度
bool ProjectionFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const

5.3 边缘化

边缘化的时候外参和时延不在滑窗内维护，因此滑窗内不需要移动他们

边缘化的fill-in现象， 把原来稀疏的矩阵变成了稠密矩阵，VINS里面选择边缘化掉最老帧及最老帧看到的路标点和IMU数据<窗口内01帧>，既然稠密矩阵无法避免，就减少矩阵的维度

MarginalizationInfo *marginalization_info = new MarginalizationInfo();
        vector2double();//这里使用了手写高斯牛顿法， 因此需要转化为double数组

        //关于边缘化有几点需要注意的地方
        //1、找到需要边缘化的参数块，这里是地图点，第0帧位姿，第0帧速度以及零偏
        //2、找到高斯牛顿法下降时跟这些待边缘化相关的参数块有关的残差约束，就是预积分约束、重投影约束，以及上一次边缘化的约束
        //3、这些约束链接的参数块中，不需要被边缘化的参数块，就是提供先验约束的部分，也就是滑窗中剩下的位姿、速度、零偏

1、添加IMU预积分约束

        // 只有第一个预积分结果和待边缘化的参数块相连
        {
            if (pre_integrations[1]->sum_dt < 10.0)//预积分时间跨度要在10s的范围内
            {
                //跟构建ceres约束问题一样，这里也需要得到残差和雅克比
                IMUFactor* imu_factor = new IMUFactor(pre_integrations[1]);//预积分factor
                //ResiualBlockInfo(imu的costfunction, 核函数，涉及的参数块在double数组中的指针位置，需要被边缘化的参数的索引，这里是第0帧和1帧 ）
                ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(imu_factor, NULL,
                                                                           vector{para_Pose[0], para_SpeedBias[0], para_Pose[1], para_SpeedBias[1]},
                                                                           vector{0, 1});//这里就是第0帧和第1帧的参数块需要被边缘化的
                marginalization_info->addResidualBlockInfo(residual_block_info);//加入残差块给边缘化对象
            }
        }

2、 添加视觉重投影约束

        {//===========
        ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);
                        ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(f, loss_function,
                                                                                       vector<double *>{para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index]},
                                                                                       vector<int>{0, 3});//这里第0帧和地图点被marge掉
                        marginalization_info->addResidualBlockInfo(residual_block_info);//添加视觉重投影误差约束
                    }

6 回环检测

使用词袋模型，特征点+描述子

回环检测中两种位姿的表示

vio位姿：滑窗优化输出的位姿，里程计位姿不受回环检测影响，一直按照自己的估计
最终位姿：经过回环优化后的位姿
回环检测的最终位姿不会反馈给里程计，因此在vio位姿和最终位姿中存在一个T,r

6.1 Ros节点处理

VinsMono_ws/src/pose_graph/src/pose_graph_node.cpp

 //订阅的一些topic
    ros::Subscriber sub_imu_forward = n.subscribe("/vins_estimator/imu_propagate", 2000, imu_forward_callback);//接收imu预警传播的话题
    ros::Subscriber sub_vio = n.subscribe("/vins_estimator/odometry", 2000, vio_callback);//接收滑动窗口内的最新位姿，不一定是KF
    ros::Subscriber sub_image = n.subscribe(IMAGE_TOPIC, 2000, image_callback);//接收相机原图
    ros::Subscriber sub_pose = n.subscribe("/vins_estimator/keyframe_pose", 2000, pose_callback);//接收关键帧位姿
    ros::Subscriber sub_extrinsic = n.subscribe("/vins_estimator/extrinsic", 2000, extrinsic_callback);//接收外参消息，如果外参需要估计，那么外参在优化过程中会更新
    ros::Subscriber sub_point = n.subscribe("/vins_estimator/keyframe_point", 2000, point_callback);//接收地图点消息
    ros::Subscriber sub_relo_relative_pose = n.subscribe("/vins_estimator/relo_relative_pose", 2000, relo_relative_pose_callback);

//回环检测的主处理函数
void process(){
//.............
//创建关键帧
KeyFrame* keyframe = new KeyFrame(pose_msg->header.stamp.toSec(), frame_index, T, R, image, point_3d, point_2d_uv, point_2d_normal, point_id, sequence); 
 posegraph.addKeyFrame(keyframe, 1);//回环检测的核心入口函数，添加关键帧
}

6.2 回环检测关键帧处理

VinsMono_ws/src/pose_graph/src/pose_graph/src/keyframe.cpp

特征点的描述子计算

//计算描述子类
class BriefExtractor
{
public:
//重载括号运算符，用于计算描述子
  virtual void operator()(const cv::Mat &im, vector &keys, vector &descriptors) const;
  BriefExtractor(const std::string &pattern_file);

  DVision::BRIEF m_brief;//DBOW二进制描述子文件
};

//调用Dbow的描述子计算函数，计算特征点的描述子，在DVision文件夹下
void BriefExtractor::operator() (const cv::Mat &im, vector &keys, vector &descriptors) const
{
  m_brief.compute(im, keys, descriptors);
}

computeBRIEFPoint();//仅靠前端提取的特征点数目可能有点少，因此额外的提取一些FAST角点并进行描述子计算!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

6.3 回环检测

/**
 * @brief 进行回环检测
 * 
 * @param keyframe 关键帧
 * @param frame_index 关键帧的索引
 * @return int 
 */
int PoseGraph::detectLoop(KeyFrame* keyframe, int frame_index){
    //.........
    //调用词袋查询接口，查询的结果是ret，最多返回4个备选ret，查找距离当前至少50帧的KF
    db.query(keyframe->brief_descriptors, ret, 4, frame_index - 50);
    //printf("query time: %f", t_query.toc());
    //cout << "Searching for Image " << frame_index << ". " << ret << endl;

    TicToc t_add;
    //把当前帧送进数据库，便于后序的帧查询
    db.add(keyframe->brief_descriptors);
    //..............
}

回环校验

/**
 * @brief 通过PNP对当前帧和回环帧是够构成回环进行校验
 * 
 * @param [in] matched_2d_old_norm 回环帧2d归一化坐标
 * @param [in] matched_3d 当前帧匹配到的点的3d坐标
 * @param [out] status 状态位
 * @param [out] PnP_T_old //回环帧在vio坐标系下的位姿
 * @param [out] PnP_R_old 
 */
void KeyFrame::PnPRANSAC(const vector &matched_2d_old_norm,
                         const std::vector &matched_3d,
                         std::vector &status,
                         Eigen::Vector3d &PnP_T_old, Eigen::Matrix3d &PnP_R_old)
//cheak1： 通过描述子校验，更新匹配上的点的坐标matched_2d_old, matched_2d_old_norm,和每个点对应的状态位status，匹配点对大于25
	searchByBRIEFDes(matched_2d_old, matched_2d_old_norm, status, old_kf->brief_descriptors, old_kf->keypoints, old_kf->keypoints_norm);

//cheak2 : 进行PNP几何验证，利用当前的地图点（3d）和回环帧的归一化相机坐标（2d）进行匹配,匹配点对大于25个
PnPRANSAC(matched_2d_old_norm, matched_3d, status, PnP_T_old, PnP_R_old);

//cheak3: 两帧的相对位姿差是否在一定阈值内，yaw角 < 30°，两帧平移小于20
if (abs(relative_yaw) < 30.0 && relative_t.norm() < 20.0)

位姿图合并

// 如果这两帧来自于不用的（地图）序列，并且当前的序列没有与之前的合并，则将两个序列合并
            if (old_kf->sequence != cur_kf->sequence && sequence_loop[cur_kf->sequence] == 0)
            {  
                w_r_vio = shift_r;//这是在cur的vio下的世界坐标系下的位姿
                w_t_vio = shift_t;
                vio_P_cur = w_r_vio * vio_P_cur + w_t_vio;
                vio_R_cur = w_r_vio *  vio_R_cur;
                cur_kf->updateVioPose(vio_P_cur, vio_R_cur);//将当前帧的位姿更新到合并地图的世界坐标系下
                //同时把这个序列里面其他所有帧装换到上一个地图坐标里面取
                list::iterator it = keyframelist.begin();
                for (; it != keyframelist.end(); it++)   
                {
                    if((*it)->sequence == cur_kf->sequence)
                    {
                        Vector3d vio_P_cur;
                        Matrix3d vio_R_cur;
                        (*it)->getVioPose(vio_P_cur, vio_R_cur);
                        vio_P_cur = w_r_vio * vio_P_cur + w_t_vio;
                        vio_R_cur = w_r_vio *  vio_R_cur;
                        (*it)->updateVioPose(vio_P_cur, vio_R_cur);
                    }
                }
                sequence_loop[cur_kf->sequence] = 1;//当前序列和以前的序列已经合并
            }

6.4 滑窗后端优化具有回环的帧的位姿

pose_graph把（老）回环帧的位姿发送给VIO节点

6.5 四自由度位姿优化

只优化yaw角和平移

problem.AddParameterBlock(euler_array[i], 1, angle_local_parameterization);//第一个参数是数组的指针，第二个参数1是参数块大小，取1表示只取yaw角，第三个参数对应更新方式
                problem.AddParameterBlock(t_array[i], 3);//添加平移参数块
//固定回环（老）帧的位姿，以及（地图）序列0包含的帧，地图序列（0）表示事先加载的地图
if ((*it)->index == first_looped_index || (*it)->sequence == 0)
 {                                problem.SetParameterBlockConstant(euler_array[i]);                  problem.SetParameterBlockConstant(t_array[i]);
}

//添加帧间约束，把每5帧联系起来
problem.AddResidualBlock(cost_function, NULL, euler_array[i-j], 
                                            t_array[i-j], 
                                            euler_array[i], 
                                            t_array[i]);
//添加回环约束
//euler_array[connected_index]回环帧的yaw，t_array[connected_index]回环帧的位置,
                    //euler_array[i]当前帧的欧拉角, t_array[i]当前帧的位置
problem.AddResidualBlock(cost_function, loss_function, euler_array[connected_index], t_array[connected_index],                                         euler_array[i],                                                 t_array[i]);

6.7 视觉地图的保存与加载

保存路径在.yaml文件中

pose_graph_save_path: "../../../output/pose_graph/" # save and load path     加载或保存地图的路径
load_previous_pose_graph: 0        # load and reuse previous pose graph; load from 'pose_graph_save_path 是否加载已有地图

7 时间延时优化

通常将接收imu上报的数据时的时间点作为imu数据的时间戳。将接收图像采集单元上报的图像数据的时间点作为图像数据的时间戳。时间延迟来源于时钟不同步，传感器的触发和传输延迟（triggering and transmission delays）,假设时间延迟是一个固定的但是未知