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。这个参数是可选参数。 |