VINS-MONO 前端代码feature_trackers

1、主函数入口在feature_tracker_node.cpp

1.1、ros::init

1.2、设置logger级别

1.3、读取参数 readParameters()

readParameters()

读取config_file和vins_folder，并针对鱼眼相机添加mask图片路径

1.4、读取内参readIntrinsicParameter

1.5、判断是否加入鱼眼mask来去除边缘噪声

1.6、订阅话题，执行img_callback  subscribe(IMAGE_TOPIC, 100, img_callback)

img_callback(const sensor_msgs::ImageConstPtr &img_msg)

如果是第一帧，设置第一帧时间

当前帧与上一帧时间间隔有问题，则restart

通过控制间隔时间内的发布次数进行发布频率控制，判断是否达到设定的时间间隔，决定是否发布特征点和img，不发布时也执行readimage()读取图片并处理

读取图片数据并进行处理FeatureTracker::readImage

将id,矫正后的归一化坐标，像素2D点，像素点的速度封装成PointCloudPtr发布到pub_img中； 将图像封装成cvtColor类型的ptr实例中，并发布到pub_match；

1.7、ros::spin()

int main(int argc, char **argv)
{
    ros::init(argc, argv, "feature_tracker");
    ros::NodeHandle n("~");
    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);    //设置logger级别
    readParameters(n);    //读取参数

    for (int i = 0; i < NUM_OF_CAM; i++)
        trackerData[i].readIntrinsicParameter(CAM_NAMES[i]);      //读取内参

	//是否加入鱼眼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");
        }
    }

    //订阅话题，执行img_callback
    ros::Subscriber sub_img = n.subscribe(IMAGE_TOPIC, 100, img_callback);

    pub_img = n.advertise("feature", 1000);
    pub_match = n.advertise("feature_img",1000);
    pub_restart = n.advertise("restart",1000);
    /*
    if (SHOW_TRACK)
        cv::namedWindow("vis", cv::WINDOW_NORMAL);
    */
    ros::spin();
    return 0;
}

 

FeatureTracker::readImage

cv::createCLAHE是否对图像进行直方图均衡化
cv::calcOpticalFlowPyrLK从cur_pts到forw_pts做光流跟踪
把在边界外的点剔除，置status;	剔除操作包括prev_pts\cur_pts\forw_pts\ids\cur_un_pts\track_cnt
记录特征点被成功追踪的次数，追踪成功，则+1
rejectWithF()通过基础矩阵F剔除外点	将图像坐标cur_pts\forw_pts转为深度归一化坐标，根据不同的相机模型将二维坐标转换为3D坐标PinholeCamera::liftProjective；去畸变之后，计算F矩阵，利用得到的status去除outliers——cv::findFundamentalMat
setMask()对跟踪点进行排序并依次选点，设置mask:去掉密集点，使特征点分布均匀	对跟踪到的特征点进行排序（跟踪次数）存入到cnt_pts_id，使用mask进行非极大值抑制（mask初始化为cv::Scalar(255)，然后遍历：for (auto &it : cnt_pts_id)，如果该特征点对应位置的mask为255,则保留当前特征点，在mask中将该特征点周围半径为30的区域设置为0,后面不再选取该区域内的点）
如果特征点不够,寻找新的特征点（shi-tomasi角点）保存到n_pts  cv::goodFeaturesToTrack
addPoints()  向forw_pts添加新的跟踪点n_pts,id初始化-1,track_cnt初始化1