vins学习----代码逻辑

参考博文链接@Manii

一、 ros的逻辑结构

1、std_msgs/Header

在image/PointCoud/IMU等各种传感器数据结构中的头信息

uint32 seq   //ID
time stamp //时间戳
string frame_id //坐标系ID

2、sensor_msgs::ImageConstPtr

在feature_trackers_node.cpp中回调函数img_callbck的输入，表示一副图像。

std_msgs/Header header //头信息
uint32 height //高
uint32 width //宽
string encoding //像素编码——通道含义、顺序、大小

uint8 is_bigendian //
uint32 sted //行长度
uint8[] data //实际矩阵大小 ，size is (step * rows)

3、sensor_msgs::PointCloudPtr featur_points

在feature_trackers_node.cpp中创建并封装，在main()中发布话题“/feature_tracker/feature”，包含一帧图像的特征点信息

sensor_msgs::PointCloudPtr feature_points(new sensor_msgs::PointCloud);

pub_img.publish(feature_points);
pub_img = n.advertise<sensor_msgs::PointCloud>("feature", 1000);

Definition

std_msgs/Header header #头信息
	feature_points->header = img_msg->header;
	feature_points->header.frame_id = "world";

geometry_msgs/Point32[] points #3D点(x,y,z)

sensor_msgs/ChannelFloat32[] channels  #[特征点的ID,像素坐标u,v,速度vx,vy]
	feature_points->channels.push_back(id_of_point);
	feature_points->channels.push_back(u_of_point);
	feature_points->channels.push_back(v_of_point);
	feature_points->channels.push_back(velocity_x_of_point);
	feature_points->channels.push_back(velocity_y_of_point);

sensor_msgs::PointCloud msg_match_points

这个东西数据格式和之前的feature_points一样，但是channels不一样。
在feature_trackers_node.cpp中建立并封装成msg_match_points,
在pose_graph_node.cpp的main()中发布话题“/pose_graph/match_points”
主要包含重定位的两帧间匹配点和匹配关系（变换矩阵）

sensor_msgs::PointCloud msg_match_points;
pub_match_points.publish(msg_match_points);

pub_match_points = n.advertise<sensor_msgs::PointCloud>("match_points", 100);

在estimator_node.cpp的main()中该话题被订阅，回调函数为relocalization_callback()

ros::Subscriber sub_relo_points = n.subscribe("/pose_graph/match_points", 2000, relocalization_callback);
void relocalization_callback(const sensor_msgs::PointCloudConstPtr &points_msg)

Definition

std_msgs/Header header #头信息
	#msg_match_points.header.stamp = ros::Time(time_stamp);

geometry_msgs/Point32[] points #3D点(x,y,z)

sensor_msgs/ChannelFloat32[] channels  #[重定位帧的平移向量T的x,y,z，旋转四元数w,x,y,z和索引值]
	#t_q_index.values.push_back(T.x());
	#t_q_index.values.push_back(T.y());
	#t_q_index.values.push_back(T.z());
	#t_q_index.values.push_back(Q.w());
	#t_q_index.values.push_back(Q.x());
	#t_q_index.values.push_back(Q.y());
	#t_q_index.values.push_back(Q.z());
	#t_q_index.values.push_back(index);
	#msg_match_points.channels.push_back(t_q_index);

4、sensor_msgs::ImuConstptr

IMU信息的标准数据结构

Header header	#头信息

geometry_msgs/Quaternion orientation	#四元数[x,y,z,w]
float64[9] orientation_covariance		# Row major about x, y, z axes

geometry_msgs/Vector3 angular_velocity	#角速度[x,y,z]轴
float64[9] angular_velocity_covariance	# 对应协方差矩阵，Row major(行主序) about x, y, z axes

geometry_msgs/Vector3 linear_acceleration	#线性加速度[x,y,z]
float64[9] linear_acceleration_covariance	# 对应协方差矩阵 Row major x, y z 

代码中的组合结构

1、measurements

std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>> measurements;

estimator_node.cpp中的getMeasurements()函数，将对imu和图像数据进行初步对齐得到的数据结构，确保图像关联着对应时间戳内的所有imu数据
sensor_msgs::PointCloudConstPtr 表示某一帧图像的feature_points
std::vector 表示当前帧和上一帧时间间隔中的所有IMU数据
将两者组合成一个数据结构，并构建元素为这种结构的vector进行存储
2、map>>> image
在estimator.cpp中的process()中被建立，在Estimator::processImage()中被调用
作用是建立每个特征点(camera_id,[x,y,z,u,v,vx,vy])构成的map，索引为feature_id
3、map all_image_frame

