VINS-mono学习笔记：1.视觉前端

视觉前端

有关于vins-mono的前端特征提取的时序详解见：【Vins-mono一】vins-mono的前端特征提取_vins-mono的特征点提取_a_happy_bird的博客-CSDN博客

概述：

• 对于第一帧，还没有特征点，因此不进行光流，先进行特征点提取，提取出MAX_CNT个特征点，刚提取出的特征点id都是-1，通过updateID()赋予真正id
• 对于第二帧及以后的帧，上一帧已存在特征点因此用光流法进行追踪，但由于会通过status以及本质矩阵剔除一部分特征点，因此当前特征点个数会小于MAX_CNT，因此还要在提取MAX_CNT-n个特征点，同样刚提取出的特征点id都是-1，通过updateID()赋予真正id

1. 配置feature_tracker节点

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); // 读取配置文件

    for (int i = 0; i < NUM_OF_CAM; i++)
        trackerData[i].readIntrinsicParameter(CAM_NAMES[i]);    // 获得每个相机的内参

    if(FISHEYE)
    {
       ...
    }

    // 这个向roscore注册订阅这个topic，收到一次message就执行一次回调函数
    ros::Subscriber sub_img = n.subscribe(IMAGE_TOPIC, 100, img_callback);
    // 注册一些publisher
    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);
    /*
    if (SHOW_TRACK)
        cv::namedWindow("vis", cv::WINDOW_NORMAL);
    */
    ros::spin();    // spin代表这个节点开始循环查询topic是否接收
    return 0;
}

1.1 读取配置文件

void readParameters(ros::NodeHandle &n)
{
    std::string config_file;
    // 首先获得配置文件的路径
    config_file = readParam<std::string>(n, "config_file");
    // 使用opencv的yaml文件接口来读取文件
    cv::FileStorage fsSettings(config_file, cv::FileStorage::READ);
    if(!fsSettings.isOpened())
    {
        std::cerr << "ERROR: Wrong path to settings" << std::endl;
    }
    std::string VINS_FOLDER_PATH = readParam<std::string>(n, "vins_folder");

    fsSettings["image_topic"] >> IMAGE_TOPIC;
    fsSettings["imu_topic"] >> IMU_TOPIC;
    MAX_CNT = fsSettings["max_cnt"];
    MIN_DIST = fsSettings["min_dist"];
    ROW = fsSettings["image_height"];
    COL = fsSettings["image_width"];
    FREQ = fsSettings["freq"];
    F_THRESHOLD = fsSettings["F_threshold"];
    SHOW_TRACK = fsSettings["show_track"];
    EQUALIZE = fsSettings["equalize"];  // 是否做均衡化处理
    FISHEYE = fsSettings["fisheye"];
    if (FISHEYE == 1)
        FISHEYE_MASK = VINS_FOLDER_PATH + "config/fisheye_mask.jpg";
    CAM_NAMES.push_back(config_file);

    WINDOW_SIZE = 20;
    STEREO_TRACK = false;
    FOCAL_LENGTH = 460;
    PUB_THIS_FRAME = false;

    if (FREQ == 0)
        FREQ = 100;

    fsSettings.release();
}

config_file的值体现在launch文件中，以euroc.launch为例：

<launch>
    <arg name="config_path" default = "$(find feature_tracker)/../config/euroc/euroc_config.yaml" />
	  <arg name="vins_path" default = "$(find feature_tracker)/../config/../" />
    
    <node name="feature_tracker" pkg="feature_tracker" type="feature_tracker" output="log">
        <param name="config_file" type="string" value="$(arg config_path)" />   //此部分体现了feature_tracker中的config_file的值
        <param name="vins_folder" type="string" value="$(arg vins_path)" />
    </node>

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

在euroc_config.yaml文件中可以详见各个参数的细节：

#camera calibration 
model_type: PINHOLE
camera_name: camera
image_width: 752
image_height: 480
distortion_parameters:
   k1: -2.917e-01
   k2: 8.228e-02
   p1: 5.333e-05
   p2: -1.578e-04
projection_parameters:
   fx: 4.616e+02
   fy: 4.603e+02
   cx: 3.630e+02
   cy: 2.481e+02

