VINS-Mono 代码详细解读——视觉跟踪 feature_trackers

原理

视觉：

1）提取Harris角点，KLT金字塔光流跟踪相邻帧；

2）2 维特征点先矫正为不失真的，然后在通过外点剔除后投影到一个单位球面上  ；

3）去除异常点：先进行F矩阵测试，通过RANSAC去除异常点；

4）关键帧选取：1、当前帧相对最近的关键帧的特征平均视差大于一个阈值就为关键帧（因为视差可以根据平移和旋转共同得到，而纯旋转则导致不能三角化成功，所以这一步需要IMU预积分进行补偿）2、当前帧跟踪到的特征点数量小于阈值视为关键帧；

代码

代码流程如下图：主要三个源程序，feature_tracker_node是特征跟踪线程的系统入口，feature_tracker是特征跟踪算法的具体实现，parameters是设备等参数的读取和存放。

一、feature_tracker_node.cpp

主要分为两部分：int main（）函数为程序入口，void img_callback（）为ROS的回调函数，对图像进行特征点追踪，处理和发布。

1、程序入口 int main()

int main(int argc, char **argv)
{
    //ros初始化和设置句柄
    ros::init(argc, argv, "feature_tracker");
    ros::NodeHandle n("~");// 命名空间为/node_namespace/node_name

    //设置logger的级别。 只有级别大于或等于level的日志记录消息才会得到处理。
    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
    
    // 1.读取yaml中的一些配置参数
    readParameters(n);

    // 2.读取每个相机实例对应的相机内参，NUM_OF_CAM 经常为1，单目
    for (int i = 0; i < NUM_OF_CAM; i++) 
        trackerData[i].readIntrinsicParameter(CAM_NAMES[i]);

    // 3.判断是否加入鱼眼mask来去除边缘噪声
    if(FISHEYE)
    {
        for (int i = 0; i < NUM_OF_CAM; i++)
        {
            trackerData[i].fisheye_mask = cv::imread(FISHEYE_MASK, 0);
            if(!trackerData[i].fisheye_mask.data)
            {
                ROS_INFO("load mask fail");
                ROS_BREAK();
            }
            else
                ROS_INFO("load mask success");
        }
    }

    // 4.订阅话题IMAGE_TOPIC(/cam0/image_raw),有图像发布到这个话题时，执行回调函数img_callback
    ros::Subscriber sub_img = n.subscribe(IMAGE_TOPIC, 100, img_callback);

    // 5.发布feature点云，实例feature_points，跟踪的特征点，给后端优化用
    pub_img = n.advertise<sensor_msgs::PointCloud>("feature", 1000);
    //发布feature_img，实例ptr，跟踪的特征点图，给RVIZ用和调试用
    pub_match = n.advertise<sensor_msgs::Image>("feature_img",1000);
    //发布restart
    pub_restart = n.advertise<std_msgs::Bool>("restart",1000);
    /*
    if (SHOW_TRACK)
        cv::namedWindow("vis", cv::WINDOW_NORMAL);
    */
    ros::spin();
    return 0;
}

1、其中，步骤1中读取配置参数，地址为config->euroc->euroc_config.yaml

2、步骤5中，三个发布的话题为下图所示，

feature_trackers是单独的一个模块，输入IMAGE_TOPIC(/cam0/image_raw)，输出三个话题topic：feature_img（跟踪的图像，之后RVIZ显示）、feature（跟踪的特征点信息，由/vins_estimator订阅并优化）、restart（特征跟踪出错，复位）

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数据结构：sensor_msgs::PointCloud、sensor_msgs::Image、std_msgs::Bool

sensor_msgs::PointCloud 点云

sensor_msgs::PointCloudPtr  feature_points(new sensor_msgs::PointCloud)

std_msgs/Header header #头信息
	#feature_points->header = img_msg->header;
	#feature_points->header.frame_id = "world";

geometry_msgs/Point32[] points #3D点(x,y,z)

sensor_msgs/ChannelFloat32[] channels  #[特征点的ID,像素坐标u,v,速度vx,vy]
	#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);
	#如img_msg->channels[0].values[i]表示第i个特征点的ID值

