VINS-Mono+Fusion源码解析系列（二）：视觉前端的图像处理

1. 图片的回调函数(img_callback)

（1）大致流程

• 检查时间戳是否异常：图像时间间隔超过1s，或者时间差错乱：当前帧时间戳 < 上一帧时间戳

• 检查频率：控制向后端发送频率，以及运行时频率隐性异常处理

• 读取图片并进行相应处理（readImage）：光流追踪 + 去畸变 + 均匀化等

• 更新特征点ID（updateID）

• 向后端发送特征点信息：特征点的id + 去畸变后的像素坐标 + 特征点的速度

（2） 整体代码

// /feature_tracker/src/feature_tracker_node.cpp
// 图片的回调函数    传入的参数为图像的消息类型
void img_callback(const sensor_msgs::ImageConstPtr &img_msg)
{
    // Step 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;
    }
    
    // 检查时间戳是否正常，这里认为超过一秒或者错乱就异常
    // 图像时间差太多光流追踪就会失败，这里没有描述子匹配，因此对时间戳要求就高
    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;
    }
    
    last_image_time = img_msg->header.stamp.toSec();
/*
  Step 2: 检查频率
  (a) 控制向后端发送的频率
  因为如果送给后端太频繁，后端运算时间会跟不上，可能会崩掉
  pub_count表示发送帧数  img_msg->header.stamp.toSec()-first_image_time表示发送的时间间隔    
  发送帧数 / 时间间隔 = 频率  下面的判断条件是保证后端发送的频率不超过所设的FREQ
*/
    if (round(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time)) <= FREQ)  
    {
        // 满足频率要求才将发布标志位置为true，不满足频率要求的帧就不发布  
        PUB_THIS_FRAME = true;  

        // (b) 运行时的频率隐性异常  需要及时清空，下面做进一步解释
        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;

    cv_bridge::CvImageConstPtr ptr;
    // Step 3: 通过cv_bridge把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);

    TicToc t_r;
    // Step 5: readImage读取图片，进行光流追踪与去畸变操作
    // 单目情况下，NUM_OF_CAM=1
    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));
        }
    }
    
    for (unsigned int i = 0;; i++)
    {
        bool completed = false;
        for (int j = 0; j < NUM_OF_CAM; j++)
            if (j != 1 || !STEREO_TRACK)
                // updateID中是对新提取到特征点的id进行更新
                completed |= trackerData[j].updateID(i);    
        if (!completed)
            break;
    }
    
   // 若需要向后端发送数据，则要进行如下操作
   if (PUB_THIS_FRAME)
   {
        pub_count++;    // 计数器更新
        // 定义发送的数据类型
        sensor_msgs::PointCloudPtr feature_points(new sensor_msgs::PointCloud);  
        // 定义要发送的信息
        sensor_msgs::ChannelFloat32 id_of_point;  // 特征点的id
        sensor_msgs::ChannelFloat32 u_of_point;   // 特征点的x像素坐标
        sensor_msgs::ChannelFloat32 v_of_point;   // 特征点的y像素坐标
        sensor_msgs::ChannelFloat32 velocity_x_of_point;  // 特征点的x方向速度
        sensor_msgs::ChannelFloat32 velocity_y_of_point;  // 特征点的y方向速度

        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++)  // 单目相机情况下，NUM_OF_CAM=1
       {
            // 向后端发送的内容
            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);  // 特征点的id
                    // 像素坐标
                    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);
                }
            }
       }
       
        // 最终发送给后端的是：特征点的id，去畸变后的像素坐标，以及特征点的速度
        // 去畸变后特征点的归一化坐标个人认为是没有发送给后端的，要确认一下
        // 若真的没有发送给后端，那for循环中为什么要有归一化坐标    疑问？？？
        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);
        // skip the first image; since no optical speed on frist image
        if (!init_pub)
        {
            init_pub = 1;
        }
        else
            pub_img.publish(feature_points);    // 调用接口，将feature_points发送给后端
   }
}

(4) 运行时的频率隐性异常

​ 频率计算公式如下：
$$freq=\frac{frame\_count}{\Delta t}$$
​ 随着程序的运行，帧数frame_count和时间差$\Delta t$都会增加。假设预设的频率阈值FREQ=10Hz，当frame_count=900，$\Delta t=100$时，$freq=9Hz<10Hz$，满足要求。之前1s内收到30帧图像，若此时出现异常，1s内收到50帧图像，则频率$freq=950/101<10Hz$，显示仍然满足频率阈值要求，但实际上此时已经出现了异常。

​ 这里的解决办法是：当实际频率freq和预设频率阈值FREQ很接近时，将frame_count和$\Delta t$都清空掉。此时若出现上述异常，则频率计算为：$freq=50Hz>10Hz$，不满足频率阈值要求。

（5）对新提取到的特征点被追踪的次数进行更新updateID

/**
 * @brief 
 * 
 * @param[in] i 
 * @return true 
 * @return false 
 *  给新的特征点赋上id,越界就返回false
 */
bool FeatureTracker::updateID(unsigned int i)
{
    if (i < ids.size())
    {
        if (ids[i] == -1)
            // n_id初始值为-1, 表示当前特征点为新特征点, 因此特征点id从0开始,
            // 之后每来一个特征点, 都是根据n_id来更新其id
            ids[i] = n_id++;
        return true;
    }
    else
        return false;
}

