VINS-Mono代码学习记录（二）---feature_tracker

客官°小女子只卖身不卖艺 2023-05-30 08:52 73阅读 0赞

一、feature_tracker总体流程图解

先给出一张feature_tracker的总体流程图，让自己有一个全局意识。这张图真好，感谢原作者。基本这就是feature_tracker这个node搞的事情啦！重点关注它，对! 就是它！
在这里插入图片描述图片转自：https://blog.csdn.net/qq\_41839222/article/details/85797156

二、main()函数分析

开始进入feature_tracker–>src文件夹可以看到，里面的文件共6个，先看feature_tracker_node.cpp中的主函数main()

  1. int main(int argc, char **argv)
  2. {
  3.     //【1】初始化，设置句柄，设置logger级别
  4.     ros::init(argc, argv, "feature_tracker");
  5.     ros::NodeHandle n("~");
  6.     ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
  8.     //【2】从config-->euroc-->euroc_config.yaml文件中读取参数，包括话题，图像宽高,追踪的特征点数量等等
  9.     readParameters(n);
  11.     //【3】循环读取每个相机的内参
  12.     for (int i = 0; i < NUM_OF_CAM; i++) //NUM_OF_CAM=1
  13.         trackerData[i].readIntrinsicParameter(CAM_NAMES[i]);
  15.     //【4】如果是鱼眼相机，添加mask去除边缘噪声
  16.     if(FISHEYE)
  17.     {
  18.         for (int i = 0; i < NUM_OF_CAM; i++)
  19.         {
  20.             trackerData[i].fisheye_mask = cv::imread(FISHEYE_MASK, 0);
  21.             if(!trackerData[i].fisheye_mask.data)
  22.             {
  23.                 ROS_INFO("load mask fail");
  24.                 ROS_BREAK();
  25.             }
  26.             else
  27.                 ROS_INFO("load mask success");
  28.         }
  29.     }
  31.     //【5】订阅话题IMAGE_TOPIC(/cam0/image_raw)，执行回调函数img_callback
  32.     ros::Subscriber sub_img = n.subscribe(IMAGE_TOPIC, 100, img_callback);//当订阅的topic有新的信息时，激活回调函数
  34.     //【6】发布feature，实例feature_points，跟踪的特征点，给后端优化用
  35.     //发布feature_img，实例ptr，跟踪的特征点图，给RVIZ用和调试用
  36.     //发布restart
  37.     pub_img = n.advertise<sensor_msgs::PointCloud>("feature", 1000);
  38.     pub_match = n.advertise<sensor_msgs::Image>("feature_img",1000);
  39.     pub_restart = n.advertise<std_msgs::Bool>("restart",1000);
  40.     /*
  41.     if (SHOW_TRACK)
  42.         cv::namedWindow("vis", cv::WINDOW_NORMAL);
  43.     */
  44.     ros::spin();//ros::spin() 在调用后不会再返回，也就是主程序到这儿就不往下执行了
  45.     return 0;
  46. }

上面每一步都给出了详细的注释，其中步骤【2】读取参数的函数readParameters()如下:

  1. void readParameters(ros::NodeHandle &n)
  2. {
  3.     std::string config_file;
  4.     config_file = readParam<std::string>(n, "config_file");//ans-->config_file
  5.     std::cout<<"config_file"<<config_file<<endl;//自己加的，为了查看config_file的具体值
  6.     cv::FileStorage fsSettings(config_file, cv::FileStorage::READ);
  7.     if(!fsSettings.isOpened())
  8.     {
  9.         std::cerr << "ERROR: Wrong path to settings" << std::endl;
  10.     }
  11.     std::string VINS_FOLDER_PATH = readParam<std::string>(n, "vins_folder");
  13.     fsSettings["image_topic"] >> IMAGE_TOPIC; //image_topic: "/cam0/image_raw"
  14.     fsSettings["imu_topic"] >> IMU_TOPIC; //imu_topic: "/imu0"
  15.     MAX_CNT = fsSettings["max_cnt"];//tracking最大特征点数量150
  16.     MIN_DIST = fsSettings["min_dist"];//两个特征点之间的最小距离30
  17.     ROW = fsSettings["image_height"];//480
  18.     COL = fsSettings["image_width"];//752
  19.     FREQ = fsSettings["freq"];//publish tracking result 的频率,默认设置为10hz
  20.     F_THRESHOLD = fsSettings["F_threshold"];//ransac threshold (pixel)值为1
  21.     SHOW_TRACK = fsSettings["show_track"];//发布image topic 值为1
  22.     EQUALIZE = fsSettings["equalize"];//if image is too dark or light, trun on equalize to find enough features,默认为1
  23.     FISHEYE = fsSettings["fisheye"];//默认不用fisheye 默认值为0
  24.     if (FISHEYE == 1)
  25.         FISHEYE_MASK = VINS_FOLDER_PATH + "config/fisheye_mask.jpg";
  26.     CAM_NAMES.push_back(config_file);//string类型的config_file push到CAM_NAMES vector中
  28.     WINDOW_SIZE = 20;
  29.     STEREO_TRACK = false;//如果双目 把这个参数改为true
  30.     FOCAL_LENGTH = 460;
  31.     PUB_THIS_FRAME = false;
  33.     if (FREQ == 0)
  34.         FREQ = 100;
  36.     fsSettings.release();
  39. }