std_msgs/Header header #头信息
	#msg_match_points.header.stamp = ros::Time(time_stamp);

geometry_msgs/Point32[] points #3D点(x,y,z)

sensor_msgs/ChannelFloat32[] channels  #[重定位帧的平移向量T的x,y,z，旋转四元数w,x,y,z和索引值]
	#t_q_index.values.push_back(T.x());
	#t_q_index.values.push_back(T.y());
	#t_q_index.values.push_back(T.z());
	#t_q_index.values.push_back(Q.w());
	#t_q_index.values.push_back(Q.x());
	#t_q_index.values.push_back(Q.y());
	#t_q_index.values.push_back(Q.z());
	#t_q_index.values.push_back(index);
	#msg_match_points.channels.push_back(t_q_index);

sensor_msgs::Image

std_msgs/Header header #头信息
uint32 height         # image height, number of rows
uint32 width          # image width, number of columns
string encoding       # Encoding of pixels -- channel meaning, ordering, size
                      # taken from the list of strings in include/sensor_msgs/image_encodings.h
uint8 is_bigendian    #大端big endian(从低地址到高地址的顺序存放数据的高位字节到低位字节)和小端little endian
uint32 step           # Full row length in bytes
uint8[] data          # actual matrix data, size is (step * rows)

sensor_msgs::ImuConstPtr

Header header	#头信息

geometry_msgs/Quaternion orientation	#四元数[x,y,z,w]
float64[9] orientation_covariance		# Row major about x, y, z axes

geometry_msgs/Vector3 angular_velocity	#角速度[x,y,z]轴
float64[9] angular_velocity_covariance	# 对应协方差矩阵，Row major(行主序) about x, y, z axes

geometry_msgs/Vector3 linear_acceleration	#线性加速度[x,y,z]
float64[9] linear_acceleration_covariance	# 对应协方差矩阵 Row major x, y z 

2、回调函数 void img_callback(const sensor_msgs::ImageConstPtr &img_msg)

ROS的回调函数，主要功能包括：readImage()函数对新来的图像使用光流法进行特征点跟踪，并将追踪的特征点封装成feature_points发布到pub_img的话题下，将图像封装成ptr发布在pub_match下。

pub_img.publish(feature_points);

pub_match.publish(ptr->toImageMsg())

void img_callback(const sensor_msgs::ImageConstPtr &img_msg)
{
    // 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.通过时间间隔判断相机数据流是否稳定，有问题则restart
    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;
    }
    last_image_time = img_msg->header.stamp.toSec();// 更新上一帧图像时间戳

    // 3.发布频率控制，保证每秒钟处理的Image小于FREQ，频率控制在10HZ以内
    // 并不是每读入一帧图像，就要发布特征点
    // 判断间隔时间内的发布次数
    if (round(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time)) <= FREQ)
    {
        PUB_THIS_FRAME = true;// 发布当前帧
        // 时间间隔内的发布频率十分接近设定频率时，更新时间间隔起始时刻，并将数据发布次数置0
        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.将图像编码8UC1转换为mono8,单色8bit
    cv_bridge::CvImageConstPtr ptr;
    if (img_msg->encoding == "8UC1")
    {
        sensor_msgs::Image img;// ROS图像消息
        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";
        // cv_bridge的toCVCopy函数将ROS图像消息转化为OpenCV图像，
        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;
    TicToc t_r;
    // 5. 重要！！！trackerData[i].readImage读取图像数据
    for (int i = 0; i < NUM_OF_CAM; i++)
    {
        ROS_DEBUG("processing camera %d", i);
        if (i != 1 || !STEREO_TRACK)//单目
            //readImage()函数读取图像数据进行处理
            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
    }

    // 6.更新全局ID
    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);// 更新feature的id
        if (!completed)
            break;
    }

    // 7、将特征点id，矫正后归一化平面的3D点(x,y,z=1)，像素2D点(u,v)，像素的速度(vx,vy)，
    //封装成sensor_msgs::PointCloudPtr类型的feature_points实例中,发布到pub_img;
   if (PUB_THIS_FRAME)
   {
        pub_count++;
        // 重要！！！ feature_points
        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);// 哈希表id
        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);// 哈希表id  insert
            
                    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());
        
        // 第一帧不发布，因为没有光流速度
        if (!init_pub)
        {
            init_pub = 1;
        }
        else
            pub_img.publish(feature_points);

        // 8、将图像封装到cv_bridge::cvtColor类型的ptr实例中发布到pub_match
        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);// show_img灰度图转RGB（tmp_img）
                //显示追踪状态，越红越好，越蓝越不行
                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);
                    //draw speed line
                    /*
                    Vector2d tmp_cur_un_pts (trackerData[i].cur_un_pts[j].x, trackerData[i].cur_un_pts[j].y);
                    Vector2d tmp_pts_velocity (trackerData[i].pts_velocity[j].x, trackerData[i].pts_velocity[j].y);
                    Vector3d tmp_prev_un_pts;
                    tmp_prev_un_pts.head(2) = tmp_cur_un_pts - 0.10 * tmp_pts_velocity;
                    tmp_prev_un_pts.z() = 1;
                    Vector2d tmp_prev_uv;
                    trackerData[i].m_camera->spaceToPlane(tmp_prev_un_pts, tmp_prev_uv);
                    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);
                    */
                    //char name[10];
                    //sprintf(name, "%d", trackerData[i].ids[j]);
                    //cv::putText(tmp_img, name, trackerData[i].cur_pts[j], cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 0, 0));
                }
            }
            //cv::imshow("vis", stereo_img);
            //cv::waitKey(5);
            pub_match.publish(ptr->toImageMsg());
        }
    }
    ROS_INFO("whole feature tracker processing costs: %f", t_r.toc());
}

