* @brief 从ros参数服务器中读取相关参数，设置初始协方差等 * * Imu状态向量和对应协方差的初始值 */

* @brief ROS的参数服务器中读取相关参数 * * 配置文件位于/config/ * 读取相机模型的类型、相机内参以及相机与imu之间的外参 * 读取图像的分辨率（长宽） */

This function is used to compute the measurement Jacobian for a single feature observed at a single camera frame. * @brief 计算某个特征点的单个相机状态对应的雅克比和归一化相机坐

* @brief 计算某个特征点对应所有的相机测量的雅克比，并消除Hf * @param H_x 雅克比矩阵 * @param r 量测残差 */

* @brief 根据单应性原理：已知一个平面的关键点可以得到另一个平面的关键点 * @param input_pts：上一时刻的第一个相机对应的关键点 * @param R_p_c: 利用imu数据计算得到的前后两个时刻图像帧的旋转初值 * @param intrinsics:相机内参 *

* @brief 将imu的当前状态通过四阶龙哥库塔积分来估计新的imu状态 * * 计算Omega * 计算四阶龙哥库塔积分的四个系数k1,k2,k3和k4 * 根据四阶龙格库塔积分得到新的状态：四元数、速度和位置 */

* @brief imu状态误差传递方程得到新的状态，循环处理每个imu数据 * * 得到gyro为^ω，而acc为^a * 根据论文得到矩阵F和G，并根据连续时间下的动态方程得到状态转移矩阵 * 四阶龙哥库塔积分得到预测的新状态 * 离散化状态转移方程得到噪声协方差矩阵Qk，并imu状态

* update的时机 * 1.失去feature的时候,也就是丢失的feature但是观测超过3个值 见removeFeatureLost函数 * 2.slideWindow满了的时候 */

* @brief 剔除那些不能被三角化,并且观测过于少的特征点, 同时计算雅克比和残差 * */

* @brief 归一化关键点的坐标，计算得到尺度因子 * @param pts1：上一时刻的关键点位置 * @param pts2:当前时刻跟踪匹配到的关键点位置 * @return scaling_factor：尺度因子 * */

* Set Covariance */

* @brief Set Covariance using Square Root * * @param covSquareRoot Lower triangular Matrix representing the covariance *

These are used to tune the Kalman filter */

* @brief 做了两部分工作：根据已知的imu与相机外参以及Imu运动模型 \n * 推测出当前相机的位姿并加入msckf状态向量中； 对系统的协方差矩阵进行增广 */

* @brief 将imu的消息类型保存在缓冲中 * */

* @brief 对两帧图像对做特征匹配，对极几何约束剔除外点 * @param cam0_points：第一帧图像帧的关键点位置 * @return cam1_points:第二帧图像中的关键点位置 * @return inlier_markers:匹配成功返回1，否则为0 * */