VINS-Fusion回环检测,在节点pose_graph_node中启动。
pose_graph_node总体流程如下:
重点看process线程。
process线程中,将订阅的图像、点云、位姿时间戳对齐,对齐后分别存入image_msg、point_msg、pose_msg。pose_msg为VIO后端优化发布的位姿。
一、创建关键帧keyFrame
根据位姿平移距离SKIP_DIS,选取关键帧,提取当前帧位姿、3d世界坐标系点,2d归一化相机平面坐标点,2d像素坐标点,特征创建关键帧keyframe。
KeyFrame::KeyFrame(double _time_stamp, int _index, Vector3d &_vio_T_w_i, Matrix3d &_vio_R_w_i, cv::Mat &_image,vector<cv::Point3f> &_point_3d, vector<cv::Point2f> &_point_2d_uv, vector<cv::Point2f> &_point_2d_norm,vector<double> &_point_id, int _sequence)
{time_stamp = _time_stamp;index = _index;vio_T_w_i = _vio_T_w_i;vio_R_w_i = _vio_R_w_i;T_w_i = vio_T