VINS理论与代码详解2——单目视觉跟踪

一．Feature_tracker文件夹中

首先讲第一部分，也就是纯粹的图像处理部分内容，在论文中的第IV点观测值预处理的A部分视觉前端处理，为了更好的理解代码，有必要将论文中的相关内容和大家讨论一番。

论文内容：每当进入新的图像，都会使用KLT稀疏光流法进行跟踪，同时提取100-300个角点信息，我的理解是角点是用来建立图像，光流跟踪是用来快速定位。同时在这里还进行了关键帧的选取（注意这一过程在代码中是由vins_estimate文件中实现的），主要是两个剔除关键帧的策略，分别是平均视差法和跟踪质量法。平均视差法：如果当前帧的和上一个关键帧跟踪点的平均视差超出了一个设定的阈值，就将当前帧设为关键帧。这里有一个问题，就是旋转和平移都会产生视差（不只是平移哦），当出现纯旋转的时候特征点无法被三角化，无法计算出旋转值，也就无法计算跟踪点间的平均视差，为了解决这一问题，采用短时的陀螺仪观测值来补偿旋转，从而计算出视差，这一过程只应用到平均视差的计算，不会影响真实的旋转结果。

具体代码实现：主要负责图像角点提取和光流跟踪，只有一个主线程。主要是三个源程序，分别是feature_tracker、feature_tracker_node以及parameters。feature_tracker_node是特征跟踪线程的系统入口，feature_tracker是特征跟踪算法的具体实现，parameters是设备等参数的读取和存放。

1. feature_tracker_node.cpp系统入口

（1） main()函数

步骤1：readParameters(n);读取参数，是config->euroc->euroc_config.yaml中的一些配置参数。

步骤2: trackerData[i].readIntrinsicParameter(CAM_NAMES[i]);在这里NUM_OF_CAM设置成常量1，只有一个摄像头（单目），读取相机内参。

步骤3:判断是否加入鱼眼mask来去除边缘噪声

步骤4: ros::Subscriber sub_img = n.subscribe(IMAGE_TOPIC, 100, img_callback);订阅话题和发布话题，监听IMAGE_TOPIC（/cam0/image_raw），有图像发布到这个话题上的时候，执行回调函数，这里直接进入到img_callback函数中接收图像，前端视觉的算法基本在这个回调函数中。

1） img_callback(const sensor_msgs::ImageConstPtr &img_msg)接收图像

步骤1: 频率控制，保证每秒钟处理的image不多于FREQ，这里将平率控制在10hz以内。

步骤2: 处理单目相机

步骤2.1: trackerData[i].readImage(ptr->image.rowRange(ROW * i, ROW *(i + 1)));读取到的图像数据存储到trackerData中，读取完之后如果图像太亮或太黑（EQUALIZE=1），使用createCLAHE对图像进行自适应直方图均衡化，如果图像正常，设置成当前图像。在读取图像的时候进行光流跟踪和特征点的提取。FeatureTracker类中处理的主要函数就是readImage()，这里涉及到3个img(prev_img, cur_img, forw_img)和pts(prev_pts,cur_pts, forw_pts)，两者是相似的。刚开始看不是太好理解，cur和forw分别是LK光流跟踪的前后两帧，forw才是真正的“当前”帧，cur实际上是上一帧，而prev是上一次发布的帧，它实际上是光流跟踪以后，prev和forw根据Fundamental Matrix做RANSAC剔除outlier用的，也就是rejectWithF()函数. readImage()的处理流程为:

①先调用cv::CLAHE对图像做直方图均衡化(如果EQUALIZE=1，表示太亮或则太暗)

②调用calcOpticalFlowPyrLK()跟踪cur_pts到forw_pts,根据status,把跟踪失败的点剔除(注意:prev, cur,forw, ids, track_cnt都要剔除),这里还加了个inBorder判断,把跟踪到图像边缘的点也剔除掉.

③如果不需要发布特征点,则到这步就完了,把当前帧forw赋给上一帧cur, 然后退出.如果需要发布特征点(PUB_THIS_FRAME=1), 则执行下面的步骤

④先调用rejectWithF()对prev_pts和forw_pts做ransac剔除outlier.(实际就是调用了findFundamentalMat函数), 在光流追踪成功就记被追踪+1，数值代表被追踪的次数，数值越大，说明被追踪的就越久

