VINS-Mono源码分析1— feature_tracker(视觉前端)

ros::init(argc, argv, "feature_tracker");
ros::NodeHandle n("~");
ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);

feature_tracker包的程序就是从这里开始一步一步往下执行的，这里是程序的入口。基本上所有ROS程序开始都有的东西。

readParameters(n);

它的功能就是读取参数文件的参数，默认情况下参数文件是～/VINS-Mono/config/euroc文件夹下的euroc_config.yaml文件。

std::string config_file;
config_file = readParam<std::string>(n, "config_file");

这段代码在parameters.cpp的readParameters(ros::NodeHandle &n) 函数下，readParam默认读取的对象是～/VINS-Mono/vins_estimator/launch文件夹下的euroc.launch文件的内容

for (int i = 0; i < NUM_OF_CAM; i++)
    trackerData[i].readIntrinsicParameter(CAM_NAMES[i]);

读取相机参数，这部分具体读取参数的代码在camera_model包下的PinholeCamera.cc文件中，如下所示

PinholeCamera::Parameters::readFromYamlFile(const std::string& filename)
{
    cv::FileStorage fs(filename, cv::FileStorage::READ);
    [此处略去部分代码]
    m_modelType = PINHOLE;
    fs["camera_name"] >> m_cameraName;
    m_imageWidth = static_cast<int>(fs["image_width"]);
    m_imageHeight = static_cast<int>(fs["image_height"]);

    cv::FileNode n = fs["distortion_parameters"];
    m_k1 = static_cast<double>(n["k1"]);
    m_k2 = static_cast<double>(n["k2"]);
    m_p1 = static_cast<double>(n["p1"]);
    m_p2 = static_cast<double>(n["p2"]);

    n = fs["projection_parameters"];
    m_fx = static_cast<double>(n["fx"]);
    m_fy = static_cast<double>(n["fy"]);
    m_cx = static_cast<double>(n["cx"]);
    m_cy = static_cast<double>(n["cy"]);

    return true;
}

上面的代码就是读取相机参数的详情。

if(FISHEYE)
    {
        ......
    }

默认针孔相机模型,这部分代码不执行，故不做解释。

ros::Subscriber sub_img = n.subscribe(IMAGE_TOPIC, 100, img_callback);

pub_img = n.advertise<sensor_msgs::PointCloud>("feature", 1000);
pub_match = n.advertise<sensor_msgs::Image>("feature_img",1000);
pub_restart = n.advertise<std_msgs::Bool>("restart",1000);
ros::spin();

订阅视觉话题IMAGE_TOPIC的消息，默认情况下它代表着一些特定的.bag文件发布的话题消息，比如MH_05_difficult.bag文件发布的话题"/cam0/image_raw"。定义pub_img、pub_match、pub_restart话题。
接下来执行回调函数img_callback()

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

如果是第一帧图像则执行上面代码，得到时间戳。不是第一帧图像不执行。

if (img_msg->header.stamp.toSec() - last_image_time > 1.0 || img_msg->header.stamp.toSec() < last_image_time)
{
    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;
}

判断图像帧在时间上是否连续，如果连续，则跳过此段代码，继续往下执行。如果不连续，则重新初始化。

if (round(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time)) <= FREQ)
{
    PUB_THIS_FRAME = true;
    // reset the frequency control
    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;

发布图像帧的频率控制，默认情况下发布频率为10，如果当前帧的发布频率小于10且非常接近10，则将当前帧的时间戳设为频率控制的起始时间。

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

将ROS格式下的图像通过桥梁指针ptr转换为OpenCV格式下的图像。

for (int i = 0; i < NUM_OF_CAM; i++)
{
    ROS_DEBUG("processing camera %d", i);
    if (i != 1 || !STEREO_TRACK)
        trackerData[i].readImage(ptr->image.rowRange(ROW * i, ROW * (i + 1)), img_msg->header.stamp.toSec());
    else
    {
        if (EQUALIZE)
        {
            cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE();
            clahe->apply(ptr->image.rowRange(ROW * i, ROW * (i + 1)), trackerData[i].cur_img);
        }
        else
            trackerData[i].cur_img = ptr->image.rowRange(ROW * i, ROW * (i + 1));
    }

#if SHOW_UNDISTORTION
    trackerData[i].showUndistortion("undistrotion_" + std::to_string(i));
#endif
}

因为是单目相机，所以NUM_OF_CAM是1。默认情况下上面这段代码只执行了readImage() 函数，此函数是feature_tracker包的核心函数，必须重点分析。

if (EQUALIZE)
{
    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;

判断是否对图像进行自适应直方图均衡化，默认情况下EQUALIZE值为1。

if (forw_img.empty())
{
    prev_img = cur_img = forw_img = img;
}
else
{
    forw_img = img;
}

如果是第一帧图像则执行if里的代码，后续帧执行else的代码。

forw_pts.clear();

当前帧的光流点先清空处理。

if (cur_pts.size() > 0)
{
    TicToc t_o;
    vector<uchar> status;
    vector<float> err;
    cv::calcOpticalFlowPyrLK(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++)
        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);
    reduceVector(cur_un_pts, status);
    reduceVector(track_cnt, status);
    ROS_DEBUG("temporal optical flow costs: %fms", t_o.toc());
}

如果是第一帧图像，则这段代码是不执行的。
cur_img的光流点cur_pts通过基于图像金字塔的光流法得到forw_img的光流点forw_pts。然后判断光流点forw_pts是否在图像边界内。最后将状态status[i] = 0的光流点forw_pts及其相关量通过reduceVector()函数全部剔除掉。

for (auto &n : track_cnt)
    n++;

光流点forw_pts追踪成功的,其被跟踪的次数就加1，n就是代表被追踪的次数大小。

rejectWithF();

void FeatureTracker::rejectWithF()
{
    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;
            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;
        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());
    }
}

