三角化两帧间某个对应特征点的深度

三角化frame0和frame1间所有对应点

* \brief Projects an undistorted 2D point p_u to the image plane * * \param p_u 2D point coordinates * \return image point coordinates */

* \brief Projects an undistorted 2D point p_u to the image plane * * \param p_u 2D point coordinates * \return image point coordinates */

* \brief Projects an undistorted 2D point p_u to the image plane * * \param p_u 2D point coordinates * \return image point coordinates */

* \brief Projects an undistorted 2D point p_u to the image plane * * \param p_u 2D point coordinates * \return image point coordinates */

对角点图像坐标进行去畸变矫正，转换到归一化坐标系上，并计算每个角点的速度。

更新轨迹并发布

vector转换成double数组，因为ceres使用数值数组 Ps、Rs转变成para_Pose，Vs、Bas、Bgs转变成para_SpeedBias

VIO回调函数，根据pose_msg中的位姿得到IMU位姿和cam位姿

* @brief 视觉惯性联合初始化 * @Description 陀螺仪的偏置校准(加速度偏置没有处理) 计算速度V[0:n] 重力g 尺度s * 更新了Bgs后，IMU测量量需要repropagate * 得到尺度s和重力g的方向

* Returns the exception message */

* Destructor */