#feature traker paprameters
max_cnt: 150            # max feature number in feature tracking
min_dist: 30            # min distance between two features 
freq: 10                # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image 
F_threshold: 1.0        # ransac threshold (pixel)
show_track: 1           # publish tracking image as topic
equalize: 1             # if image is too dark or light, trun on equalize to find enough features
fisheye: 0              # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points

1.2 配置相机内参与畸变系数

根据相机类型将配置文件当中的内参矩阵与畸变系数读取进来

void FeatureTracker::readIntrinsicParameter(const string &calib_file)
{
    ROS_INFO("reading paramerter of camera %s", calib_file.c_str());
    // 读到的相机内参赋给m_camera
    m_camera = CameraFactory::instance()->generateCameraFromYamlFile(calib_file);
}

CameraFactory::generateCameraFromYamlFile(const std::string& filename)
{
    cv::FileStorage fs(filename, cv::FileStorage::READ);

    Camera::ModelType modelType = Camera::MEI;
    // 通过配置文件获取相机的模型

    switch (modelType)
    {
    case Camera::KANNALA_BRANDT:
  
    case Camera::PINHOLE: //根据针孔相机模型读取配置文件中的内参矩阵与畸变系数
    {
        PinholeCameraPtr camera(new PinholeCamera);

        PinholeCamera::Parameters params = camera->getParameters();
        params.readFromYamlFile(filename);
        camera->setParameters(params);
        return camera;
    }
    case Camera::SCARAMUZZA:
  
    case Camera::MEI:
 
    }

    return CameraPtr();
}

1.3 img_callback回调函数

1. 对第一帧图像的操作

   if(first_image_flag) // 对第一帧图像的基本操作
       {
           first_image_flag = false;
           first_image_time = img_msg->header.stamp.toSec();
           last_image_time = img_msg->header.stamp.toSec();
           return;
       }
   

2. 检查时间戳是否正常

   // 检查时间戳是否正常，这里认为超过一秒或者错乱就异常
       // 图像时间差太多光流追踪就会失败，这里没有描述子匹配，因此对时间戳要求就高
       if (img_msg->header.stamp.toSec() - last_image_time > 1.0 || img_msg->header.stamp.toSec() < last_image_time)
       {
           // 一些常规的reset操作
           ROS_WARN("image discontinue! reset the feature tracker!");
           first_image_flag = true; 
           last_image_time = 0;
           pub_count = 1;
           std_msgs::Bool restart_flag;
           restart_flag.data = true;
           pub_restart.publish(restart_flag);  // 告诉其他模块要重启了
           return;
       }
   

3. 控制发送给后端的频率

   // 控制一下发给后端的频率
       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;
   

4. 将ros message转成cv::Mat

      cv_bridge::CvImageConstPtr ptr;
       // 把ros message转成cv::Mat
       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";
           ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::MONO8);
       }
       else
           ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::MONO8);
   

5. readImage进行光流追踪，然后更新id

6. 向后端发送数据：发布像素坐标、去畸变的归一化相机坐标系、id、归一化坐标下的速度四个特征点信息

   if (PUB_THIS_FRAME)
      {
           for (int i = 0; i < NUM_OF_CAM; i++)
           {
               auto &un_pts = trackerData[i].cur_un_pts;   // 去畸变的归一化相机坐标系
               auto &cur_pts = trackerData[i].cur_pts; // 像素坐标
               auto &ids = trackerData[i].ids; // id
               auto &pts_velocity = trackerData[i].pts_velocity;   // 归一化坐标下的速度
               for (unsigned int j = 0; j < ids.size(); j++)
               {
                   // 只发布追踪大于1的，因为等于1没法构成重投影约束，也没法三角化
                   if (trackerData[i].track_cnt[j] > 1)
                   {
                       int p_id = ids[j];
                       hash_ids[i].insert(p_id);   // 这个并没有用到
                       geometry_msgs::Point32 p;
                       p.x = un_pts[j].x;
                       p.y = un_pts[j].y;
                       p.z = 1;
                       // 利用这个ros消息的格式进行信息存储
                       feature_points->points.push_back(p);
                       id_of_point.values.push_back(p_id * NUM_OF_CAM + i);
                       u_of_point.values.push_back(cur_pts[j].x);
                       v_of_point.values.push_back(cur_pts[j].y);
                       velocity_x_of_point.values.push_back(pts_velocity[j].x);
                       velocity_y_of_point.values.push_back(pts_velocity[j].y);
                   }
               }
           }
           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);
           ROS_DEBUG("publish %f, at %f", feature_points->header.stamp.toSec(), ros::Time::now().toSec());
   

