Vins-mono 源码笔记 (1) feature_tracker_node

从这里开始VINSMONO的学习, 由于VINSFusion 是VINSMONO的改进版本, 所以学习计划就是, VINSMONO就学习学习视觉惯导里程计的部分, 剩下的部分还是直接学习 VINSFusion吧 .

入口

feature_tracker节点的全部实现都在feature_tracker文件夹中

节点运行的主函数为 feature_tracker_node.cpp中的main 函数，执行流程：
1、readParameters(n)(feature_tracker/src/parameters.cpp)
通过读取参数文件获取参数
// 读取图像话题 image_topic: “/cam0/image_raw”
fsSettings[“image_topic”] >> IMAGE_TOPIC;
// 读取IMU话题 “/imu0”
fsSettings[“imu_topic”] >> IMU_TOPIC;
2、对存在的每个相机的FeatureTracker类对象的m_camera进行初始化
trackerData[i].readIntrinsicParameter(CAM_NAMES[i]);
其中trackerData定义为 FeatureTracker trackerData[NUM_OF_CAM];
CAM_NAMES[i]为参数文件的地址string

// 根据参数文件创建相机对象  
void FeatureTracker::readIntrinsicParameter(const string &calib_file)
{
    ROS_INFO("reading paramerter of camera %s", calib_file.c_str());
    // 返回读取了畸变与内参的PinholeCamera 相机对象  
    m_camera = CameraFactory::instance()->generateCameraFromYamlFile(calib_file);    
}

m_camera 定义在feature_tracker.h中,

 camodocal::CameraPtr m_camera;

CameraPtr定义在 Camera.h

typedef boost::shared_ptr<Camera> CameraPtr;

因此, m_camera是指向Camera类的智能指针。

generateCameraFromYamlFile()根据参数返回子类的相机对象，然后父类指针m_camera指向子类对象。
Camera是一个抽象类, 可以看到内部很多纯虚函数

有多个继承与Camera类的子类, 分别表示不同种类的相机.

相机种类的声明在Camera.h的Camera类中：

enum ModelType
{
    KANNALA_BRANDT,
    MEI,
    PINHOLE,
    SCARAMUZZA
};

每一种相机类都是父类Camera的子类，声明在camera_models中。

通过让父类指针m_camera指向子类对象, 从而实现多态.

这里 generateCameraFromYamlFile()返回的是 PinholeCamera对象,即针孔相机.
那么通过 m_camera 对象调用的方法也都是 PinholeCamera类的方法.

3、话题的订阅，可以看到和图像相关的话题订阅是
订阅图像话题： “/cam0/image_raw” 回调函数 img_callback()
发布特征点到topic： feature
发布用于显示的图片到topic：feature_img

img_callback回调函数

整体流程：
1、如果是第一帧，则先记录时间， first_image_time - 用于频率控制，last_image_time 用于判断数据是否错位。
2、根据时间戳判断图像是否错位。
3、比较当前图像特征的发布频率与需要发布的频率，如果需要发送特征则控制 PUB_THIS_FRAME=true.
4、执行readImage() 对当前图片进行特征点跟踪
trackerData[i].readImage(show_img, img_msg->header.stamp.toSec());
5、updateID() - 为新添加的特征点设置id ，这个id为当前特征点在历史所有特征点中的序号， 表示按先后顺序排在第几个 ，跟踪失败被删除的也包含在内。
6、如果需要发布特征点 (PUB_THIS_FRAME=TRUE)
构造sensor_msgs::PointCloudPtr feature_points(new sensor_msgs::PointCloud);
pub_img.publish(feature_points) 发布到"feature"topic .

// 一下容器元素均是一一对应的  
feature_points->points.push_back(p);          //  去畸变后的归一化坐标                
id_of_point.values.push_back(p_id * NUM_OF_CAM + i); // i = 0      NUM_OF_CAM = 1   这样   /NUM_OF_CAM = 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);            // channels[0]
feature_points->channels.push_back(u_of_point);             // channels[1]  .....
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);

FeatureTracker类

这个类实现了所有特征提取的方法.
首先注意这些重要的变量
// cur_img： 上一帧 forw_img：当前帧
cv::Mat cur_img, forw_img;
// cur_pts： 上一帧的特征点像素坐标容器 forw_pts：当前帧特征点像素坐标容器
vector< cv::Point2f> cur_pts, forw_pts;
// 特征点id容器
vector ids;
// 当前帧特征点跟踪的次数
vector track_cnt;
// cur_un_pts： 去畸变后的归一化坐标
vector< cv::Point2f> cur_un_pts;

readImage()

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

1、先通过calcOpticalFlowPyrLK()跟踪上一帧的特征点cur_pts，然后上一帧特征点在当前帧中的坐标为forw_pts，跟踪的状态为status，跟踪成功的特征点再判断是否在图像范围内，清除跟踪失败的特征点。
2、如果不需要发布特征点就没什么要做的了，如果要发布特征点，那么再通过rejectWithF()清除一部分
outliers特征点，setMask() 优先保留跟踪数量多的特征点以及防止特征点聚集，最后利用goodFeaturesToTrack()再生成一部分特征点n_pts，通过addPoints()添加n_pts。
3、undistortedPoints()

疑问总结：
1、每一帧图像中的特征点是怎么形成的？
如果是第一帧图像，通过goodFeaturesToTrack()生成特征点，此后的图像帧，先通过calcOpticalFlowPyrLK()跟踪上一帧的特征点，再通过goodFeaturesToTrack()增加特征点。
2、forw_pts、ids、track_cnt、pts_velocity容器是如何一一对应的？
看代码很清楚： 新增特征点后，addPoints()