二、feature_tracker.cpp

void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time)

  主要分为两部分：

1、特征点检测：Frame1，goofFeaturesToTrack()检测MAX_CNT个特征点，添加到forw_pts中

2、特征点跟踪：calOpticalFlowPyrLK跟踪，将跟踪失败的点剔除，跟踪成功的跟踪次数+1，调用goodFeaturesToTracl（）再检测出MAX_CNT-forw_pts.size()个特征点，添加到forw_pts中，调用updateID 更新ids。

undistortedPoints()中cur_un_pts为归一化相机坐标系下坐标，pts_velocity为当前帧相对于前一帧特征点沿着x、y方向的像素移动速度。

对图像使用光流法进行特征点跟踪，具体流程有：

①先调用createCLAHE() 对图像进行自适应直方图均衡化(如果EQUALIZE=1，表示太亮或则太暗)

②调用calcOpticalFlowPyrLK()跟踪cur_pts到forw_pts,根据status,把跟踪失败的点剔除(注意:prev, cur,forw, ids, track_cnt都要剔除),这里还加了个inBorder判断,把跟踪到图像边缘的点也剔除掉.

③如果不需要发布特征点,则到这步就完了,把当前帧forw赋给上一帧cur, 然后退出.如果需要发布特征点(PUB_THIS_FRAME=1), 则执行下面的步骤

④先调用rejectWithF()对prev_pts和forw_pts做ransac剔除outlier.(实际就是调用了findFundamentalMat函数), 在光流追踪成功就记被追踪+1，数值代表被追踪的次数，数值越大，说明被追踪的就越久

⑤调用setMask(), 先对跟踪点forw_pts按跟踪次数降排序, 然后依次选点, 选一个点, 在mask中将该点周围一定半径的区域设为0, 后面不再选取该区域内的点. 有点类似与non-max suppression, 但区别是这里保留track_cnt最高的点.

⑥在mask中不为0的区域,调用goodFeaturesToTrack提取新的特征角点n_pts, 通过addPoints()函数push到forw_pts中, id初始化-1,track_cnt初始化为1.

undistortedPoints() 对角点图像坐标去畸变矫正，并计算每个角点的速度

值得注意的是，当前帧为forw_pts,被追踪到的帧；curr_pts实际上是上一帧；

