寻找并建立关键帧与回环帧之间的匹配关系

得到frame_count_l与frame_count_r两帧之间的对应特征点

* Returns the scoring method * @return scoring method */

* Returns the weighting method * @return weighting method */

多线程构造先验项舒尔补AX=b的结构，计算Jacobian和残差

开始一个新的图像序列（地图合并功能）

计算每个残差，对应的Jacobian，并更新parameter_block_data

从IMU测量值imu_msg和上一个PVQ递推得到下一个tmp_Q，tmp_P，tmp_V，中值积分

* @brief 处理IMU数据 * @Description IMU预积分，中值积分得到当前PQV作为优化初值 * @param[in] dt 时间间隔 * @param[in] linear_acceleration 线加速度 * @param[in] angular_v

剔除feature中估计失败的点（solve_flag == 2）

Reset the boundary conditions

视觉测量残差的协方差矩阵

对特征点进行三角化求深度（SVD分解）

标定外参的旋转矩阵

* @brief 计算尺度，重力加速度和速度 * @optional 速度、重力向量和尺度初始化Paper -> V-B-2 * 相邻帧之间的位置和速度与IMU预积分出来的位置和速度对齐，求解最小二乘 * 重力细化 -> Paper

* @brief 重力矢量细化 * @optional 重力细化，在其切线空间上用两个变量重新参数化重力 -> Paper V-B-3 g^ = ||g|| * (g^-) + w1b1 + w2b2 * @param[in] all_image

在半径为G的半球找到切面的一对正交基 -> Algorithm 1

视觉和IMU对齐

* @brief 把特征点放入feature的list容器中，计算每一个点跟踪次数和它在次新帧和次次新帧间的视差，返回是否是关键帧 * @param[in] frame_count 窗口内帧的个数 * @param[in] image 某帧所有特征点的[camera_id,[x,y,

* @brief 添加关键帧，进行回环检测与闭环 * @Description * @return void */

清空或初始化滑动窗口中所有的状态量

* @brief 纯视觉sfm，求解窗口中的所有图像帧的位姿和特征点坐标 * @param[in] frame_num 窗口总帧数（frame_count + 1） * @param[out] q 窗口内图像帧的旋转四元数q（相对于第l帧） * @param[out] T 窗口内

* Returns the branching factor of the tree (k) * @return k */

* Returns the depth levels of the tree (L) * @return L */

* @brief 对imu和图像数据进行对齐并组合 * @Description img: i -------- j - -------- k * imu: - jjjjjjjj - j/k kkkkkkkk - *

* Sets the test pattern * @param x1 x1 coordinates of pairs * @param y1 y1 coordinates of pairs * @param x2 x2 coordinates of pairs * @par

判断跟踪的特征点是否在图像边界内

从file_path读取位姿图

加载Brief字典

* @brief 处理图像特征数据 * @Description addFeatureCheckParallax()添加特征点到feature中，计算点跟踪的次数和视差，判断是否是关键帧 * 判断并进行外参标定 *

发布重定位帧的位姿信息