目录

前言

feature_tracker_node详解

准备工作

img_callback()详解

---

前言

        在vins前端中主要包含图像光流追踪和imu预积分两部分。光流追踪主要是为了实现追踪相邻两帧图像的相同地图点信息，以供后端求解两帧图像之间的位姿变换。对于相邻两帧图像求得匹配的地图点有两种方案，一种是进行特征点提取，然后根据描述子进行特征匹配；另一种就是光流追踪法，基于灰度不变假设通过求解特征点的运动速度进而估计匹配的特征点像素坐标。在前端feature_tracker_node节点中，主要实现了该功能，还包含了一些其他的细节问题。在vins-fusion中，光流追踪放到了和后端优化同一个节点中。本篇内容详细介绍一下在前端光流追踪节点vins-mono都做了哪些工作。

feature_tracker_node详解

准备工作

         1.读取参数

        在前端中包含很多vins-mono的系统参数，这些参数是通过读取参数文件获得的。前端主要包含的参数如下面代码所示：

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;

        从上到下依次是原始图像话题名称、imu数据话题名、提取特征点的最大个数、提取特征点的最小像素距离、图像的高度和宽度、前端发布的频率、ransac除杂的像素阈值、是否显示轨迹、是否进行直方图均衡标志位、鱼眼相机标志位。后面其他参数直接在程序里面设置，比较重要的是FOCAL_LENGTH=460，这是虚拟焦距，具体作用在后面有解释。

        2.生成相机模型

        通过trackerData[i].readIntrinsicParameter(CAM_NAMES[i])生成一个相机模型，先从参数文件读取相机类型，vins-mono中是针孔模型。然后再将参数文件中的相机参数赋给该相机模型，该过程在函数CameraPtr
CameraFactory::generateCameraFromYamlFile(const std::string& filename)完成。此外，该函数内还包含camera->setParameters(params)函数，主要是记录一些中间变量，这些中间变量是为了去畸变部分使用。

img_callback()详解

        该部分是前端处理的主要函数，实现的功能是计算出特征点的相关信息。

• 第一帧图像进来，存储第一帧信息，不进行其他处理；
• 判断图像时间流是否正常；
• 控制发布频率小于100Hz；
• 将ros格式图像转化成opencv格式；
•  进入光流追踪及特征点相关信息计算，trackerData[i].readImage(ptr->image.rowRange(ROW * i, ROW * (i + 1)), img_msg->header.stamp.toSec())；

        该函数第一个参数是确定图像的行的范围，这是为了双目做准备，对于单目相机i就是0，第二个参数是图像的时间戳。函数具体实现的功能下面详细介绍：

• 自适应图像直方图均衡

        该部分主要是为了解决在过亮或过暗情况下特征点提取困难问题。然而，在vins-fusion版本中，该功能被作者去掉了，可能是对于光流追踪的实际效果增益不大另外比较耗时。

• 第一帧图像，直接进行特征点提取，第二帧进行光流追踪，追踪后重新提取额外的特征点，并去除外点，对相关容器进行瘦身

        特征点提取函数是cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.01, MIN_DIST, mask)，这个函数是在当前图像特征点基础上额外提取MAX_CNT - forw_pts.size()个特征点。当图像是第一帧时，PUB_THIS_FRAME标志位一定是true，程序运行直接提取MAX_CNT个，这个参数也是在参数文件中读取到的。

        如果不是第一帧图像，那么需要进行光流追踪获得追踪到的特征点，光流追踪函数在 cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, cv::Size(21, 21), 3)中完成，该函数参数列表为：上一帧图像（入参）、当前帧图像（入参）、上一帧特征点（入参）、追踪到的特征点（出参）、状态位（出参，表示上一帧特征点在当前帧是否追踪成功），其他参数应用意义不大，这里不进行介绍，详细可以看官网。

        接下来是对一些容器进行“瘦身”，具体作用是将没有跟踪成功的特征点在一些相关容器中更新。这些容器分别是倒数第三帧图像的特征点、上一帧特征点、当前帧特征点、上一帧图像的特征点ID、上一帧去畸变的特征点、上一帧特征点跟踪数量容器。之后要将剩余的track_cnt里面的所有数值都加1，表示该索引对应的特征点追踪次数加1。

 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);
 for (auto &n : track_cnt)//更新当前特征点被追踪到的次数
        n++;

         这个“瘦身”函数实现是用双指针实现的，把status里面为0的索引在v中相同的索引清除掉。status的值为0有两种情况，要么该特征点没有跟踪到，要么追踪到的点在图像边界外。

