VINS-Mono SLAM框架(一) visual feature_tracker

feature_tracker

• 入口函数为 feature_tracker_node.cpp/main()

• 先通过readParameters(ros::NodeHandle &n)读取配置参数

  所以 feature_tracker Node 需要有config_file参数 来指定话题名称/相机类型/相机内参/cam-IMU外参/视觉特征追踪参数/后端优化参数/imu参数/回环检测的参数

• 再通过 FeatureTracker::readIntrinsicParameter(const string &calib_file)读取相机类型与相机内参

  根据不同的相机模型， 实例化不同camera,并设置参数
  CameraFactory::generateCameraFromYamlFile(const std::string& filename)
  params = camera->getParameters();
  params.readFromYamlFile(filename);
  camera->setParameters(params);

• 订阅 IMAGE_TOPIC 话题 主要的处理在IMAGE_TOPIC的回调函数中

• 发布 feature_point / feature_img / restart_msg话题

特征点提取

cv::goodFeaturesToTrack(forw_img, n_pts, 
MAX_CNT - forw_pts.size(), 0.01, 
MIN_DIST, mask);

void goodFeaturesToTrack(
InputArray image, OutputArray corners, 
int maxCorners, double qualityLevel, 
double minDistance, InputArray mask=noArray(), 
int blockSize=3, bool useHarrisDetector=false, double k=0.04 )

形参	功能
>InputArray image	输入图像（8位或32位单通道图）
OutputArray corners	检测到的所有角点，类型为vector或数组，由实际给定的参数类型而定。如果是vector，那么它应该是一个包含cv::Point2f的vector对象；如果类型是cv::Mat,那么它的每一行对应一个角点，点的x、y位置分别是两列。
int maxCorners	限定检测到的点数的最大值
double qualityLevel	表示检测到的角点的质量水平（通常是0.10到0.01之间的数值，不能大于1.0）
double minDistance	用于区分相邻两个角点的最小距离（小于这个距离得点将进行合并）
InputArray mask	mask参数，如果指定，它的维度必须和输入图像一致，且在mask值为0处不进行角点检测。
blockSize	表示在计算角点时参与运算的区域大小，常用值为3，但是如果图像的分辨率较高则可以考虑使用较大一点的值。
useHarrisDetector	指定角点检测的方法，如果是true则使用Harris角点检测，false则使用Shi Tomasi算法
double k	使用Harris算法时使用，最好使用默认值0.04。

光流法追踪

cv::calcOpticalFlowPyrLK(cur_img, forw_img,
 cur_pts, forw_pts,
 status, err, cv::Size(21, 21), 3);


void calcOpticalFlowPyrLK(
InputArray prevImg, InputArray nextImg, 
InputArray prevPts, InputOutputArray nextPts, 
OutputArray status, OutputArray err, 
Size winSize=Size(21,21), int maxLevel=3, 
TermCriteria criteria=TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, 0.01), 
int flags=0, double minEigThreshold=1e-4 

形参	功能
>prevImg	第一幅8位输入图像 或 由buildOpticalFlowPyramid()构造的金字塔
nextImg	第二幅与preImg大小和类型相同的输入图像或金字塔
prevPts	光流法需要找到的二维点的vector。点坐标必须是单精度浮点数
nextPts	包含输入特征在第二幅图像中计算出的新位置的二维点（单精度浮点坐标）的输出vector。当使用OPTFLOW_USE_INITIAL_FLOW 标志时，nextPts的vector必须与input的大小相同。
status	输出状态vector(类型：unsigned chars)。如果找到了对应特征的流，则将向量的每个元素设置为1；否则，置0。
err	误差输出vector。vector的每个元素被设置为对应特征的误差，可以在flags参数中设置误差度量的类型；如果没有找到流，则未定义误差（使用status参数来查找此类情况）。
winSize	每级金字塔的搜索窗口大小。
maxLevel	基于最大金字塔层次数。如果设置为0，则不使用金字塔（单级）；如果设置为1，则使用两个级别，等等。如果金字塔被传递到input，那么算法使用的级别与金字塔同级别但不大于MaxLevel。
criteria	指定迭代搜索算法的终止准则（在指定的最大迭代次数标准值（criteria.maxCount）之后，或者当搜索窗口移动小于criteria.epsilon。）
flags	操作标志，可选参数： OPTFLOW_USE_INITIAL_FLOW：使用初始估计，存储在nextPts中；如果未设置标志，则将prevPts复制到nextPts并被视为初始估计。 OPTFLOW_LK_GET_MIN_EIGENVALS：使用最小本征值作为误差度量（见minEigThreshold描述）；如果未设置标志，则将原始周围的一小部分和移动的点之间的 L1 距离除以窗口中的像素数，作为误差度量。
minEigThreshold	算法所计算的光流方程的2x2标准矩阵的最小本征值（该矩阵称为[Bouguet00]中的空间梯度矩阵）÷ 窗口中的像素数。如果该值小于MinEigThreshold，则过滤掉相应的特征，相应的流也不进行处理。因此可以移除不好的点并提升性能。

对极几何 算F 矩阵

cv::findFundamentalMat(un_cur_pts, un_forw_pts,
 		cv::FM_RANSAC, F_THRESHOLD, 0.99, status);

cv::findFundamentalMat( 
InputArray points1, InputArray points2,
int method=FM_RANSAC,
double param1=3., double param2=0.99,
OutputArray mask=noArray()
);
                                     

形参	功能
points1	第一幅图像点的数组，大小为2xN/Nx2 或 3xN/Nx3 (N 点的个数)，多通道的1xN或Nx1也可以。点坐标应该是浮点数(双精度或单精度)。
points2	第二副图像的点的数组，格式、大小与第一幅图像相同。
method	计算基本矩阵的方法,CV_FM_7POINT – 7-点算法，点数目＝ 7;CV_FM_8POINT – 8-点算法，点数目 >= 8;CV_FM_RANSAC – RANSAC 算法，点数目 >= 8;CV_FM_LMEDS - LMedS 算法，点数目 >= 8
param1	这个参数只用于方法RANSAC 或 LMedS 。它是点到对极线的最大距离，超过这个值的点将被舍弃，不用于后面的计算。通常这个值的设定是0.5 or 1.0 。
param2	这个参数只用于方法RANSAC 或 LMedS 。 它表示矩阵正确的可信度。例如可以被设为0.99 。
status	具有N个元素的输出数组，在计算过程中没有被舍弃的点，元素被被置为1；否则置为0。这个数组只可以在方法RANSAC and LMedS 情况下使用；在其它方法的情况下，status一律被置为1。这个参数是可选参数。