代码中涉及到的变量基本根据变量名理解即可。
步骤【3】读取相机内参，根据config_file文件中model_type的值决定采用何种相机模型，并创建相应模型的对象指针，读取文件在camera_models–>CameraFctory.cc文件中。
步骤【5】和【6】发布和订阅的topic图形表示如下：
在这里插入图片描述在上一节给出的是整个算法中的所有nodes和topics，这里就单独把feature_tracker这个node和相关的topic拿出来了。

三、img_callback()回调函数

接下来看img_callback()回调函数：

  1. void img_callback(const sensor_msgs::ImageConstPtr &img_msg)
  2. {
  3.     //[1]判断是否是第一帧
  4.     if(first_image_flag)//首先是true
  5.     {
  6.         first_image_flag = false;
  7.         first_image_time = img_msg->header.stamp.toSec();
  8.         last_image_time = img_msg->header.stamp.toSec();
  9.         return;
  10.     }
  11.     //[2] 判断时间间隔是否正确，有问题则restart
  12.     if (img_msg->header.stamp.toSec() - last_image_time > 1.0 || img_msg->header.stamp.toSec() < last_image_time)
  13.     {
  14.         ROS_WARN("image discontinue! reset the feature tracker!");
  15.         first_image_flag = true; 
  16.         last_image_time = 0;
  17.         pub_count = 1;
  18.         std_msgs::Bool restart_flag;
  19.         restart_flag.data = true;
  20.         pub_restart.publish(restart_flag);
  21.         return;
  22.     }
  23.     last_image_time = img_msg->header.stamp.toSec();
  25.     //[3] 发布频率控制，并不是每读入一帧图像，就要发布特征点，通过判断间隔时间内的发布次数
  26.     if (round(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time)) <= FREQ)//FREQ=10hz
  27.     {
  28.         PUB_THIS_FRAME = true;
  29.         // reset the frequency control,pub_count=1  时间间隔内的发布频率十分接近设定频率时，更新时间间隔起始时刻，并将数据发布次数置0
  30.         if (abs(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time) - FREQ) < 0.01 * FREQ)
  31.         {
  32.             first_image_time = img_msg->header.stamp.toSec();
  33.             pub_count = 0;
  34.         }
  35.     }
  36.     else
  37.         PUB_THIS_FRAME = false;
  39.     //[4]将图像编码8UC1转换为mono8
  40.     cv_bridge::CvImageConstPtr ptr;
  41.     if (img_msg->encoding == "8UC1")
  42.     {
  43.         sensor_msgs::Image img;
  44.         img.header = img_msg->header;
  45.         img.height = img_msg->height;
  46.         img.width = img_msg->width;
  47.         img.is_bigendian = img_msg->is_bigendian;
  48.         img.step = img_msg->step;
  49.         img.data = img_msg->data;
  50.         img.encoding = "mono8";
  51.         ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::MONO8);
  52.     }
  53.     else
  54.         ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::MONO8);
  56.     cv::Mat show_img = ptr->image;
  59.     TicToc t_r;
  60.     for (int i = 0; i < NUM_OF_CAM; i++)  //NUM_OF_CAM默认为1
  61.     {
  62.         ROS_DEBUG("processing camera %d", i);
  63.         //[5]单目时：FeatureTracker::readImage() 函数读取图像数据进行处理
  64.         if (i != 1 || !STEREO_TRACK) //STEREO_TRACK默认为false
  65.             trackerData[i].readImage(ptr->image.rowRange(ROW * i, ROW * (i + 1)), img_msg->header.stamp.toSec());//ROW为图像的height
  66.         else
  67.         {
  68.             if (EQUALIZE)//默认为1,均衡化
  69.             {
  70.                 cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE();//直方图均衡化
  71.                 clahe->apply(ptr->image.rowRange(ROW * i, ROW * (i + 1)), trackerData[i].cur_img);//cur_img是光流跟踪的前一帧的图像数据
  72.             }
  73.             else
  74.                 trackerData[i].cur_img = ptr->image.rowRange(ROW * i, ROW * (i + 1));
  75.         }
  77. #if SHOW_UNDISTORTION
  78.         trackerData[i].showUndistortion("undistrotion_" + std::to_string(i));
  79. #endif
  80.     }
  82.     //【6】更新全局ID
  83.     for (unsigned int i = 0;; i++)
  84.     {
  85.         bool completed = false;
  86.         for (int j = 0; j < NUM_OF_CAM; j++)
  87.             if (j != 1 || !STEREO_TRACK)
  88.                 completed |= trackerData[j].updateID(i);
  89.         if (!completed)
  90.             break;
  91.     }
  94. //[7]如果PUB_THIS_FRAME=1则进行发布
  95.    if (PUB_THIS_FRAME)
  96.    {
  97.         //将特征点id，矫正后归一化平面的3D点(x,y,z=1)，像素2D点(u,v)，像素的速度(vx,vy)，封装成sensor_msgs::PointCloudPtr类型的feature_points实例中,发布到pub_img;
  98.         pub_count++;
  99.         sensor_msgs::PointCloudPtr feature_points(new sensor_msgs::PointCloud);
  100.         sensor_msgs::ChannelFloat32 id_of_point;
  101.         sensor_msgs::ChannelFloat32 u_of_point;
  102.         sensor_msgs::ChannelFloat32 v_of_point;
  103.         sensor_msgs::ChannelFloat32 velocity_x_of_point;
  104.         sensor_msgs::ChannelFloat32 velocity_y_of_point;
  106.         feature_points->header = img_msg->header;
  107.         feature_points->header.frame_id = "world";
  109.         vector<set<int>> hash_ids(NUM_OF_CAM);
  110.         for (int i = 0; i < NUM_OF_CAM; i++)
  111.         {
  112.             auto &un_pts = trackerData[i].cur_un_pts;
  113.             auto &cur_pts = trackerData[i].cur_pts;
  114.             auto &ids = trackerData[i].ids;
  115.             auto &pts_velocity = trackerData[i].pts_velocity;
  116.             for (unsigned int j = 0; j < ids.size(); j++)
  117.             {
  118.                 if (trackerData[i].track_cnt[j] > 1)
  119.                 {
  120.                     int p_id = ids[j];
  121.                     hash_ids[i].insert(p_id);
  122.                     geometry_msgs::Point32 p;
  123.                     p.x = un_pts[j].x;
  124.                     p.y = un_pts[j].y;
  125.                     p.z = 1;
  127.                     feature_points->points.push_back(p);
  128.                     id_of_point.values.push_back(p_id * NUM_OF_CAM + i);
  129.                     u_of_point.values.push_back(cur_pts[j].x);
  130.                     v_of_point.values.push_back(cur_pts[j].y);
  131.                     velocity_x_of_point.values.push_back(pts_velocity[j].x);
  132.                     velocity_y_of_point.values.push_back(pts_velocity[j].y);
  133.                 }
  134.             }
  135.         }
  136.         feature_points->channels.push_back(id_of_point);
  137.         feature_points->channels.push_back(u_of_point);
  138.         feature_points->channels.push_back(v_of_point);
  139.         feature_points->channels.push_back(velocity_x_of_point);
  140.         feature_points->channels.push_back(velocity_y_of_point);
  141.         ROS_DEBUG("publish %f, at %f", feature_points->header.stamp.toSec(), ros::Time::now().toSec());
  142.         // skip the first image; since no optical speed on frist image
  143.         if (!init_pub)
  144.         {
  145.             init_pub = 1;
  146.         }
  147.         else
  148.             pub_img.publish(feature_points);
  150.        //将图像封装到cv_bridge::cvtColor类型的ptr实例中发布到pub_match;
  151.         if (SHOW_TRACK)
  152.         {
  153.             ptr = cv_bridge::cvtColor(ptr, sensor_msgs::image_encodings::BGR8);
  154.             //cv::Mat stereo_img(ROW * NUM_OF_CAM, COL, CV_8UC3);
  155.             cv::Mat stereo_img = ptr->image;
  157.             for (int i = 0; i < NUM_OF_CAM; i++)
  158.             {
  159.                 cv::Mat tmp_img = stereo_img.rowRange(i * ROW, (i + 1) * ROW);//Mat的rowRange和colRange函数可以获取某些范围内行或列的指针
  160.                 cv::cvtColor(show_img, tmp_img, CV_GRAY2RGB);
  162.                 for (unsigned int j = 0; j < trackerData[i].cur_pts.size(); j++)
  163.                 {
  164.                     double len = std::min(1.0, 1.0 * trackerData[i].track_cnt[j] / WINDOW_SIZE);
  165.                     cv::circle(tmp_img, trackerData[i].cur_pts[j], 2, cv::Scalar(255 * (1 - len), 0, 255 * len), 2);
  166.                     //draw speed line
  167.                     /*
  168.                     Vector2d tmp_cur_un_pts (trackerData[i].cur_un_pts[j].x, trackerData[i].cur_un_pts[j].y);
  169.                     Vector2d tmp_pts_velocity (trackerData[i].pts_velocity[j].x, trackerData[i].pts_velocity[j].y);
  170.                     Vector3d tmp_prev_un_pts;
  171.                     tmp_prev_un_pts.head(2) = tmp_cur_un_pts - 0.10 * tmp_pts_velocity;
  172.                     tmp_prev_un_pts.z() = 1;
  173.                     Vector2d tmp_prev_uv;
  174.                     trackerData[i].m_camera->spaceToPlane(tmp_prev_un_pts, tmp_prev_uv);
  175.                     cv::line(tmp_img, trackerData[i].cur_pts[j], cv::Point2f(tmp_prev_uv.x(), tmp_prev_uv.y()), cv::Scalar(255 , 0, 0), 1 , 8, 0);
  176.                     */
  177.                     //char name[10];
  178.                     //sprintf(name, "%d", trackerData[i].ids[j]);
  179.                     //cv::putText(tmp_img, name, trackerData[i].cur_pts[j], cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 0, 0));
  180.                 }
  181.             }
  182.             //cv::imshow("vis", stereo_img);
  183.             //cv::waitKey(5);
  184.             pub_match.publish(ptr->toImageMsg());
  185.         }
  186.     }
  187.     ROS_INFO("whole feature tracker processing costs: %f", t_r.toc());
  188. }

接下来剩下的内容就是feature_track.h变量的认识以及feature_track.cpp中几个主要函数的介绍，包括
void readImage(const cv::Mat &_img,double _cur_time)，void rejectWithF()等函数。下一节再见吧，朋友们！！！

发表评论

表情：
评论列表 （有 0 条评论，73人围观）

还没有评论，来说两句吧...

相关阅读

    相关 暑假学习记录（二）

    转眼已经回家一个礼拜了，我开始学习安装hadoop，放假之前听说hadoopp安装挺难的，但我还是鼓足了勇气去安装。 最开始也想按着教程来装，但是也遇到了许多的疑惑，于是我就

    一时失言乱红尘一时失言乱红尘/ 0 赞/ 113 阅读

    相关 VINS-Mono代码学习记录（一）

    写在前面的话 看代码真是一个令人头疼的过程，由于代码量很大，而脑子又不够使，看了总是忘，忘了又接着看，所以还是决定写博客来记录一下这个过程，也方便自己日后回忆是咋学的，哈

    亦凉亦凉/ 0 赞/ 126 阅读