在estimator.h中作为class Estimator的属性
键是图像帧的时间戳，值是图像帧类
图像帧类可由图像帧的特征点与时间戳构造，此外还保存了位姿Rt，预积分对象pre_integration，是否是关键帧。

二、视觉跟踪 feature_trackers

前言

主要介绍VINS的视觉处理前段的视觉跟踪模块，主要包括一下内容：

• 对于每一帧新图像，KLT稀疏光流算法对现有特征进行跟踪；
• 检测新的脚点特征以保证每个图像特征的最小数目（100-300）；
• 通过设置两个相邻特征之间像素的最小间隔来执行均匀的特征分布；
• 利用基本矩阵模型的RANSAC算法进行外点剔除；
• 对特征点进行去畸变矫正，然后投影到一个单位球面上。
  关键帧的选择在之后讨论。

代码逻辑

• 主程序

1. 设置logger的级别;
2. readParameters（）;
3. 是否加入鱼眼mask来去除边缘噪声;
4. 订阅MAGE_TOPIC执行img_calback;
5. 发布TOPIC(跟踪的特征点、跟踪的特征点图、复位信号);
6. ros::sqin()。

img_calback

1. 判断是否是第一帧first_image_flag；
2. 判断时间间隔，（大于1秒并且当前帧的时间小于上一帧）有问题则restart；
3. 通过时间间隔内的发布次数进行发布频率控制；
4. FeatureTracker::readImage()读取图像数据进行处理；
5. 更新全局ID；
6. 如果PUB_THIS_FRAME=1，则进行信息封装和发布。

FeatureTracker::readImage()读取图像数据进行处理

1. 如果光太亮或太暗则为1，进行直方图均衡化；
2. 对前一帧特征点cur_pts进行LK金字塔光流跟踪，得到forw_pts；
3. 根据status,剔除跟踪失败和图像便捷的外点；
4. 通过基本矩阵剔除外点（RANSCA），即用RANSCA去除外点；（十四讲P148）；
5. 对跟踪点排序并依次选点，使用mask进行类似非极大抑制，半径为30，去除密集点使特征点分布均匀；
6. 寻找新的在特征点补齐
7. 对新的特征点去畸变矫正和深度归一化，计算每个角点的速度。

三、状态估计器流程

主要内容

1. vins估计器的初始化
2. 基于滑动窗口的非线性优化实现紧耦合；
3. 关键帧的选取（平均视差法、跟踪质量法）；
4. 外参标定、可视化等。

输入：

1. IMU的教书度和线加速度，订阅"/imu0"
2. 图像跟踪的特征点，订阅"/feature_tracker/feature"
3. 复位信号，订阅"/feature_tracker/restart"
4. 重定位的匹配点，"/pose_graph/match_points"

输出：

1. 在线程void process()中给RVIZ发送里程计信息PQV、关键点三维坐标、相机位姿、点云信息、IMU到相机的外参、重定位位姿等。
2. 在回调函数void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg)发布最新的由IMU直接递推得到的PQV。

具体步骤

一、 vins_estimator/estimator_node.cpp
1.

多线程共享，设置互斥锁、条件锁

2. measurements

std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>> measurements;

estimator_node.cpp中getMeaturement()函数将对imu和图像数据进行初步对齐得到的数据结构，确保图像关联着对应时间戳内所有imu数据。
sensor_msgs::PointCloudConstPtr 表示某一帧图像的feature_points
std::vector 表示当前帧和上一阵时间间隔中的所有IMU数据将两者组合成一个数据结构，并构建元素为这种Vector进行存储。

主线程　void process()

通过while不断循环，主要功能包括等待并获取measurements,计算dt，然后执行一下函数：
estimator.processIMU()进行预积分
estimator.setReloFrame()设置重定位帧
estimator.processimage()处理图像帧：初始化，紧耦合的非线性优化
其中meaturements的数据格式可以表示为：(IMUs, img_msg)Vector

主要函数介绍

std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>>
getMeasurements()
{
...
}

对imu和图像数据进行对齐并组合，返回的是(IMUs,img_msg),即图像所对应的所有imu数据，并将其放入一个容器vector中。
IMU和图像帧的对应关系在新版的代码中有变化：对图像帧j，每次取完imu_buf中所有时间戳小于它的imu_msg,以及第一个时间戳大于图像时间戳的imu_msg（这里还需要加上同步时间存在的延迟td）。