⑤调用setMask(), 先对跟踪点forw_pts按跟踪次数降排序, 然后依次选点, 选一个点, 在mask中将该点周围一定半径的区域设为0, 后面不再选取该区域内的点. 有点类似与non-max suppression, 但区别是这里保留track_cnt最高的点.

⑥在mask中不为0的区域,调用goodFeaturesToTrack提取新的角点n_pts, 通过addPoints()函数push到forw_pts中, id初始化-1,track_cnt初始化为1.

整体来说需要注意的是：光流跟踪在②中完成，角点提取在⑥中完成

步骤2.2:判断是否需要显示畸变。

步骤2.3:将特征点矫正（相机模型camodocal）后归一化平面的3D点（此时没有尺度信息，3D点p.z=1），像素2D点，以及特征的id，封装成ros的sensor_msgs::PointCloud消息类型的feature_points实例中；将图像封装到cv_bridge::CvImageConstPtr类型的ptr实例中

步骤3: 发布消息的数据

pub_img.publish(feature_points);

pub_match.publish(ptr->toImageMsg())

将处理完的图像信息用PointCloud实例feature_points和Image的实例ptr消息类型，发布到"feature"和"feature_img"的topic（此步骤在main函数中完成）

至此，已经将图像数据包装成特征点数据和图像数据发布出来了，下面就是在开一个线程，发布一个话题，接收这两种消息，也就是下面的vins_esitimate文件中做的事。

VINS理论与代码详解2——单目视觉跟踪相关推荐

从PointNet到PointNet++理论及代码详解
从PointNet到PointNet++理论及代码详解 1. 点云是什么 1.1 三维数据的表现形式 1.2 为什么使用点云 1.3 点云上以往的相关工作 2. PointNet 2.1 基于点云的置 ...
强化学习PPO从理论到代码详解(1)--- 策略梯度Policy gradient
第0章闲聊吹水 Proximal Policy Optimization(PPO) 近端策略优化,可以说是目前最稳定,最强的强化学习算法之一了,也是openAI默认的强化学习算法,有多叼不用我说了吧 ...
[经典的图像warping方法] Thin Plate Spline: TPS理论和代码详解
0. 前言 2022年没有新写什么博客, 主要精力都在搞论文. 今年开始恢复! 本文的目标是详细分析一个经典的基于landmark(文章后面有时也称之为控制点control point)的图像warp ...
ORB-SLAM2从理论到代码实现（三）：ORB特征提取和匹配理论和代码详解
1. 理论知识特征点由关键点(Key-point)和描述子(Descriptor)两部分组成.ORB特征点(Oriented FAST and Rotated BRIEF)是由Oriented FA ...
python平均入门时间_理论+Python代码详解：入门时间序列分类
对时间序列进行分类?这真的有可能吗?这会有什么用呢? 我们接触的大多数时间序列数据主要涉及产生预测的交易.无论是预测产品的需求还是销售额,航空公司的乘客数量或特定股票的收盘价,我们都习惯于利用久经考验 ...
时空图卷积ST-GCN理论和代码详解
1 前言基于骨架的动作识别(Skeleton-Based Action Recognition)主要任务是从一系列时间连续的骨骼关键点(2D/3D)中识别出正在执行的动作.因为牵涉到骨骼框架这种图结 ...
VINS技术路线与代码详解
VINS技术路线写在前面:本文整和自己的思路,希望对学习VINS或者VIO的同学有所帮助,如果你觉得文章写的对你的理解有一点帮助,可以推荐给周围的小伙伴们,当然,如果你有任何问题想要交流,欢迎随时探 ...
isight参数优化理论与实例详解_案例1（ISIGHT集成ADAMS CAR方法实现）
本文字数1304字25图,建议阅读时间7分钟强调一下是ISIGHT,不是INSIGHT INSIGHT是ADAMS内置的一个试验设计模块,它提供了一组统计工具,用于分析仿真结果,辅助优化和改进系统 ...
来FAL学风控|风控策略分析师的日常是怎样的？（案例+代码详解篇）
风控策略分析师的日常是怎样的?(案例+代码详解篇) FAL金科应用研究院做了5年的金融,3年的数据分析工作,从17年6月才真正接触代码,算不到熟练,但在不断的学习和工作实践中目前是可以解决任何问题的 ...

VINS理论与代码详解2——单目视觉跟踪

VINS理论与代码详解2——单目视觉跟踪相关推荐

最新文章

热门文章