7. 可视化操作

2. 光流追踪readImage

2.0 光流追踪 VS 描述子匹配

• 在帧间匹配时，光流无论是从耗时还是鲁棒性来讲，都要略优于描述子匹配，但如果出现特征点被短时遮挡也会造成光利追踪失败
• 在全局一致性的判断以及重定位、回环检测时，描述子匹配有着光流追踪无法替代的作用

2.1 图像均衡化预处理

   if (EQUALIZE)
    {
        // 图像太暗或者太亮，提特征点比较难，所以均衡化一下
        // ! opencv 函数看一下
        cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));
        TicToc t_c;
        clahe->apply(_img, img);
        ROS_DEBUG("CLAHE costs: %fms", t_c.toc());
    }
    else
        img = _img;

2.2 光流追踪

    if (cur_pts.size() > 0) // 上一帧有特征点，就可以进行光流追踪了
    {
        TicToc t_o;
        vector<uchar> status;
        vector<float> err;
        // 调用opencv函数进行光流追踪
        // Step 1 通过opencv光流追踪给的状态位剔除outlier
        cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, cv::Size(21, 21), 3);
        //cur_img上一帧图像 forw_img当前帧图像 cur_pts上一帧特征点 forw_pts当前帧光流追踪到的特征点 status表示特征点是否追踪成功 err表示追踪误差 cv::Size(21, 21)表示图像尺寸 3表示金字塔层级

        for (int i = 0; i < int(forw_pts.size()); i++)
            // Step 2 通过图像边界剔除outlier
            if (status[i] && !inBorder(forw_pts[i]))    // 追踪状态好检查在不在图像范围
                status[i] = 0;
        //双指针法去除跟踪失败的特征点，减小特征点信息向量的维度
        reduceVector(prev_pts, status); // 没用到
        reduceVector(cur_pts, status);
        reduceVector(forw_pts, status);
        reduceVector(ids, status);  // 特征点的id
        reduceVector(cur_un_pts, status);   // 去畸变后的坐标
        reduceVector(track_cnt, status);    // 追踪次数
        ROS_DEBUG("temporal optical flow costs: %fms", t_o.toc());
    }
    // 被追踪到的是上一帧就存在的，因此追踪数+1
    for (auto &n : track_cnt)
        n++;

    if (PUB_THIS_FRAME)
    {
        // Step 3 通过对级约束来剔除outlier
        rejectWithF();
        ROS_DEBUG("set mask begins");
        TicToc t_m;
        // Step 4 均匀化特征点
        setMask();
        ROS_DEBUG("set mask costs %fms", t_m.toc());

rejectWithF()计算本质矩阵进一步剔除跟踪外点：相邻帧之间同一点的本质矩阵值不应过大，超过阈值就剔除

void FeatureTracker::rejectWithF()
{
    // 当前被追踪到的光流至少8个点
    if (forw_pts.size() >= 8)
    {
        ROS_DEBUG("FM ransac begins");
        TicToc t_f;
        vector<cv::Point2f> un_cur_pts(cur_pts.size()), un_forw_pts(forw_pts.size());
        for (unsigned int i = 0; i < cur_pts.size(); i++)
        {
            Eigen::Vector3d tmp_p;
            // 得到相机归一化坐标系的值
            // 这里用一个虚拟相机，原因同样参考https://github.com/HKUST-Aerial-Robotics/VINS-Mono/issues/48
            // 这里有个好处就是对F_THRESHOLD和相机无关
            // 投影到虚拟相机的像素坐标系
            // 上一帧特征点投影到虚拟相机的像素坐标系
            m_camera->liftProjective(Eigen::Vector2d(cur_pts[i].x, cur_pts[i].y), tmp_p);
            tmp_p.x() = FOCAL_LENGTH * tmp_p.x() / tmp_p.z() + COL / 2.0;
            tmp_p.y() = FOCAL_LENGTH * tmp_p.y() / tmp_p.z() + ROW / 2.0;
            un_cur_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y());

            // 当前帧特征点投影到虚拟相机的像素坐标系
            m_camera->liftProjective(Eigen::Vector2d(forw_pts[i].x, forw_pts[i].y), tmp_p);
            tmp_p.x() = FOCAL_LENGTH * tmp_p.x() / tmp_p.z() + COL / 2.0;
            tmp_p.y() = FOCAL_LENGTH * tmp_p.y() / tmp_p.z() + ROW / 2.0;
            un_forw_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y());
        }

        vector<uchar> status;
        // opencv接口计算本质矩阵，某种意义也是一种对级约束的outlier剔除
        cv::findFundamentalMat(un_cur_pts, un_forw_pts, cv::FM_RANSAC, F_THRESHOLD, 0.99, status);
        int size_a = cur_pts.size();
        reduceVector(prev_pts, status);
        reduceVector(cur_pts, status);
        reduceVector(forw_pts, status);
        reduceVector(cur_un_pts, status);
        reduceVector(ids, status);
        reduceVector(track_cnt, status);
        ROS_DEBUG("FM ransac: %d -> %lu: %f", size_a, forw_pts.size(), 1.0 * forw_pts.size() / size_a);
        ROS_DEBUG("FM ransac costs: %fms", t_f.toc());
    }
}