void reduceVector(vector<cv::Point2f> &v, vector<uchar> status)
{
    int j = 0;
    for (int i = 0; i < int(v.size()); i++)
        if (status[i])
            v[j++] = v[i];
    v.resize(j);
}

         如果图像的频率满足发布的频率，那么先通过基础矩阵去除外点，再重新提取新的特征点。最后将新提取的特征点id置为-1。

        根据基础矩阵去除外点，即rejectWithF()函数，这个函数先把前一帧图像的特征点和当前帧图像特征点去畸变后得到相机归一化平面上的点，再根据虚拟焦距恢复成像素平面的点，然后利用cv::findFundamentalMat(un_cur_pts, un_forw_pts, cv::FM_RANSAC, F_THRESHOLD, 0.99, status)函数计算出外点，同样根据status对相关容器进行“瘦身”操作。这里计算基础矩阵完全是为了求出外点，而不是求位姿。另外虚拟焦距能够对所有相机一视同仁，当特征点转换到相机归一化平面后，那么得到像素坐标的计算尺度（焦距）都是一样的，使用虚拟焦距的好处在于不会因为不同相机的分辨率不同导致像素尺度大小不一，该好处在后面也会有所提及。

        去除外点后，使用setMask()函数，主要是为了提取特征点能够比较均匀，具体做法就是先根据特征点被追踪的次数把相关容器进行排序，然后把已经提取特征点的部分在一张空白图像上以特征点像素坐标为圆心，以MIN_DIS为半径画黑色的圆，作为下一次特征点提取的掩膜。这样做的确会保证特征点提取比较均匀，在orb中采用的是四叉树方法保证提取特征点的均匀性，从直观上比较，vins中这种使用掩膜进行均匀化比四叉树实现更加简洁，但是同时也更加暴力，可能在特征比较差的地方提取出质量差的特征点。个人认为这里可以借鉴四叉树的方法进行修改。

        最后，由于cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.01, MIN_DIST, mask)函数在最新帧图像上提取了新的特征点，所以需要将这些特征点添加到相关容器里面，在addPoints()函数中主要是把新提取的特征点添加到最新帧的特征点容器中，特征点id置为-1，追踪次数置为0。

 if (PUB_THIS_FRAME)//图像帧频率小于100hz
    {
        rejectWithF();//通过基础矩阵去除外点
        ROS_DEBUG("set mask begins");
        TicToc t_m;
        setMask();
        ROS_DEBUG("set mask costs %fms", t_m.toc());

        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;
                //0.01表示最差特征点得分不能小于最好的得分的0.01倍
            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());
    }

• 去畸变并计算特征点速度

        在前端光流追踪节点中，最后一个算法是将当前帧特征点去畸变后计算出特征点在归一化平面的速度。在vins中去畸变并没有直接调用opencv的去畸变的库函数，而是利用畸变的特性使用迭代的方式完成。

        去畸变过程如上图所示，第一张图的A点表示准确的地图点对应的归一化平面坐标，A'点代表发生畸变后的归一化坐标的位置。至于为啥畸变会向着图像中心移动，原因是和相机的特性有关：slam多采用广角镜头，而广角镜头多产生桶形畸变，所以特征点畸变后会向内移动。

        处理畸变是一个迭代过程，首先让带有畸变的A'点根据畸变公式进行畸变，畸变公式如slam14讲所述：

        

        

