/feature_tracker 的主要作用是向后端提供图片的特征点信息
特征点信息主要包括:
- 像素坐标 -》特征点提取算法
- 去畸变后归一化坐标 -》 特征点去畸变算法
- 特征点id -》 光流追踪算法
- 特征点速度 (用于相机IMU世界矫正)
时间戳定时清零
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;
}
}
如果不进行时间戳定时清零的话,可能会造成给后端突然发送大量数据的情况:
比如我的发布总数是900帧,时间戳是100s,设定频率阈值为100hz。某一时刻突然接受了50帧的图像,但此时的频率也只是95HZ,没有超过设定频率阈值,但系统本身是无法处理这么多图像的。所以需要在发布频率十分接近设定频率时进行清零操作。
光流追踪
- 光流追踪:基于灰度不变假设
- 图像都是灰度图,灰度从0~255取值.
目的:以上一帧的特征点坐标为起点,在当前帧的同样的坐标下以一定范围找到与上一帧特征点附近区域灰度值相近的区域(patch)
从定位一个点的灰度值->定位一个区域的灰度值,可以有效提高鲁棒性
trackerData[i].readImage(ptr->image.rowRange(ROW * i, ROW * (i + 1)), img_msg->header.stamp.toSec());
↓ ↓ ↓readImage
void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time)
图像金字塔
- 在ORB中主要是实现提取特征点对距离鲁棒
- 在VINS中主要为了提高光流的稳定性.比如level0:level4 = 2:1,本来在两帧图像上特征点距离相差10个像素点,通过光流金字塔可以让它们的距离变为5个像素,使其更难陷入局部最小值,更容易实现追踪.
调用cv::calcOpticalFlowPyrLK()对前一帧的特征点cur_pts进行LK金字塔光流跟踪,得到forw_pts
cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, cv::Size(21, 21), 3);
VINS中对图像金字塔的处理:
- 把上一层图像金字塔追踪的结果作为下一帧图像金字塔追踪的初值.既保证了像素精度又保证了追踪结果在还原时的准确度
对特征点的筛选和均匀化操作
光流 VS 描述子匹配
时间上的比较
比如,我们设定每张图像提取100个特帧点。
- 光流法:会在第一帧图像提取100个特征点,然后在下一帧对其进行跟踪。假设成功追踪到了80个特征点,光流法只需要在下一帧多提取20个特征点保证每次的特征点数有100个即可。当图像帧数趋于无穷时,光流法的追踪时间主要就在于提取20个特征点的时间+LK光流追踪的时间。
- 描述子匹配: 对每张图像都提取100个特征点,同时都要计算每张图像各自的100个特帧点对应的描述子,然后做特征点匹配。当图像帧数趋于无穷时,特征点法的追踪时间主要就在于提取100个特征点的时间+提取100个特征点的描述子时间+描述子匹配的时间。
性能上的比较
-
LK光流的匹配的鲁棒性优于描述子匹配。
-
但是当涉及到遮挡时,光流法无法认为遮挡前和遮挡回复后的特征点是完全不同的点,就会三角化出两个点,也就失去了同一个3D点对这两个特征点的约束。而在ORB中,会通过一些trick寻找没有匹配上的特征点的可能匹配点。
-
当然特征点法的主要作用是在重定位和回环检测上。光流追踪更适合帧见匹配。
高效去畸变
//转换为归一化像素坐标
m_camera->liftProjective(Eigen::Vector2d(cur_pts[i].x, cur_pts[i].y), tmp_p);
↓ ↓ ↓ liftProjective
// Lifts a point from the image plane to its projective ray
void PinholeCamera::liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const
↓ ↓ ↓
// 逐渐逼近式去畸变
distortion(Eigen::Vector2d(mx_u, my_u), d_u);
mx_u = mx_d - d_u(0);
my_u = my_d - d_u(1);
- 图像的畸变发生从相机坐标系投影到归一化坐标系的时候。
- 假设像素坐标真值为A点,因为发生了畸变,所以A点变化到了A‘点。我们称△AA’为畸变程度
图像的畸变程度在中心点为0,越靠近边缘畸变程度越大。
- 假设我们有一个B点更靠近图像的边缘,那么我们有△AA’ < △BB’
- 当我们用2d点恢复3d点时,我们可以获得畸变的点为A‘点,我们的目的是要获得未畸变的点A点。
我们的问题是如何通过畸变发生后的坐标推算畸变发生前的坐标?
即已知A’ 如何 = > => => A?
步骤(逐渐逼近):
- 已知我们A‘点,我们通过A’点作为未发生畸变的B点,对其发生畸变操作,得到B‘点。
- 我们沿畸变方向,把A’点延升△BB’的距离得到C点。因为B点更加靠近图像中心,所以我们有△AA’ > △BB’,所以C点一定在AA‘之间。也有AC < AA’,使得C点比A点更加接近真实的A点。
- 对C点发生畸变操作,得到C‘点。即可以得到 △BB’ < △CC’ < △AA’ .再次把A’点延升△CC’的距离得到D点。可以保证D点更加接近A点。如此往复即可不断接近真值A点。
- 主要误差在阈值范围内,即可求得。