2. readImage

（1）readImage大致流程

• 图像均衡化处理：防止图像亮暗影响光流追踪
• 光流追踪：调用opencv接口对上一帧的特征点进行光流追踪
• 根据光流追踪的状态位status和追踪点的坐标是否在图像范围内（inBorder）对特征点进行剔除
• 若向后端发布特征点，则需要依次执行如下操作：
  • 通过对极约束来剔除outlier
  • 通过在提取到的特征点周围设置mask对其进行均匀化处理
  • 通过opencv提取剩余特征点
• undistortedPoints：通过逐次逼近法对特征点进行去畸变，并计算特征点速度

（3）readImage代码实现

// /feature_tracker/src/feature_tracker.cpp
/**
 * @brief 
 * 
 * @param[in] _img 输入图像
 * @param[in] _cur_time 图像的时间戳
 * 1、图像均衡化预处理
 * 2、光流追踪
 * 3、提取新的特征点（如果发布）
 * 4、所有特征点去畸变，计算速度
 * 5、外点剔除情况：光流追踪后剔除一次外点，若要向后端发送，则根据对极约束再剔除一次外点
 */
void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time)
{
    cv::Mat img;
    TicToc t_r;
    cur_time = _cur_time;

    // Step 1: 图像均衡化处理
    if (EQUALIZE)
    {
        // 图像太暗或者太亮，提特征点比较难，所以均衡化一下
        cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));
        TicToc t_c;
        clahe->apply(_img, img);  // 通过均衡化得到img
        ROS_DEBUG("CLAHE costs: %fms", t_c.toc());
    }
    else
        img = _img;  // 若不进行均衡化，则直接赋给img

    // 获取相邻两帧图像及特征点, 这里forw表示当前, cur表示上一帧
    if (forw_img.empty())   // 第一次输入图像，prev_img为空
    {
        // 第一次输入图像时，均为当前帧
        prev_img = cur_img = forw_img = img;  
    }
    else
    {
        // 后面在输入图像时，对forw_img进行更新，所以forw_img表示当前帧
        // 而由于没有对cur_img进行更新，故cur_img表示上一帧
        forw_img = img;
    }

    // forw_pts中存放当前帧的特征点信息
    // 当前帧刚送进来时，还没有对其进行特征提取，所以存储当前帧特征点的forw_pts要清空
    // 后面提取到的当前帧特征点就存放在forw_pts中
    forw_pts.clear();

    // cur_pts中存放上一帧的特征点信息
    if (cur_pts.size() > 0) // 上一帧有特征点，就可以进行光流追踪了
    {
        TicToc t_o;
        vector<uchar> status;
        vector<float> err;
        // Step 2 调用opencv函数(带图像金字塔)进行光流追踪
        // cur_img: 上一帧图像  forw_img: 当前帧图像
        // cur_pts: 上一帧图像上的特征点  
        // forw_pts: 上一帧图像上的特征点通过光流追踪在当前图像上的特征点
        // status: 上一帧图像上的特征点是否被成功追踪到的状态位  
        cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, 
                                 status, err, cv::Size(21, 21), 3);

        // Step 3 根据状态位status和追踪点的坐标是否在图像范围内进行剔除
        for (int i = 0; i < int(forw_pts.size()); i++)
            if (status[i] && !inBorder(forw_pts[i]))    
                status[i] = 0;
   
/*
   根据状态位status，使用双指针去除outlier进行瘦身
   status中保存的是通过光流追踪得到的每个点的状态，追踪到的status为1，每追踪到的status为0
   reduceVector就是根据status的值，将status为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  
    // 这里n取的是引用，因此n自增后track_cnt中对应值也自增
    for (auto &n : track_cnt)
        n++;

    // 当向后端发布时(PUB_THIS_FRAME == true)，需要执行以下步骤：
    // 1. 通过对极约束来剔除outlier
    // 2. 对提取到的特征点进行均匀化处理
    // 3. 为防止提取到的特征点数目不够，均匀化处理后再提取剩余数目的特征点
    if (PUB_THIS_FRAME) 
    {
        // Step 4 通过对极约束来剔除outlier
        rejectWithF();
        ROS_DEBUG("set mask begins");
        TicToc t_m;
        // Step 5 进行特征点均匀化
        setMask();  
        ROS_DEBUG("set mask costs %fms", t_m.toc());

/*
  Step 6: 提取剩余特征点
  前面经过光流追踪、对极约束以及对特征点进行均匀化提取，可能会使得提取到的特征点不够
  下面提取剩余的特征点  MAX_CNT表示最多提取的特征点数，forw_pts.size()表示当前提取到的特征点数，二者之差表示还需要提取的特征点数
*/
        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;
/*
   通过opencv的cv::goodFeaturesToTrack()提取剩余特征点时，提取到的特征点可能比较集中，但没关系, 因为当来下一帧时，当前提取到的集中特征点还会进行上述均匀化处理
   另外，cv::goodFeaturesToTrack()提取剩余特征点时，由于设置了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;
        // 将新提取的特征点的相关信息(坐标、id、被追踪次数)添加到相应容器中
        addPoints();
        ROS_DEBUG("selectFeature costs: %fms", t_a.toc());
    }