计算畸变是一个正向过程，很容易实现，通过上式计算可以得到在A'位置处经过畸变后的坐标为B'。根据畸变特性：越远离中心畸变越严重，可知BB'的长度肯定小于AA',那么将BB'叠加到A'上，那么可以得到图3中的C点此时完成了一次迭代，更新后的C坐标更靠近真值A。然后C再去畸变，得到的点为C'，可知，BB'<CC'<AA'，把得到的CC'叠加到A'上，这样就更靠近真值，如此迭代8次基本就得到了真值。

        注意：这种迭代的做法有一个前提——桶形畸变，该类型畸变效果是边缘处发生畸变后的点向内收缩，如果是枕形畸变，或者说畸变后的点向外扩散那么此种方法不适用，迭代会发散。在这个原理基础上是否可以经过修改达到去枕形畸变效果，笔者经过尝试未能成功，如果有佬能解决这个问题，欢迎评论区交流！

        去畸变技巧性比较强，理解了原理代码也就好理解了。在实际代码中，传入参数是一个二维点坐标，就是像素坐标，输出的是去畸变后的归一化平面坐标。具体代码如下：

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)
        {
            double k1 = mParameters.k1();
            double k2 = mParameters.k2();
            double p1 = mParameters.p1();
            double p2 = mParameters.p2();

            // Apply inverse distortion model
            // proposed by Heikkila
            mx2_d = mx_d*mx_d;
            my2_d = my_d*my_d;
            mxy_d = mx_d*my_d;
            rho2_d = mx2_d+my2_d;
            rho4_d = rho2_d*rho2_d;
            radDist_d = k1*rho2_d+k2*rho4_d;
            Dx_d = mx_d*radDist_d + p2*(rho2_d+2*mx2_d) + 2*p1*mxy_d;
            Dy_d = my_d*radDist_d + p1*(rho2_d+2*my2_d) + 2*p2*mxy_d;
            inv_denom_d = 1/(1+4*k1*rho2_d+6*k2*rho4_d+8*p1*my_d+8*p2*mx_d);

            mx_u = mx_d - inv_denom_d*Dx_d;
            my_u = my_d - inv_denom_d*Dy_d;
        }
        else
        {
            // Recursive distortion model
            int n = 8;
            Eigen::Vector2d d_u;
            distortion(Eigen::Vector2d(mx_d, my_d), d_u);
            // 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;
}

        去畸变以后进行计算特征点速度，前端光流的所有信息就已经准备好，可以发往后端。计算速度比较简单，就是像素位置差除以时间差。

        在更新特征点id后就把归一化坐标、像素坐标、特征点id和特征点速度发出。更新id是把新提取的特征点按照顺序继续赋id。发到后端归一化坐标进行位姿估计，而像素坐标和特征点速度是为了估计td使用的。

        发布数据的程序代码如下：

   if (PUB_THIS_FRAME)
   {
        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)  //跟踪次数大于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);

        至此，前端光流追踪节点的任务就完成了，整个流程比较简单。最后对前端光流进行一下扩展：首先是光流法和特征点+描述子方法的对比，光流法的确会比描述子进行匹配要快一些，不过也十分依赖光度不变假设，而且需要相邻两帧图像运动不大，不然很容易追踪失败。对于想用vins-mono框架进行实际开发的，可以考虑实际场景需求，选择是否更改前端提取追踪特征点的方式。

        然后就是前面提到的四叉树均匀化和掩膜均匀化问题，vins的方案是把特征点按照追踪次数进行排序，然后把追踪次数多的画实心圆，保证下次提取不在以特征点为中心的一个范围内进行。这样做对于特征点较少的情况相对不合理，有点圆外的区域特征差也要强制提的感觉，相比之下，四叉树是提完了之后进行剔除，貌似更合理。

        还有就是在成员变量中有一些变量是没有用到的，比如prev相关的只有计算速度时用到一个容器，其他都没有用到，可以去除。

        以上就是关于vins-mono前端光流部分，下一篇文章将详细介绍关于前端的另外一项重要工作，imu预积分。