vins_mono-前端feature_tracker节点

vins_mono系统启动

01

roslaunch启动命令

roslaunch vins_estimator euroc.launch

roslaunch vins_estimator vins_rviz.launch

rosbag play YOUR_PATH/MH_01_easy.bag

02

roslaunch语法

使用roslaunch命令通过launch文件，同时启动多个节点;

roslaunch  pkg_name  name.launch

03

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>

04

rosbag命令

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；

feature_tracker前端节点

VINS_MONO共有9个package，前端特征追踪节点feature_tracker对应于feature_tracker包中的feature_tracker_node.cpp，即该节点主函数；

01

节点主函数

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;

02

订阅者sub_img

1、接收从bagfile经话题/cam/image_raw发送来的图像数据const sensor_msgs::ImageConstPtr &img_msg；

2、 调用回调函数img_callback()处理图像;

注：ros中图像格式不同与opencv中图像格式，需要进行转换；http://wiki.ros.org/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;

03

类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；

3.9、去畸变

当前帧所有点统一去畸变，同时计算特征点速度，用来后续时间戳标定

void FeatureTracker::undistortedPoints()// PinholeCamera::liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) // https://blog.csdn.net/u010848251/article/details/118212562