// 新添加的点id 统统设置为-1 
// 可以看到 forw_pts、ids、track_cnt容器是一一对应的
void FeatureTracker::addPoints()
{
    for (auto &p : n_pts)
    {
        forw_pts.push_back(p);
        ids.push_back(-1);
        track_cnt.push_back(1);
    }
}

特征点筛选时 setMask()

// 清空准备重新放置   
forw_pts.clear();
ids.clear();
track_cnt.clear();
// 遍历 排序后的(cnt，(pts，id) )序列    优先cnt大的
for (auto &it : cnt_pts_id)
{   //  如果该特征点位置没有被占据   
    if (mask.at<uchar>(it.second.first) == 255)
    {
 // 放置该特征点      注意排序后 forw_pts、ids、track_cnt 是一一对应的
        forw_pts.push_back(it.second.first);
        ids.push_back(it.second.second);          // 这是ids 里面的id已经是无序的了  
        track_cnt.push_back(it.first);
 // 该特征点附近一个圆的区域 半径30  设置为 0    表示不能再放置特征点  
        cv::circle(mask, it.second.first, MIN_DIST, 0, -1);   // -1 表示填充   
    }
}

可以看到每个特征点在 forw_pts、ids、track_cnt序号相同。

setMask()

作用：优先保留被跟踪次数多的特征点 ，并在这些特征点的附近设置mask ，mask区域不能存在其他特征点， 类似于非极大值抑制，防止特征点聚集 。
步骤：
1、构造mask 与原图像尺寸相同 每个像素点赋值 255
mask = cv::Mat(ROW, COL, CV_8UC1, cv::Scalar(255));
2、构造容器cnt_pts_id (跟踪次数，(坐标，id) )
vector<pair<int, pair<cv::Point2f, int>>> cnt_pts_id;
3、对cnt_pts_id容器按照跟踪次数从大到小进行排序。
4、清空forw_pts、ids、track_cnt容器，准备根据排序结果重置。
4、遍历cnt_pts_id容器，优先选择跟踪次数多的且该特征点位置处的mask=255的特征点push_back到forw_pts、ids、track_cnt容器，放置完将特征点将附近一个半径30的圆区域mask设置为 0。

说明： forw_pts、ids、track_cnt容器最后仍然保持一一对应性——同一index代表相同的特征点。

undistortedPoints()

1、将cur_pts的点投影到归一化平面并进行去畸变处理 得到 cur_un_pts，将坐标和id组成pair放置与
cur_un_pts_map。
去畸变是通过 m_camera->liftProjective(a, b) 完成的, 其中这里调用的 liftProjective()是 PinholeCamera 类中重写基类的函数.
整个去畸变的流程为:
1. 像素坐标反投影到归一化坐标 (mx_d, my_d) .
2. distortion(Eigen::Vector2d(mx_d, my_d), d_u); 计算畸变量 , 结果保存在 d_u.
3. 利用畸变两进行修正 得到 (mx_u, my_u).

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)
        {
        }
        else
        {
            // Recursive distortion model
            int n = 8;
            Eigen::Vector2d d_u;
            // 计算畸变     畸变量为 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;
}

但是上面这里为什么要在一个循环里反复去畸变???
畸变量的计算:

/**
 * \brief Apply distortion to input point (from the normalised plane)
 *
 * \param p_u undistorted coordinates of point on the normalised plane
 * \return to obtain the distorted point: p_d = p_u + d_u
 */
void
PinholeCamera::distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) const
{
    // 径向畸变参数
    double k1 = mParameters.k1();
    double k2 = mParameters.k2();
    // 切向畸变参数 
    double p1 = mParameters.p1();
    double p2 = mParameters.p2();

    double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u;
    
    mx2_u = p_u(0) * p_u(0);    // x^2
    my2_u = p_u(1) * p_u(1);    // y^2
    mxy_u = p_u(0) * p_u(1);    // x*y
    rho2_u = mx2_u + my2_u;     // r^2  
    rad_dist_u = k1 * rho2_u + k2 * rho2_u * rho2_u;   
    d_u << p_u(0) * rad_dist_u + 2.0 * p1 * mxy_u + p2 * (rho2_u + 2.0 * mx2_u),  // x*(k1*r^2+k2*r^4)+2*p1*x*y + p2(r^2+2*x^2)
           p_u(1) * rad_dist_u + 2.0 * p2 * mxy_u + p1 * (rho2_u + 2.0 * my2_u);  
}

其中 畸变的坐标与校正畸变后的坐标关系为:
p_d = p_u + d_u;
d_u即distortion计算的畸变量, p_u为校正畸变后的坐标, p_d 为畸变的坐标;
因此,已知畸变量与畸变后的坐标, 可求出校正畸变后的坐标:
p_u = p_d - d_u;

2、计算 归一化平面上点的移动速度 pts_velocity。
情况一：第一帧 速度设置为0。
情况二：非第一帧，新特征点速度直接设为0，否则去prev_un_pts_map匹配id，然后坐标求差/时间得到速度。

特征点的管理

由于vins有一个滑动窗口, 所以需要管理整个滑动窗口的全部特征点信息, 这方面的操作在
vins_estimator/src/featurn_manage.h / cpp中完成 , 结构如下图