这个函数实现的功能是:通过基础矩阵F来进一步剔除光流点forw_pts中的外点。其具体实现可解释为，先把光流点cur_pts的像素坐标，通过liftProjective()函数转换为无畸变的归一化相机坐标tmp_p，然后再将tmp_p转回到无畸变的像素坐标，得到un_cur_pts。光流点forw_pts的操作相同。再然后通过OpenCV的库函数计算F矩阵并且得到外点，最后用reduceVector()函数去除外点。

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);
            cv::circle(mask, it.second.first, MIN_DIST, 0, -1);
        }
    }
}

对光流点forw_pts按着跟踪的次数大小track_cnt重新排序，然后在mask中将光流点forw_pts周围半径为MIN_DIST的区域设置为0，角点提取时不再在这些区域内提取，这样可以使光流点不会过于集中。

int n_max_cnt = MAX_CNT - static_cast<int>(forw_pts.size());
if (n_max_cnt > 0)
{
    ......
    cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.01, MIN_DIST, mask);
}
else
    n_pts.clear();

每帧需要MAX_CNT个角点，当前有forw_pts.size()个角点，所以需要提取MAX_CNT - forw_pts.size()个角点。它不仅支持Harris角点检测，也支持Shi Tomasi算法的角点检测。

addPoints();

void FeatureTracker::addPoints()
{
    for (auto &p : n_pts)
    {
        forw_pts.push_back(p);
        ids.push_back(-1);
        track_cnt.push_back(1);
    }
}

将新提取出来的角点n_pts添加到当前光流点forw_pts中，并更新ids和track_cnt。

prev_img = cur_img;
prev_pts = cur_pts;
prev_un_pts = cur_un_pts;
cur_img = forw_img;
cur_pts = forw_pts;
undistortedPoints();
prev_time = cur_time;

参数的传递。

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

得到光流点cur_pts的归一化相机坐标系下的去畸变光流点cur_un_pts，注意此时的cur_pts是由刚刚的光流点forw_pts传递后得到的。更新光流速度pts_velocity。

for (unsigned int i = 0;; i++)
{
    bool completed = false;
    for (int j = 0; j < NUM_OF_CAM; j++)
        if (j != 1 || !STEREO_TRACK)
            completed |= trackerData[j].updateID(i);
    if (!completed)
        break;
}

接着看feature_tracker_node.cpp文件中的img_callback()函数。上面代码主要是updateID()函数，它的功能是更新光流点cur_pts的ids。

bool FeatureTracker::updateID(unsigned int i)
{
    if (i < ids.size())
    {
        if (ids[i] == -1)
            ids[i] = n_id++;
        return true;
    }
    else
        return false;
}

ids的更新主要靠n_id，n_id从程序一开始执行就不断累加，以保证光流点ID的唯一性。

if (PUB_THIS_FRAME)
{
     ......
}

接下来分析这个if语句里面的代码。

pub_count++;
sensor_msgs::PointCloudPtr feature_points(new sensor_msgs::PointCloud);
sensor_msgs::ChannelFloat32 id_of_point;
sensor_msgs::ChannelFloat32 u_of_point;
sensor_msgs::ChannelFloat32 v_of_point;
sensor_msgs::ChannelFloat32 velocity_x_of_point;
sensor_msgs::ChannelFloat32 velocity_y_of_point;

feature_points->header = img_msg->header;
feature_points->header.frame_id = "world";

vector<set<int>> hash_ids(NUM_OF_CAM);
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;
    auto &pts_velocity = trackerData[i].pts_velocity;
    for (unsigned int j = 0; j < ids.size(); j++)
    {
        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;

            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());
// skip the first image; since no optical speed on frist image
if (!init_pub)
{
    init_pub = 1;
}
else
    pub_img.publish(feature_points);

将光流点的cur_pts的像素坐标、光流点的cur_un_pts的归一化去畸变相机坐标以及光流点的速度pts_velocity打包，构造为ROS消息feature_points，通过话题pub_img将其发送出去。

if (SHOW_TRACK)
{
     ptr = cv_bridge::cvtColor(ptr, sensor_msgs::image_encodings::BGR8);
     //cv::Mat stereo_img(ROW * NUM_OF_CAM, COL, CV_8UC3);
     cv::Mat stereo_img = ptr->image;

     for (int i = 0; i < NUM_OF_CAM; i++)
     {
         cv::Mat tmp_img = stereo_img.rowRange(i * ROW, (i + 1) * ROW);
         cv::cvtColor(show_img, tmp_img, CV_GRAY2RGB);

         for (unsigned int j = 0; j < trackerData[i].cur_pts.size(); j++)
         {
             double len = std::min(1.0, 1.0 * trackerData[i].track_cnt[j] / WINDOW_SIZE);
             cv::circle(tmp_img, trackerData[i].cur_pts[j], 2, cv::Scalar(255 * (1 - len), 0, 255 * len), 2); 
         }
     }
     pub_match.publish(ptr->toImageMsg());
}

将光流点画在图像tmp_img上，也就是ptr->image上，最后把ROS消息ptr->toImageMsg() 通过话题pub_match发布出去。