* @brief 基于滑动窗口紧耦合的非线性优化，残差项的构造和求解 * @Description 添加要优化的变量 (p,v,q,ba,bg) 一共15个自由度，IMU的外参也可以加进来 * 添加残差，残差项分为4块 先验残差+IMU残差+视觉残差+闭环检测残差

四自由度位姿图优化

地图点云回调函数，把point_msg放入point_buf

图像帧位姿回调函数，把pose_msg放入pose_buf

主线程

* @brief VIO的主线程 * @Description 等待并获取measurements：(IMUs, img_msg)s，计算dt * estimator.processIMU()进行IMU预积分 * est

* @brief IMU预积分传播方程 * @Description 积分计算两个关键帧之间IMU测量的变化量： * 旋转delta_q 速度delta_v 位移delta_p * 加速度的biaslineari

* @brief 通过F矩阵去除outliers * @Description 将图像坐标转换为归一化坐标 * cv::findFundamentalMat()计算F矩阵 * reduceVector()去除outliers * @r

* @brief 判断两帧有足够视差30且内点数目大于12则可进行初始化，同时得到R和T * @Description 判断每帧到窗口最后一帧对应特征点的平均视差是否大于30 solveRelativeRT()通过基础矩阵计算当前帧与第l帧之间的R和T,并

重定位回调函数，将重定位帧的相对位姿放入loop_info，updateKeyFrameLoop()进行回环更新

relocalization回调函数，将points_msg放入relo_buf

移除外点

restart回调函数，收到restart时清空feature_buf和imu_buf，估计器重置，时间重置

* @brief 将关键帧与回环帧进行BRIEF描述子匹配 * @param[out] matched_2d_old 回环帧匹配后的二维坐标 * @param[out] matched_2d_old_norm 回环帧匹配后的二维归一化坐标 * @param[out] status

关键帧中某个特征点的描述子与回环帧的所有描述子匹配

* @brief 对跟踪点进行排序并去除密集点 * @Description 对跟踪到的特征点，按照被追踪到的次数排序并依次选点 * 使用mask进行类似非极大抑制，半径为30，去掉密集点，使特征点分布均匀 * @return v

* Sets the timestamp from seconds and microseconds * @param secs: seconds * @param usecs: microseconds */

* @brief 实现滑动窗口all_image_frame的函数 * @Description 如果次新帧是关键帧，则边缘化最老帧，将其看到的特征点和IMU数据转化为先验信息 如果次新帧不是关键帧，则舍弃视觉测量而保留IMU测量值，从而保证IMU预积分的连贯性

滑动窗口边缘化次新帧时处理特征点被观测的帧号 real marginalization is removed in solve_ceres()

滑动窗口边缘化最老帧时处理特征点被观测的帧号 real marginalization is removed in solve_ceres()

PNP方法得到第l帧到第i帧的R_initial、P_initial

三角化求解所有特征点的深度，并进行非线性优化

根据两帧特征点求解两帧的旋转矩阵

* \brief Project a 3D point (\a x,\a y,\a z) to the image plane in (\a u,\a v) * * \param P 3D point coordinates * \param p return value, contains

* \brief Project a 3D point (\a x,\a y,\a z) to the image plane in (\a u,\a v) * * \param P 3D point coordinates * \param p return value, contains

* \brief Project a 3D point (\a x,\a y,\a z) to the image plane in (\a u,\a v) * * \param P 3D point coordinates * \param p return value, contains

//PRIVATE FUNCTIONS Function to calculate the value of a given spline at a point xval