setMask()追踪到的特征点要进行均匀化：就是以当前特征点为中心，以一定半径画圆，让这个圆形区域内只有这一个特征点

void FeatureTracker::setMask()
{
    if(FISHEYE)
        mask = fisheye_mask.clone();
    else
        mask = cv::Mat(ROW, COL, CV_8UC1, cv::Scalar(255));
    

    // prefer to keep features that are tracked for long time
    vector<pair<int, pair<cv::Point2f, int>>> cnt_pts_id;

    for (unsigned int i = 0; i < forw_pts.size(); i++)
        cnt_pts_id.push_back(make_pair(track_cnt[i], make_pair(forw_pts[i], ids[i])));
    // 利用光流特点，追踪多的稳定性好，排前面
    sort(cnt_pts_id.begin(), cnt_pts_id.end(), [](const pair<int, pair<cv::Point2f, int>> &a, const pair<int, pair<cv::Point2f, int>> &b)
         {
            return a.first > b.first;
         });

    forw_pts.clear();
    ids.clear();
    track_cnt.clear();

    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);
        }
    }
}

2.3 特征点数小于阈值时再次提取特征点

        ROS_DEBUG("detect feature begins");
        TicToc t_t;
        int n_max_cnt = MAX_CNT - static_cast<int>(forw_pts.size());
        if (n_max_cnt > 0)
        {
            if(mask.empty())
                cout << "mask is empty " << endl;
            if (mask.type() != CV_8UC1)
                cout << "mask type wrong " << endl;
            if (mask.size() != forw_img.size())
                cout << "wrong size " << endl;
            // 只有发布才可以提取更多特征点，同时避免提的点进mask
            // 会不会这些点集中？会，不过没关系，他们下一次作为老将就得接受均匀化的洗礼
            cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.01, MIN_DIST, mask);
        }
        else
            n_pts.clear();
        ROS_DEBUG("detect feature costs: %fms", t_t.toc());

        ROS_DEBUG("add feature begins");
        TicToc t_a;
        addPoints(); // 把新提取的特征点加入到当前特征点容器中
        ROS_DEBUG("selectFeature costs: %fms", t_a.toc());

2.4 所有特征点去畸变，并计算特征点速度