void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time)
{
    cv::Mat img;
    TicToc t_r;
    cur_time = _cur_time;

    // 1.如果EQUALIZE=1，表示太亮或太暗，进行直方图均衡化处理
    if (EQUALIZE)
    {
        //自适应直方图均衡
        //createCLAHE(double clipLimit, Size tileGridSize)
        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. 判断当前帧图像forw_img是否为空
    if (forw_img.empty())
    {
        //如果当前帧的图像数据forw_img为空，说明当前是第一次读入图像数据
        //将读入的图像赋给当前帧forw_img，同时还赋给prev_img、cur_img
        prev_img = cur_img = forw_img = img;
    }
    else
    {
        //否则，说明之前就已经有图像读入，只需要更新当前帧forw_img的数据
        forw_img = img;
    }
    //此时forw_pts还保存的是上一帧图像中的特征点，所以把它清除
    forw_pts.clear();

    if (cur_pts.size() > 0)// 前一帧有特征点
    {
        TicToc t_o;
        vector<uchar> status;
        vector<float> err;

        // 3. 调用cv::calcOpticalFlowPyrLK()对前一帧的特征点cur_pts进行LK金字塔光流跟踪，得到forw_pts
        //status标记了从前一帧cur_img到forw_img特征点的跟踪状态，无法被追踪到的点标记为0
        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]))// 将当前帧跟踪的位于图像边界外的点标记为0
                status[i] = 0;

        // 4. 根据status,把跟踪失败的点剔除
        //不仅要从当前帧数据forw_pts中剔除，而且还要从cur_un_pts、prev_pts和cur_pts中剔除
        //prev_pts和cur_pts中的特征点是一一对应的
        //记录特征点id的ids，和记录特征点被跟踪次数的track_cnt也要剔除
        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());
    }

    // 5. 光流追踪成功,特征点被成功跟踪的次数就加1
    //数值代表被追踪的次数，数值越大，说明被追踪的就越久
    for (auto &n : track_cnt)
        n++;

    //PUB_THIS_FRAME=1 需要发布特征点
    if (PUB_THIS_FRAME)
    {
        // 6. rejectWithF()通过基本矩阵剔除outliers
        rejectWithF();

        // 7. setMask()保证相邻的特征点之间要相隔30个像素,设置mask
        ROS_DEBUG("set mask begins");
        TicToc t_m;
        setMask();
        ROS_DEBUG("set mask costs %fms", t_m.toc());

        // 8. 寻找新的特征点 goodFeaturesToTrack()
        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())//在mask中不为0的区域,
                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;
            /** 
             *void cv::goodFeaturesToTrack(    在mask中不为0的区域检测新的特征点
             *   InputArray  image,              输入图像
             *   OutputArray     corners,        存放检测到的角点的vector
             *   int     maxCorners,             返回的角点的数量的最大值
             *   double  qualityLevel,           角点质量水平的最低阈值（范围为0到1，质量最高角点的水平为1），小于该阈值的角点被拒绝
             *   double  minDistance,            返回角点之间欧式距离的最小值
             *   InputArray  mask = noArray(),   和输入图像具有相同大小，类型必须为CV_8UC1,用来描述图像中感兴趣的区域，只在感兴趣区域中检测角点
             *   int     blockSize = 3,          计算协方差矩阵时的窗口大小
             *   bool    useHarrisDetector = false,  指示是否使用Harris角点检测，如不指定则使用shi-tomasi算法
             *   double  k = 0.04                Harris角点检测需要的k值
             *)   
             */
            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());

        // 9. addPoints()向forw_pts添加新的追踪点
        ROS_DEBUG("add feature begins");
        TicToc t_a;

        //添将新检测到的特征点n_pts添加到forw_pts中，id初始化-1,track_cnt初始化为1.
        addPoints();
        ROS_DEBUG("selectFeature costs: %fms", t_a.toc());
    }
    // 10. 更新帧、特征点
    //当下一帧图像到来时，当前帧数据就成为了上一帧发布的数据
    prev_img = cur_img;
    prev_pts = cur_pts;
    prev_un_pts = cur_un_pts;

    //把当前帧的数据forw_img、forw_pts赋给上一帧cur_img、cur_pts
    cur_img = forw_img;
    cur_pts = forw_pts;

    // 11. 根据不同的相机模型去畸变矫正和转换到归一化坐标系上，计算速度
    undistortedPoints();
    prev_time = cur_time;
}

inBorder（）函数

