roslaunch vins_estimator euroc.launch
roslaunch vins_estimator vins_rviz.launch
rosbag play YOUR_PATH/MH_01_easy.bag
使用roslaunch命令通过launch文件，同时启动多个节点;
roslaunch pkg_name name.launch
1、 完成启动节点feature_tracker、vins_estimator、pose_graph三个节点；
2、 将相应参数和参数文件传递给对应节点；
3、 注意arg标签和param标签的使用区别；
4、 find feature_tracker找到对应包对应路径
5、解读 euroc_config.yaml
<launch> //launch文件标签
//arg参数标签，仅供launch文件内部使用//参数1和2名字分别为config_path和vins_path//参数1值是feature_tracker所在文件夹+/../config/euroc/euroc_config.yaml"//参数2值是feature_tracker所在文件夹+/../config/../ <arg name="config_path" default = "$(find feature_tracker)/../config/euroc/euroc_config.yaml" /> <arg name="vins_path" default = "$(find feature_tracker)/../config/../" />
//node节点标签，启动feature_tracker包中名为feature_tracker的节点//param参数标签，可以传给ros参数服务器，供该节点读取参数文件；//参数1名为config_file，其值是上面arg名字为config_path的值；//参数2名为vins_folder，其值是上面arg名字为vins_path的值； <node name="feature_tracker" pkg="feature_tracker" type="feature_tracker" output="log"> <param name="config_file" type="string" value="$(arg config_path)" /> <param name="vins_folder" type="string" value="$(arg vins_path)" /> </node>
//node节点标签，启动vins_estimator包中名为vins_estimator的节点//param参数标签，可以传给ros参数服务器，供该节点读取参数文件；//参数1名为config_file，其值是上面arg名字为config_path的值；//参数2名为vins_folder，其值是上面arg名字为vins_path的值； <node name="vins_estimator" pkg="vins_estimator" type="vins_estimator" output="screen"> <param name="config_file" type="string" value="$(arg config_path)" /> <param name="vins_folder" type="string" value="$(arg vins_path)" /> </node>
//node节点标签，启动pose_graph包中名为pose_graph的节点//param参数标签，可以传给ros参数服务器，供该节点读取参数文件；//参数1名为visualization_shift_x，值为0；//参数2名为visualization_shift_y，值为0；//参数3名为skip_cnt，值为0；//参数2名为skip_dis，值为0； <node name="pose_graph" pkg="pose_graph" type="pose_graph" output="screen"> <param name="config_file" type="string" value="$(arg config_path)" /> <param name="visualization_shift_x" type="int" value="0" /> <param name="visualization_shift_y" type="int" value="0" /> <param name="skip_cnt" type="int" value="0" /> <param name="skip_dis" type="double" value="0" /> </node>
</launch>
注1：启动过程中，涉重要的yaml参数设置文件euroc_config.yaml
注2：注意yaml文件格式
%YAML:1.0
6、解读vins_rviz.launch
6.1、启动包名为rviz中的节点rvizvisualisation，地图等展示界面
6.2、args参数标签，提供主函数入口argv和argc对应参数；
<launch> <node name="rvizvisualisation" pkg="rviz" type="rviz" output="log" args="-d $(find vins_estimator)/../config/vins_rviz_config.rviz" /></launch>
rosbag play YOUR_PATH_TO_DATASET/MH_01_easy.bag
rosbag主要用于记录、回访、分析rostopic中的数据；
rosbag可以将指定rostopic中的数据记录到.bag后缀的数据包中，便于离线分析和处理；
rosbag info filename.bag：可以显示数据包中的信息；
rosbag play bagfile：回访bag中包含的topic内容；
rosbag play bagfile –topic /topic：只播放感兴趣的topic；
VINS_MONO共有9个package，前端特征追踪节点feature_tracker对应于feature_tracker包中的feature_tracker_node.cpp，即该节点主函数；
int main(int argc, char **argv){ ros::init(argc, argv, "feature_tracker"); // ros节点初始化 ros::NodeHandle n("~"); // 声明一个句柄，～代表这个节点的命名空间 ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info); // 设置ros log级别 //节点初始化完成
readParameters(n); // 读取配置文件，根据rosluanch中传入的参数文件读取euroc.yaml文件中参数；
for (int i = 0; i < NUM_OF_CAM; i++) trackerData[i].readIntrinsicParameter(CAM_NAMES[i]); // 获得每个相机的内参
//IMAGE_TOPIC话题名称，在euroc.yaml中设置；//异步通讯，发布者、话题、订阅者可参考https://mp.weixin.qq.com/s/mCoDiXMSMCqyQm3DhcO0aw//sub_img向roscore注册订阅这个topic:IMAGE_TOPIC，收到一次message就执行一次回调函数//IMAGE_TOPIC：按照euroc.yaml中设定其为/cam0/image_raw ros::Subscriber sub_img = n.subscribe(IMAGE_TOPIC, 100, img_callback);
// 创建三个发布者// 发布者1：pub_img，发送数据为sensor_msgs::PointCloud，话题为feature，用于发送点云(特征点)；// 发布者2：pub_match,发送数据为sensor_msgs::Image，话题为feature_img，用于发送图像// 发布者3：pub_restart，发送数据std_msgs::Bool，话题为restart，用于发送重启信号 pub_img = n.advertise<sensor_msgs::PointCloud>("feature", 1000); // 实际发出去的是 /feature_tracker/feature pub_match = n.advertise<sensor_msgs::Image>("feature_img",1000); pub_restart = n.advertise<std_msgs::Bool>("restart",1000);
ros::spin(); // spin代表这个节点开始循环查询topic是否接收 return 0;}
//feature、feature_img、restart话题自动成为feature_tracker节点中的二级话题
//feature_tracker/feature
/feature_tracker/feature_img
/feature_tracker/restart
注：
节点/feature_tracker中包含一个订阅者sub_img，三个发布者pub_match、pub_img、pub_restart;
1、bagfile经话题/cam/image_raw将图像数据传递给节点/feature_tracker;
2、节点/feature_tracker中的订阅者sub_img收到图像，进行处理；
3、图像处理后的数据，使用节点/feature_tracker中的发布者pub_match、pub_image、pub_restart将数据分别经话题/feature_tracker/feature_img、/feature_tracker/feature、/feature_tracker/restart发送给后端节点/vins_estimator;
1、接收从bagfile经话题/cam/image_raw发送来的图像数据const sensor_msgs::ImageConstPtr &img_msg；
2、 调用回调函数img_callback()处理图像;
注：ros中图像格式不同与opencv中图像格式，需要进行转换；http://wiki./cv_bridge/Tutorials
2.1、sensor_msgs::ImageConstPtr数据格式
std_msgs/Header header #头信息uint32 height # image height, number of rowsuint32 width # image width, number of columnsstring encoding # Encoding of pixels -- channel meaning, ordering, size # taken from the list of strings in include/sensor_msgs/image_encodings.huint8 is_bigendian #大端big endian(从低地址到高地址的顺序存放数据的高位字节到低位字节)和小端little endianuint32 step # Full row length in bytesuint8[] data # actual matrix data, size is (step * rows)
2.2、std_msgs/Header.msg
一般在Image/PointCloud/IMU等各种传感器数据结构中都会出现的头信息；
uint32 seq # sequence ID: consecutively increasing ID time stamp #时间戳 string frame_id #坐标系ID
2.3、ros中图像数据与opencv中图像数据转换
实现：sensor_msgs::ImageConstPtr—> sensor_msgs::Imageàcv::Mat；
sub_img回调函数入参const sensor_msgs::ImageConstPtr &img_msg
//把ros图像指针数据转换成ros图像数据
// sensor_msgs::ImageConstPtr -》sensor_msgs::Image
cv_bridge::CvImageConstPtr ptr;if (img_msg->encoding == "8UC1"){ sensor_msgs::Image img; img.header = img_msg->header; img.height = img_msg->height; img.width = img_msg->width; img.is_bigendian = img_msg->is_bigendian; img.step = img_msg->step; img.data = img_msg->data; img.encoding = "mono8";
// sensor_msgs::Image—>cv::Mat ptr = cv_bridge::toCvCopy(img,sensor_msgs::image_encodings::MONO8);}else ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::MONO8);cv::Mat show_img = ptr->image;
类FeatureTracker：管理光流法追踪的特征点
cv::Mat mask;//掩码，用于做图像均一化使用
cv::Mat cur_img, forw_img; //上一帧图像，当前帧图像
vector<cv::Point2f> n_pts; //最大特征点-当前帧已经提取特征点：剩余需要提取的特征点
vector<cv::Point2f> cur_pts, forw_pts;//上一帧特征点，当前帧特征点
vector<cv::Point2f> prev_un_pts, cur_un_pts;//上一帧特征点去畸变相机归一化坐标，
vector<cv::Point2f> pts_velocity; //点x和y方向运动速度 vector<int> ids;//特征点id
vector<int> track_cnt;//对应特征点跟踪次数
map<int, cv::Point2f> cur_un_pts_map;//上一帧图像特征点map,id->坐标的map
map<int, cv::Point2f> prev_un_pts_map;camodocal::CameraPtr m_camera;double cur_time;
使用FeatureTracker trackerData对象
3.1根据该对象读取图像追踪特征点，获得特征点数据
in[1] 图像；
in[2] 图像时间戳；
trackerData[i].readImage(ptr->image.rowRange(ROW * i, ROW * (i + 1)), img_msg->header.stamp.toSec());
3.2、图像预处理，是否进行图像敏感度均衡化还是采用源图像
cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));clahe->apply(_img, img);
3.3、光流法跟踪，并根据特征点状态status[i]和是否在图像范围删减
cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, cv::Size(21, 21), 3);
3.4、根据特征点状态，删除未被连续跟踪的特征点信息
reduceVector(prev_pts, status); // 没用到reduceVector(cur_pts, status);reduceVector(forw_pts, status);reduceVector(ids, status); // 特征点的idreduceVector(cur_un_pts, status); // 去畸变后的坐标reduceVector(track_cnt, status); // 追踪次数
注：vector<int> track_cnt;//存储的是最新一直被连续跟踪的特征次数
在光流法跟踪以后，缩减掉未能连续跟踪的特征，剩余的是最新再一次跟踪的特征，所以每个值加1；
3.5、根据图像接收频率，决定当前处理图像特征结果是否发送到后端
// frequency control// 控制一下发给后端的频率// PUB_THIS_FRAME 发送标志位if (round(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time)) <= FREQ) // 保证发给后端的不超过这个频率{ PUB_THIS_FRAME = true; // reset the frequency control // 这段时间的频率和预设频率十分接近，就认为这段时间很棒，重启一下，避免delta t太大 if (abs(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time) - FREQ) < 0.01 * FREQ) { first_image_time = img_msg->header.stamp.toSec(); pub_count = 0; }}else PUB_THIS_FRAME = false;
若向后端发送特征数据，则需利用对极约束去除outliers，若不够max_cnt数量，需要增加特征点，以便后面发布数据；
3.6、利用虚拟相机将两个图像特征点转化到相机坐标系
使用opencv接口，剔除outliers，status保留标志位，删除原始特征点中outliers；
cv::findFundamentalMat(un_cur_pts, un_forw_pts, cv::FM_RANSAC, F_THRESHOLD, 0.99, status);
3.7、给现有的特征点设置mask，实现特征点均匀化
vector<pair<int, pair<cv::Point2f, int>>> cnt_pts_id;// 特征点构成的向量，成员是map<特征点被跟踪次数， <特征点坐标， 特征点id>>// 利用光流特点，追踪多的稳定性好，排前面,对cnt_pts_id排序； mask = cv::Mat(ROW, COL, CV_8UC1, cv::Scalar(255));//与图像大小一致； for (auto &it : cnt_pts_id) { if (mask.at<uchar>(it.second.first) == 255) { // 把挑选剩下的特征点重新放进容器 forw_pts.push_back(it.second.first); ids.push_back(it.second.second); track_cnt.push_back(it.first); // opencv函数，把周围一个圆内全部置0,这个区域不允许别的特征点存在，避免特征点过于集中 cv::circle(mask, it.second.first, MIN_DIST, 0, -1); }}
从被跟踪次数最多的特征点开始遍历，最先遍历的特征点mask中对应像素为255，所以加入均一化点集中，并以该点为圆心的院内所有像素值都变成0，因此这些点无法在计入到均一化点集中；
3.8、当前帧提取特征数量要求max_cnt,已经提取特征forw_pts.size()，因此还需要在提取一部分
cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.01, MIN_DIST, mask);
//将n_pts加入到forw_pts中，对应id为-1，追踪次数为1；
当前帧所有点统一去畸变，同时计算特征点速度，用来后续时间戳标定
void FeatureTracker::undistortedPoints()// PinholeCamera::liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) // https://blog.csdn.net/u010848251/article/details/118212562