// 当前帧所有点统一去畸变，同时计算特征点速度，用来后续时间戳标定
void FeatureTracker::undistortedPoints()
{
    cur_un_pts.clear();
    cur_un_pts_map.clear();
    //cv::undistortPoints(cur_pts, un_pts, K, cv::Mat());
    for (unsigned int i = 0; i < cur_pts.size(); i++)
    {
        // 有的之前去过畸变了，这里连同新人重新做一次
        Eigen::Vector2d a(cur_pts[i].x, cur_pts[i].y);
        Eigen::Vector3d b;
        m_camera->liftProjective(a, b);
        cur_un_pts.push_back(cv::Point2f(b.x() / b.z(), b.y() / b.z()));
        // id->坐标的map
        cur_un_pts_map.insert(make_pair(ids[i], cv::Point2f(b.x() / b.z(), b.y() / b.z())));
        //printf("cur pts id %d %f %f", ids[i], cur_un_pts[i].x, cur_un_pts[i].y);
    }
    // caculate points velocity
    if (!prev_un_pts_map.empty())
    {
        double dt = cur_time - prev_time;
        pts_velocity.clear();
        for (unsigned int i = 0; i < cur_un_pts.size(); i++)
        {
            if (ids[i] != -1)
            {
                std::map<int, cv::Point2f>::iterator it;
                it = prev_un_pts_map.find(ids[i]);
                // 找到同一个特征点
                if (it != prev_un_pts_map.end())
                {
                    double v_x = (cur_un_pts[i].x - it->second.x) / dt;
                    double v_y = (cur_un_pts[i].y - it->second.y) / dt;
                    // 得到在归一化平面的速度
                    pts_velocity.push_back(cv::Point2f(v_x, v_y));
                }
                else
                    pts_velocity.push_back(cv::Point2f(0, 0));
            }
            else
            {
                pts_velocity.push_back(cv::Point2f(0, 0));
            }
        }
    }
    else
    {
        // 第一帧的情况
        for (unsigned int i = 0; i < cur_pts.size(); i++)
        {
            pts_velocity.push_back(cv::Point2f(0, 0));
        }
    }
    prev_un_pts_map = cur_un_pts_map;
}

3. 去畸变

void PinholeCamera::liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const
{
    double mx_d, my_d,mx2_d, mxy_d, my2_d, mx_u, my_u;
    double rho2_d, rho4_d, radDist_d, Dx_d, Dy_d, inv_denom_d;
    //double lambda;

    // Lift points to normalised plane
    // 投影到归一化相机坐标系
    mx_d = m_inv_K11 * p(0) + m_inv_K13;
    my_d = m_inv_K22 * p(1) + m_inv_K23;

    if (m_noDistortion)
    {
        mx_u = mx_d;
        my_u = my_d;
    }
    else
    {
        if (0)
        {
           ...
        }
        else
        // 参考https://github.com/HKUST-Aerial-Robotics/VINS-Mono/issues/48
        {
            // Recursive distortion model
            int n = 8; // 迭代8次
            Eigen::Vector2d d_u;
            // 这里mx_d + du = 畸变后
            distortion(Eigen::Vector2d(mx_d, my_d), d_u); // d_u就是畸变量：delta BB`
            // Approximate value
            mx_u = mx_d - d_u(0); // 方向相反因此要减
            my_u = my_d - d_u(1);

            for (int i = 1; i < n; ++i)
            {
                distortion(Eigen::Vector2d(mx_u, my_u), d_u);
                mx_u = mx_d - d_u(0);
                my_u = my_d - d_u(1);
            }
        }
    }

    // Obtain a projective ray
    P << mx_u, my_u, 1.0;
}

on model
int n = 8; // 迭代8次
Eigen::Vector2d d_u;
// 这里mx_d + du = 畸变后
distortion(Eigen::Vector2d(mx_d, my_d), d_u); // d_u就是畸变量：delta BB`
// Approximate value
mx_u = mx_d - d_u(0); // 方向相反因此要减
my_u = my_d - d_u(1);

        for (int i = 1; i < n; ++i)
        {
            distortion(Eigen::Vector2d(mx_u, my_u), d_u);
            mx_u = mx_d - d_u(0);
            my_u = my_d - d_u(1);
        }
    }
}

// Obtain a projective ray
P << mx_u, my_u, 1.0;

}