bool inBorder(const cv::Point2f &pt)
{
    const int BORDER_SIZE = 1;
    //cvRound()：返回跟参数最接近的整数值，即四舍五入；
    int img_x = cvRound(pt.x);
    int img_y = cvRound(pt.y);
    return BORDER_SIZE <= img_x && img_x < COL - BORDER_SIZE && BORDER_SIZE <= img_y && img_y < ROW - BORDER_SIZE;
}

reduceVector（）函数

去除无法跟踪的特征点。status为0的点直接跳过，否则v[j++]=v[i]留下来，最后v.resize(j)根据最新的j安排内存。

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

rejectWithF() 函数

通过F矩阵去除outliers

首先把特征点转化到归一化相机坐标系，然后计算F矩阵，再根据status清除为0的特征点。

void FeatureTracker::rejectWithF()
{
    if (forw_pts.size() >= 8)// 当前帧（追踪上）特征点数量足够多
    {
        ROS_DEBUG("FM ransac begins");
        TicToc t_f;
        // 1.遍历所有特征点，转化为归一化相机坐标系
        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;
            //对于PINHOLE（针孔相机）可将像素坐标转换到归一化平面并去畸变。根据不同的相机模型将二维坐标转换到三维坐标
            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;
        // 2. 调用cv::findFundamentalMat对un_cur_pts和un_forw_pts计算F矩阵，需要归一化相机系，z=1
        cv::findFundamentalMat(un_cur_pts, un_forw_pts, cv::FM_RANSAC, F_THRESHOLD, 0.99, status);
        int size_a = cur_pts.size();
        // 3. 根据status删除一些特征点
        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() 函数

对跟踪到的特征点，按照被追踪到的次数排序并依次选点，使用mask进行类似非极大抑制，半径为30，去掉密集点，使特征点分布均匀。对跟踪到的特征点从大到小排序并去除密集的点。

void FeatureTracker::setMask()
{
    // 如果是鱼眼镜头直接clone即可，否则创建空白板
    if(FISHEYE)
        mask = fisheye_mask.clone();
    else
        mask = cv::Mat(ROW, COL, CV_8UC1, cv::Scalar(255));// 空白板都是255
    
    // 倾向于留下被追踪时间很长的特征点
    // 构造(cnt，pts，id)序列，（追踪次数，当前特征点坐标，id）
    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])));

    // 对光流跟踪到的特征点forw_pts，按照被跟踪到的次数cnt从大到小排序（lambda表达式）
    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;// a.first指的追踪次数track_cnt
         });

    //清空cnt，pts，id并重新存入
    forw_pts.clear();
    ids.clear();
    track_cnt.clear();

    for (auto &it : cnt_pts_id)
    {
        if (mask.at<uchar>(it.second.first) == 255)// 这个特征点对应的mask值为255，表明点是黑的，还没占有
        {
            //则保留当前特征点，将对应的特征点位置pts，id，被追踪次数cnt分别存入
            forw_pts.push_back(it.second.first);
            ids.push_back(it.second.second);
            track_cnt.push_back(it.first);

            //在mask中将当前特征点周围半径为MIN_DIST的区域设置为0，后面不再选取该区域内的点（使跟踪点不集中在一个区域上）
            cv::circle(mask, it.second.first, MIN_DIST, 0, -1);
        }
    }
}

goodFeaturesToTrack（）函数再检测出新角点

*void cv::goodFeaturesToTrack(    在mask中不为0的区域检测新的特征点
             *   InputArray  image,              输入图像
             *   OutputArray     corners,        存放检测到的角点的vector
             *   int     maxCorners,             返回的角点的数量的最大值
             *   double  qualityLevel,           角点质量水平的最低阈值（范围为0到1，质量最高角点的水平为1），小于该阈值的角点被拒绝
             *   double  minDistance,            返回角点之间欧式距离的最小值
             *   InputArray  mask = noArray(),   和输入图像具有相同大小，类型必须为CV_8UC1,用来描述图像中感兴趣的区域，只在感兴趣区域中检测角点
             *   int     blockSize = 3,          计算协方差矩阵时的窗口大小
             *   bool    useHarrisDetector = false,  指示是否使用Harris角点检测，如不指定则使用shi-tomasi算法
             *   double  k = 0.04                Harris角点检测需要的k值
             *)   
             */

addPoints() 函数

添将新检测到的特征点n_pts，新提取的特征点id初始化为-1，新提取的特征点被跟踪的次数初始化为1。

void FeatureTracker::addPoints()
{
    for (auto &p : n_pts)
    {
        forw_pts.push_back(p);
        ids.push_back(-1);//新提取的特征点id初始化为-1
        track_cnt.push_back(1);//新提取的特征点被跟踪的次数初始化为1
    }
}

undistortedPoints() 函数

1.对角点图像坐标进行去畸变矫正，转换到归一化坐标系上，

2.并计算每个角点的速度。  

void FeatureTracker::undistortedPoints()
{
    cur_un_pts.clear();
    cur_un_pts_map.clear();
    //cv::undistortPoints(cur_pts, un_pts, K, cv::Mat());

    // 1.归一化相机坐标系
    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);
    }

    // 2.计算每个特征点的速度到pts_velocity
    if (!prev_un_pts_map.empty()) // 2.1 地图不是空的判断是否新的点
    {
        double dt = cur_time - prev_time;
        pts_velocity.clear();
        for (unsigned int i = 0; i < cur_un_pts.size(); i++)
        {
            if (ids[i] != -1)// 2.2 通过id判断不是最新的点
            {
                std::map<int, cv::Point2f>::iterator it;// 地图的迭代器
                it = prev_un_pts_map.find(ids[i]);// 找到对应的id

                if (it != prev_un_pts_map.end()) // 2.3 在地图中寻找是否出现过id判断是否最新点
                {
                    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));// 之前出现过，push_back即可
                }
                else
                    pts_velocity.push_back(cv::Point2f(0, 0));// 之前没出现过，先放进去但是速度为0
            }
            else
            {
                pts_velocity.push_back(cv::Point2f(0, 0)); // 是最新的点，速度为0
            }
        }
    }
    else
    {   // 如果地图是空的，速度是0
        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;
}

showUndistortion（）函数

显示去畸变矫正后的特征点  name为图像帧名称

void FeatureTracker::showUndistortion(const string &name)
{
    cv::Mat undistortedImg(ROW + 600, COL + 600, CV_8UC1, cv::Scalar(0));
    vector<Eigen::Vector2d> distortedp, undistortedp;

    for (int i = 0; i < COL; i++)
        for (int j = 0; j < ROW; j++)
        {
            Eigen::Vector2d a(i, j);
            Eigen::Vector3d b;
            m_camera->liftProjective(a, b);
            distortedp.push_back(a);
            undistortedp.push_back(Eigen::Vector2d(b.x() / b.z(), b.y() / b.z()));
            //printf("%f,%f->%f,%f,%f\n)\n", a.x(), a.y(), b.x(), b.y(), b.z());
        }
        
    for (int i = 0; i < int(undistortedp.size()); i++)
    {
        cv::Mat pp(3, 1, CV_32FC1);
        pp.at<float>(0, 0) = undistortedp[i].x() * FOCAL_LENGTH + COL / 2;
        pp.at<float>(1, 0) = undistortedp[i].y() * FOCAL_LENGTH + ROW / 2;
        pp.at<float>(2, 0) = 1.0;
        //cout << trackerData[0].K << endl;
        //printf("%lf %lf\n", p.at<float>(1, 0), p.at<float>(0, 0));
        //printf("%lf %lf\n", pp.at<float>(1, 0), pp.at<float>(0, 0));
        if (pp.at<float>(1, 0) + 300 >= 0 && pp.at<float>(1, 0) + 300 < ROW + 600 && pp.at<float>(0, 0) + 300 >= 0 && pp.at<float>(0, 0) + 300 < COL + 600)
        {
            undistortedImg.at<uchar>(pp.at<float>(1, 0) + 300, pp.at<float>(0, 0) + 300) = cur_img.at<uchar>(distortedp[i].y(), distortedp[i].x());
        }
        else
        {
            //ROS_ERROR("(%f %f) -> (%f %f)", distortedp[i].y, distortedp[i].x, pp.at<float>(1, 0), pp.at<float>(0, 0));
        }
    }
    cv::imshow(name, undistortedImg);
    cv::waitKey(0);
}