文章目录
-
- 理论分析
- 部分理论说明
- 数据预处理框架
- ROS代码实现
- 视觉跟踪代码结构
- 一、feature_tracker_node.cpp
- 1、void ing_callback(const sensor_msgs::ImageConstPtr &img_msg)
- 二、feature_tracker.cpp
- void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time)
- 1 inBorder（）函数
- 2 reduceVector()函数
- 3 setMask()函数
- 4 addPoints()函数
- 5 rejectWithF()函数
- 6 updateID()函数
- 7 showUndistortion()函数
- 8 undistortedPoints()函数
参考链接：https://blog.csdn.net/qq_41839222/article/details/86030962
参考链接：https://blog.csdn.net/try_again_later/article/details/104847052
二位博主写的太好了 感谢博主 ~~~O(∩_∩)O哈哈~
理论分析
这部分主要说一下视觉跟踪模块（feature_trackers），先说一下涉及到的具体内容：
- 对于每一幅新图像，KLT稀疏光流算法对现有特征进行跟踪；
- 检测新的角点特征以保证每个图像特征的最小数目(100-300)
- 通过设置两个相邻特征之间像素的最小间隔来执行均匀的特征分布；
- 利用基本矩阵模型的RANSAC算法进行外点剔除；
- 对特征点进行去畸变矫正，然后投影到一个单位球面上(对于cata-fisheye camera)。
- 关键帧选取
- 当前帧相对最近的关键帧的特征平均视差大于一个阈值就为关键帧（因为视差可以根据平移和旋转共同得到，而纯旋转则导致不能三角化成功，所以这一步需要IMU预积分进行补偿）
- 当前帧跟踪到的特征点数量小于阈值视为关键帧；
部分理论说明
这里补充一下理论部分的:对特征点进行去畸变矫正，然后投影到一个单位球面上(对于cata-fisheye camera)
m_camera->liftProjective(Eigen::Vector2d(cur_pts[i].x, cur_pts[i].y), tmp_p);
- 1
可以根据不同的相机模型将二维坐标转换到三维坐标：
对于CATA（卡特鱼眼相机）将像素坐标投影到单位圆内，这里涉及了鱼眼相机模型
而对于PINHOLE（针孔相机）将像素坐标直接转换到归一化平面（z=1）并采用逆畸变模型(k1,k2,p1,p2)去畸变等。
节点在启动时会先读取相机内参，根据config_file文件中model_type的值决定采用何种相机模型，并创建相应模型的对象指针，读取在该模型下需要的参数。他们之间的调用顺序:
readIntrinsicParameter ------> generateCameraFromYamlFile -----> CameraPtr
for (int i = 0; i < NUM_OF_CAM; i++)
trackerData[i].readIntrinsicParameter(CAM_NAMES[i]);//读取相机内参,保存到CAM_NAMES[i],如何获取相机的内呢???
void FeatureTracker::readIntrinsicParameter(const string &calib_file)
{
ROS_INFO(“reading paramerter of camera %s”, calib_file.c_str());
m_camera = CameraFactory::instance()->generateCameraFromYamlFile(calib_file);
}
CameraPtr
CameraFactory::generateCameraFromYamlFile(const std::string& filename)
{
cv::FileStorage fs(filename, cv::FileStorage::READ);
if (!fs.isOpened())
{
return CameraPtr();
}
Camera::ModelType modelType = Camera::MEI;
if (!fs[“model_type”].isNone())
{
std::string sModelType;
fs[“model_type”] >> sModelType;
if (boost::iequals(sModelType, “kannala_brandt”))
{
modelType = Camera::KANNALA_BRANDT;
}
else if (boost::iequals(sModelType, “mei”))
{
modelType = Camera::MEI;
}
else if (boost::iequals(sModelType, “scaramuzza”))
{
modelType = Camera::SCARAMUZZA;
}
else if (boost::iequals(sModelType, “pinhole”))
{
modelType = Camera::PINHOLE;
}
else
{
std::cerr << "# ERROR: Unknown camera model: " << sModelType << std::endl;
return CameraPtr();
}
}
switch (modelType)
{
case Camera::KANNALA_BRANDT:
{
EquidistantCameraPtr camera(new EquidistantCamera);
EquidistantCamera::Parameters params = camera->getParameters();
params.readFromYamlFile(filename);
camera->setParameters(params);
return camera;
}
case Camera::PINHOLE:
{
PinholeCameraPtr camera(new PinholeCamera);
PinholeCamera::Parameters params = camera->getParameters();
params.readFromYamlFile(filename);
camera->setParameters(params);
return camera;
}
case Camera::SCARAMUZZA:
{
OCAMCameraPtr camera(new OCAMCamera);
OCAMCamera::Parameters params = camera->getParameters();
params.readFromYamlFile(filename);
camera->setParameters(params);
return camera;
}
case Camera::MEI:
default:
{
CataCameraPtr camera(new CataCamera);
CataCamera::Parameters params = camera->getParameters();
params.readFromYamlFile(filename);
camera->setParameters(params);
return camera;
}
}
return CameraPtr();
}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
- 61
- 62
- 63
- 64
- 65
- 66
- 67
- 68
- 69
- 70
- 71
- 72
- 73
- 74
- 75
- 76
- 77
- 78
- 79
- 80
- 81
- 82
- 83
数据预处理框架
代码流程如下图：主要三个源程序，feature_tracker_node是特征跟踪线程的系统入口，feature_tracker是特征跟踪算法的具体实现，parameters是设备等参数的读取和存放。
我们有了整体的理论以及部分理论的补充之后,我们看一下视觉跟踪整体的ROS框架
ROS代码实现
feature_trackers可被认为是一个单独的模块
输入：
- 图像，即订阅了传感器或者rosbag发布的topic：“/cam0/image_raw”
ros::Subscriber sub_img = n.subscribe(IMAGE_TOPIC, 100, img_callback);
- 1
输出：
- 发布topic：“/feature_trackers/feature_img” 即跟踪的特征点图像，主要是之后给RVIZ用和调试用
- 发布topic：“/feature_trackers/feature” 即跟踪的特征点信息，由/vins_estimator订阅并进行优化
- 发布topic：“/feature_trackers/restart” 即判断特征跟踪模块是否出错，若有问题则进行复位，由/vins_estimator订阅
pub_img = n.advertise<sensor_msgs::PointCloud>("feature", 1000);
pub_match = n.advertise<sensor_msgs::Image>("feature_img",1000);
pub_restart = n.advertise<std_msgs::Bool>("restart",1000);
- 1
- 2
- 3
现在我们知道了视觉跟踪的ROS框架, 订阅者和发布者等信息. 接下来一起看一下视觉跟踪的代码结构.
视觉跟踪代码结构
VINS对于视觉跟踪相关的代码在feature_tracker文件夹中。
feature_trackers_node.cpp：main()函数 ROS的程序入口, 进入后配置参数,在parameters.cpp中.
feature_trackers_node.cpp 中的局部代码
//1.读取yaml中的一些配置参数 地址为config->euroc->euroc_config.yaml
readParameters(n);
//2.读取每个相机实例对应的相机内参，NUM_OF_CAM 经常为1，单目
for (int i = 0; i < NUM_OF_CAM; i++)
trackerData[i].readIntrinsicParameter(CAM_NAMES[i]);//读取相机内参,保存到CAM_NAMES[i],如何获取相机的内呢???
// 3.判断是否加入鱼眼mask 来去除边缘噪声
if(FISHEYE)
{
for (int i = 0; i < NUM_OF_CAM; i++)
{
trackerData[i].fisheye_mask = cv::imread(FISHEYE_MASK, 0);
if(!trackerData[i].fisheye_mask.data)
{
ROS_INFO("load mask fail");
ROS_BREAK();
}
else
ROS_INFO("load mask success");
}
}
// 4.订阅话题IMAGE_TOPIC(/cam0/image_raw),有图像发布到这个话题时，执行回调函数img_callback
ros::Subscriber sub_img = n.subscribe(IMAGE_TOPIC, 100, img_callback);
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
feature_trackers.cpp/feature_trackers.h：构建feature_trackers类，实现特征点跟踪的函数 位于feature_tracker.cpp 与feature_tracker.h中,我们看一下各个函数的功能:
feature_trackers类的成员变量
cv::Mat mask;//图像掩码 cv::Mat fisheye_mask;//鱼眼相机mask，用来去除边缘噪点<span class="token comment">// prev_img是上一次发布的帧的图像数据</span> <span class="token comment">// cur_img是光流跟踪的前一帧的图像数据</span> <span class="token comment">// forw_img是光流跟踪的后一帧的图像数据</span> cv<span class="token operator">::</span>Mat prev_img<span class="token punctuation">,</span> cur_img<span class="token punctuation">,</span> forw_img<span class="token punctuation">;</span> vector<span class="token operator"><</span>cv<span class="token operator">::</span>Point2f<span class="token operator">></span> n_pts<span class="token punctuation">;</span><span class="token comment">//每一帧中新提取的特征点</span> vector<span class="token operator"><</span>cv<span class="token operator">::</span>Point2f<span class="token operator">></span> prev_pts<span class="token punctuation">,</span> cur_pts<span class="token punctuation">,</span> forw_pts<span class="token punctuation">;</span><span class="token comment">//对应的图像特征点</span> vector<span class="token operator"><</span>cv<span class="token operator">::</span>Point2f<span class="token operator">></span> prev_un_pts<span class="token punctuation">,</span> cur_un_pts<span class="token punctuation">;</span><span class="token comment">//归一化相机坐标系下的坐标</span> vector<span class="token operator"><</span>cv<span class="token operator">::</span>Point2f<span class="token operator">></span> pts_velocity<span class="token punctuation">;</span><span class="token comment">//当前帧相对前一帧特征点沿x,y方向的像素移动速度</span> vector<span class="token operator"><</span><span class="token keyword">int</span><span class="token operator">></span> ids<span class="token punctuation">;</span><span class="token comment">//能够被跟踪到的特征点的id</span> vector<span class="token operator"><</span><span class="token keyword">int</span><span class="token operator">></span> track_cnt<span class="token punctuation">;</span><span class="token comment">//当前帧forw_img中每个特征点被追踪的时间次数</span> map<span class="token operator"><</span><span class="token keyword">int</span><span class="token punctuation">,</span> cv<span class="token operator">::</span>Point2f<span class="token operator">></span> cur_un_pts_map<span class="token punctuation">;</span> map<span class="token operator"><</span><span class="token keyword">int</span><span class="token punctuation">,</span> cv<span class="token operator">::</span>Point2f<span class="token operator">></span> prev_un_pts_map<span class="token punctuation">;</span> camodocal<span class="token operator">::</span>CameraPtr m_camera<span class="token punctuation">;</span><span class="token comment">//相机模型</span> <span class="token keyword">double</span> cur_time<span class="token punctuation">;</span> <span class="token keyword">double</span> prev_time<span class="token punctuation">;</span> <span class="token keyword">static</span> <span class="token keyword">int</span> n_id<span class="token punctuation">;</span><span class="token comment">//用来作为特征点id，每检测到一个新的特征点，就将n_id作为该特征点的id，然后n_id加1</span>
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
可以看到，视觉跟踪的主要实现方法集中在feature_trackers_node.cpp的回调函数和feature_trackers类中，接下来将从main()函数开始对加粗的函数进行具体解读:
一、feature_tracker_node.cpp
主要分为两部分：int main（）函数为程序入口，void img_callback（）为ROS的回调函数，对图像进行特征点追踪，处理和发布。
int main(int argc, char **argv) { ros::init(argc, argv, "feature_tracker"); //ros初始化和设置句柄 ros::NodeHandle n("~");// 命名空间为/node_namespace/node_name<span class="token comment">//设置logger的级别。 只有级别大于或等于level的日志记录消息才会得到处理。</span> ros<span class="token operator">::</span>console<span class="token operator">::</span><span class="token function">set_logger_level</span><span class="token punctuation">(</span>ROSCONSOLE_DEFAULT_NAME<span class="token punctuation">,</span> ros<span class="token operator">::</span>console<span class="token operator">::</span>levels<span class="token operator">::</span>Info<span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token comment">// 1.读取yaml中的一些配置参数 地址为config->euroc->euroc_config.yaml</span> <span class="token function">readParameters</span><span class="token punctuation">(</span>n<span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token comment">// 2.读取每个相机实例对应的相机内参，NUM_OF_CAM 经常为1，单目</span> <span class="token keyword">for</span> <span class="token punctuation">(</span><span class="token keyword">int</span> i <span class="token operator">=</span> <span class="token number">0</span><span class="token punctuation">;</span> i <span class="token operator"><</span> NUM_OF_CAM<span class="token punctuation">;</span> i<span class="token operator">++</span><span class="token punctuation">)</span> trackerData<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">.</span><span class="token function">readIntrinsicParameter</span><span class="token punctuation">(</span>CAM_NAMES<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token comment">// 3.判断是否加入鱼眼mask 来去除边缘噪声</span> <span class="token keyword">if</span><span class="token punctuation">(</span>FISHEYE<span class="token punctuation">)</span> <span class="token punctuation">{<!-- --></span> <span class="token keyword">for</span> <span class="token punctuation">(</span><span class="token keyword">int</span> i <span class="token operator">=</span> <span class="token number">0</span><span class="token punctuation">;</span> i <span class="token operator"><</span> NUM_OF_CAM<span class="token punctuation">;</span> i<span class="token operator">++</span><span class="token punctuation">)</span> <span class="token punctuation">{<!-- --></span> trackerData<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">.</span>fisheye_mask <span class="token operator">=</span> cv<span class="token operator">::</span><span class="token function">imread</span><span class="token punctuation">(</span>FISHEYE_MASK<span class="token punctuation">,</span> <span class="token number">0</span><span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token keyword">if</span><span class="token punctuation">(</span><span class="token operator">!</span>trackerData<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">.</span>fisheye_mask<span class="token punctuation">.</span>data<span class="token punctuation">)</span> <span class="token punctuation">{<!-- --></span> <span class="token function">ROS_INFO</span><span class="token punctuation">(</span><span class="token string">"load mask fail"</span><span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token function">ROS_BREAK</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token punctuation">}</span> <span class="token keyword">else</span> <span class="token function">ROS_INFO</span><span class="token punctuation">(</span><span class="token string">"load mask success"</span><span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token punctuation">}</span> <span class="token punctuation">}</span> <span class="token comment">// 4.订阅话题IMAGE_TOPIC(/cam0/image_raw),有图像发布到这个话题时，执行回调函数img_callback</span> ros<span class="token operator">::</span>Subscriber sub_img <span class="token operator">=</span> n<span class="token punctuation">.</span><span class="token function">subscribe</span><span class="token punctuation">(</span>IMAGE_TOPIC<span class="token punctuation">,</span> <span class="token number">100</span><span class="token punctuation">,</span> img_callback<span class="token punctuation">)</span><span class="token punctuation">;</span>
/*
//创建话题的subscriber
ros::Subscriber subscribe(const string &topic, uint32_t queue_size, void(*)(M));
//第一个参数是订阅话题的名称
//第二个参数是订阅队列的长度,如果受到的消息都没来得及处理,那么新消息入队旧消息就会出队
//第三个参数是回调函数指针,指向回调函数来处理接收到的消息
/
pub_img = n.advertise<sensor_msgs::PointCloud>(“feature”, 1000);
pub_match = n.advertise<sensor_msgs::Image>(“feature_img”,1000);
pub_restart = n.advertise<std_msgs::Bool>(“restart”,1000);
/
if (SHOW_TRACK)
cv::namedWindow(“vis”, cv::WINDOW_NORMAL);
*/
ros::spin();
return 0;
}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
1、void ing_callback(const sensor_msgs::ImageConstPtr &img_msg)
//ROS的回调函数，主要功能包括：readImage()函数对新来的图像使用光流法进行特征点跟踪，
// 并将追踪的特征点封装成feature_points发布到pub_img的话题下，将图像封装成ptr发布在pub_match下。
//例如
//pub_img.publish(feature_points);
//pub_match.publish(ptr->toImageMsg())
- 1
- 2
- 3
- 4
- 5
我们从流程图可以看到内部函数之间的调用顺序,先看一下各个函数的功能
// 1.判断是否是第一帧
if(first_image_flag)
{
first_image_flag = false;// 更新：不再是第一帧图像
first_image_time = img_msg->header.stamp.toSec();//记录第一个图像帧的时间
last_image_time = img_msg->header.stamp.toSec();
return;
}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
// 2.通过时间间隔判断相机数据流是否稳定，有问题则restart
// detect unstable camera stream
if (img_msg->header.stamp.toSec() - last_image_time > 1.0 || img_msg->header.stamp.toSec() < last_image_time)
{
ROS_WARN("image discontinue! reset the feature tracker!");
first_image_flag = true;
last_image_time = 0;
pub_count = 1;
std_msgs::Bool restart_flag;
restart_flag.data = true;
pub_restart.publish(restart_flag); // 复位
return;
}
last_image_time = img_msg->header.stamp.toSec(); // 更新上一帧图像时间戳
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
// frequency control
// 3.发布频率控制，保证每秒钟处理的Image小于FREQ，频率控制在10HZ以内
// 并不是每读入一帧图像，就要发布特征点
// 判断间隔时间内的发布次数
if (round(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time)) <= FREQ)
{
PUB_THIS_FRAME = true; // 发布当前帧
// reset the frequency control
// 时间间隔内的发布频率十分接近设定频率时，更新时间间隔起始时刻，并将数据发布次数置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;
}
}
else
PUB_THIS_FRAME = false;
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
//OpenCV中，图像是以Mat矩阵的形式存储的，与ROS定义的图像消息的格式不同，需要利用cv_bridge进行转换 // 4.将图像编码8UC1转换为mono8,单色8bit cv_bridge::CvImageConstPtr ptr; if (img_msg->encoding == "8UC1") { sensor_msgs::Image img; // ROS图像消息 img.header = img_msg->header; img.height = img_msg->height; img.width = img_msg->width; img.is_bigendian = img_msg->is_bigendian; img.step = img_msg->step; img.data = img_msg->data; img.encoding = "mono8"; //cv_bridge将ROS的图像消息转换为OpenCV图像格式时都是通过CvImage类实现的。 //cv_bridge提供了两种方式转换为CvImage类，分别为复制（copy）和共享（share） // CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr& source, // const std::string& encoding = std::string()); // CvImagePtr toCvCopy(const sensor_msgs::Image& source, // const std::string& encoding = std::string());ptr <span class="token operator">=</span> cv_bridge<span class="token operator">::</span><span class="token function">toCvCopy</span><span class="token punctuation">(</span>img<span class="token punctuation">,</span> sensor_msgs<span class="token operator">::</span>image_encodings<span class="token operator">::</span>MONO8<span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token punctuation">}</span> <span class="token keyword">else</span> ptr <span class="token operator">=</span> cv_bridge<span class="token operator">::</span><span class="token function">toCvCopy</span><span class="token punctuation">(</span>img_msg<span class="token punctuation">,</span> sensor_msgs<span class="token operator">::</span>image_encodings<span class="token operator">::</span>MONO8<span class="token punctuation">)</span><span class="token punctuation">;</span> cv<span class="token operator">::</span>Mat show_img <span class="token operator">=</span> ptr<span class="token operator">-</span><span class="token operator">></span>image<span class="token punctuation">;</span> TicToc t_r<span class="token punctuation">;</span>
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
在这里函数进入readImage(); 读取图像数据并处理,
// 5. 重要！！！trackerData[i].readImage读取图像数据
for (int i = 0; i < NUM_OF_CAM; i++)
{
ROS_DEBUG("processing camera %d", i);
if (i != 1 || !STEREO_TRACK) //单目
//cur_img 上一针图像
//forw_img 当前针图像
//cur_pts 上一针的点坐标
//forw_pts 当前针的点坐标
//ids 每个点的ids
//track_cnt 每个点被跟踪的次数
//cur_un_=ts最新针的归一化相机洗坐标
trackerData[i].readImage(ptr->image.rowRange(ROW * i, ROW * (i + 1)), img_msg->header.stamp.toSec());
else //双目
{
if (EQUALIZE)
{
cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE();
clahe->apply(ptr->image.rowRange(ROW * i, ROW * (i + 1)), trackerData[i].cur_img);
}
else
trackerData[i].cur_img = ptr->image.rowRange(ROW * i, ROW * (i + 1));
}
#if SHOW_UNDISTORTION
trackerData[i].showUndistortion(“undistrotion_” + std::to_string(i));
#endif
}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
// 6.更新全局ID
for (unsigned int i = 0;; i++)
{
bool completed = false;
for (int j = 0; j < NUM_OF_CAM; j++)
if (j != 1 || !STEREO_TRACK)
completed |= trackerData[j].updateID(i); // 更新feature的id
if (!completed)
break;
}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
// 6.更新全局ID // 7、将特征点id，矫正后归一化平面的3D点(x,y,z=1)，像素2D点(u,v)，像素的速度(vx,vy)， //封装成sensor_msgs::PointCloudPtr类型的feature_points实例中,发布到pub_img;<span class="token keyword">if</span> <span class="token punctuation">(</span>PUB_THIS_FRAME<span class="token punctuation">)</span> <span class="token punctuation">{<!-- --></span> pub_count<span class="token operator">++</span><span class="token punctuation">;</span> <span class="token comment">// 重要！！！ feature_points</span> sensor_msgs<span class="token operator">::</span>PointCloudPtr <span class="token function">feature_points</span><span class="token punctuation">(</span><span class="token keyword">new</span> sensor_msgs<span class="token operator">::</span>PointCloud<span class="token punctuation">)</span><span class="token punctuation">;</span> sensor_msgs<span class="token operator">::</span>ChannelFloat32 id_of_point<span class="token punctuation">;</span> sensor_msgs<span class="token operator">::</span>ChannelFloat32 u_of_point<span class="token punctuation">;</span> sensor_msgs<span class="token operator">::</span>ChannelFloat32 v_of_point<span class="token punctuation">;</span> sensor_msgs<span class="token operator">::</span>ChannelFloat32 velocity_x_of_point<span class="token punctuation">;</span> sensor_msgs<span class="token operator">::</span>ChannelFloat32 velocity_y_of_point<span class="token punctuation">;</span> feature_points<span class="token operator">-</span><span class="token operator">></span>header <span class="token operator">=</span> img_msg<span class="token operator">-</span><span class="token operator">></span>header<span class="token punctuation">;</span> feature_points<span class="token operator">-</span><span class="token operator">></span>header<span class="token punctuation">.</span>frame_id <span class="token operator">=</span> <span class="token string">"world"</span><span class="token punctuation">;</span> vector<span class="token operator"><</span>set<span class="token operator"><</span><span class="token keyword">int</span><span class="token operator">>></span> <span class="token function">hash_ids</span><span class="token punctuation">(</span>NUM_OF_CAM<span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token comment">// 哈希表id</span> <span class="token keyword">for</span> <span class="token punctuation">(</span><span class="token keyword">int</span> i <span class="token operator">=</span> <span class="token number">0</span><span class="token punctuation">;</span> i <span class="token operator"><</span> NUM_OF_CAM<span class="token punctuation">;</span> i<span class="token operator">++</span><span class="token punctuation">)</span> <span class="token comment">// 循环相机数量</span> <span class="token punctuation">{<!-- --></span> <span class="token keyword">auto</span> <span class="token operator">&</span>un_pts <span class="token operator">=</span> trackerData<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">.</span>cur_un_pts<span class="token punctuation">;</span> <span class="token keyword">auto</span> <span class="token operator">&</span>cur_pts <span class="token operator">=</span> trackerData<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">.</span>cur_pts<span class="token punctuation">;</span> <span class="token keyword">auto</span> <span class="token operator">&</span>ids <span class="token operator">=</span> trackerData<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">.</span>ids<span class="token punctuation">;</span> <span class="token keyword">auto</span> <span class="token operator">&</span>pts_velocity <span class="token operator">=</span> trackerData<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">.</span>pts_velocity<span class="token punctuation">;</span> <span class="token keyword">for</span> <span class="token punctuation">(</span><span class="token keyword">unsigned</span> <span class="token keyword">int</span> j <span class="token operator">=</span> <span class="token number">0</span><span class="token punctuation">;</span> j <span class="token operator"><</span> ids<span class="token punctuation">.</span><span class="token function">size</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">;</span> j<span class="token operator">++</span><span class="token punctuation">)</span> <span class="token comment">// 特征点数量</span> <span class="token punctuation">{<!-- --></span> <span class="token keyword">if</span> <span class="token punctuation">(</span>trackerData<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">.</span>track_cnt<span class="token punctuation">[</span>j<span class="token punctuation">]</span> <span class="token operator">></span> <span class="token number">1</span><span class="token punctuation">)</span> <span class="token comment">// 该特征点被追踪次数大于1</span> <span class="token punctuation">{<!-- --></span> <span class="token keyword">int</span> p_id <span class="token operator">=</span> ids<span class="token punctuation">[</span>j<span class="token punctuation">]</span><span class="token punctuation">;</span> hash_ids<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">.</span><span class="token function">insert</span><span class="token punctuation">(</span>p_id<span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token comment">// 哈希表id insert</span> geometry_msgs<span class="token operator">::</span>Point32 p<span class="token punctuation">;</span> <span class="token comment">// 大规模点云信息</span> p<span class="token punctuation">.</span>x <span class="token operator">=</span> un_pts<span class="token punctuation">[</span>j<span class="token punctuation">]</span><span class="token punctuation">.</span>x<span class="token punctuation">;</span> p<span class="token punctuation">.</span>y <span class="token operator">=</span> un_pts<span class="token punctuation">[</span>j<span class="token punctuation">]</span><span class="token punctuation">.</span>y<span class="token punctuation">;</span> p<span class="token punctuation">.</span>z <span class="token operator">=</span> <span class="token number">1</span><span class="token punctuation">;</span> feature_points<span class="token operator">-</span><span class="token operator">></span>points<span class="token punctuation">.</span><span class="token function">push_back</span><span class="token punctuation">(</span>p<span class="token punctuation">)</span><span class="token punctuation">;</span> id_of_point<span class="token punctuation">.</span>values<span class="token punctuation">.</span><span class="token function">push_back</span><span class="token punctuation">(</span>p_id <span class="token operator">*</span> NUM_OF_CAM <span class="token operator">+</span> i<span class="token punctuation">)</span><span class="token punctuation">;</span> u_of_point<span class="token punctuation">.</span>values<span class="token punctuation">.</span><span class="token function">push_back</span><span class="token punctuation">(</span>cur_pts<span class="token punctuation">[</span>j<span class="token punctuation">]</span><span class="token punctuation">.</span>x<span class="token punctuation">)</span><span class="token punctuation">;</span> v_of_point<span class="token punctuation">.</span>values<span class="token punctuation">.</span><span class="token function">push_back</span><span class="token punctuation">(</span>cur_pts<span class="token punctuation">[</span>j<span class="token punctuation">]</span><span class="token punctuation">.</span>y<span class="token punctuation">)</span><span class="token punctuation">;</span> velocity_x_of_point<span class="token punctuation">.</span>values<span class="token punctuation">.</span><span class="token function">push_back</span><span class="token punctuation">(</span>pts_velocity<span class="token punctuation">[</span>j<span class="token punctuation">]</span><span class="token punctuation">.</span>x<span class="token punctuation">)</span><span class="token punctuation">;</span> velocity_y_of_point<span class="token punctuation">.</span>values<span class="token punctuation">.</span><span class="token function">push_back</span><span class="token punctuation">(</span>pts_velocity<span class="token punctuation">[</span>j<span class="token punctuation">]</span><span class="token punctuation">.</span>y<span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token punctuation">}</span> <span class="token punctuation">}</span> <span class="token punctuation">}</span> feature_points<span class="token operator">-</span><span class="token operator">></span>channels<span class="token punctuation">.</span><span class="token function">push_back</span><span class="token punctuation">(</span>id_of_point<span class="token punctuation">)</span><span class="token punctuation">;</span> feature_points<span class="token operator">-</span><span class="token operator">></span>channels<span class="token punctuation">.</span><span class="token function">push_back</span><span class="token punctuation">(</span>u_of_point<span class="token punctuation">)</span><span class="token punctuation">;</span> feature_points<span class="token operator">-</span><span class="token operator">></span>channels<span class="token punctuation">.</span><span class="token function">push_back</span><span class="token punctuation">(</span>v_of_point<span class="token punctuation">)</span><span class="token punctuation">;</span> feature_points<span class="token operator">-</span><span class="token operator">></span>channels<span class="token punctuation">.</span><span class="token function">push_back</span><span class="token punctuation">(</span>velocity_x_of_point<span class="token punctuation">)</span><span class="token punctuation">;</span> feature_points<span class="token operator">-</span><span class="token operator">></span>channels<span class="token punctuation">.</span><span class="token function">push_back</span><span class="token punctuation">(</span>velocity_y_of_point<span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token function">ROS_DEBUG</span><span class="token punctuation">(</span><span class="token string">"publish %f, at %f"</span><span class="token punctuation">,</span> feature_points<span class="token operator">-</span><span class="token operator">></span>header<span class="token punctuation">.</span>stamp<span class="token punctuation">.</span><span class="token function">toSec</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">,</span> ros<span class="token operator">::</span>Time<span class="token operator">::</span><span class="token function">now</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">.</span><span class="token function">toSec</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token comment">// skip the first image; since no optical speed on frist image</span> <span class="token comment">// 第一帧不发布，因为没有光流速度</span> <span class="token keyword">if</span> <span class="token punctuation">(</span><span class="token operator">!</span>init_pub<span class="token punctuation">)</span> <span class="token punctuation">{<!-- --></span> init_pub <span class="token operator">=</span> <span class="token number">1</span><span class="token punctuation">;</span> <span class="token punctuation">}</span> <span class="token keyword">else</span> pub_img<span class="token punctuation">.</span><span class="token function">publish</span><span class="token punctuation">(</span>feature_points<span class="token punctuation">)</span><span class="token punctuation">;</span>
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
// 8、将图像封装到cv_bridge::cvtColor类型的ptr实例中发布到pub_match if (SHOW_TRACK) { ptr = cv_bridge::cvtColor(ptr, sensor_msgs::image_encodings::BGR8); //cv::Mat stereo_img(ROW * NUM_OF_CAM, COL, CV_8UC3); cv::Mat stereo_img = ptr->image;<span class="token keyword">for</span> <span class="token punctuation">(</span><span class="token keyword">int</span> i <span class="token operator">=</span> <span class="token number">0</span><span class="token punctuation">;</span> i <span class="token operator"><</span> NUM_OF_CAM<span class="token punctuation">;</span> i<span class="token operator">++</span><span class="token punctuation">)</span> <span class="token punctuation">{<!-- --></span> cv<span class="token operator">::</span>Mat tmp_img <span class="token operator">=</span> stereo_img<span class="token punctuation">.</span><span class="token function">rowRange</span><span class="token punctuation">(</span>i <span class="token operator">*</span> ROW<span class="token punctuation">,</span> <span class="token punctuation">(</span>i <span class="token operator">+</span> <span class="token number">1</span><span class="token punctuation">)</span> <span class="token operator">*</span> ROW<span class="token punctuation">)</span><span class="token punctuation">;</span> cv<span class="token operator">::</span><span class="token function">cvtColor</span><span class="token punctuation">(</span>show_img<span class="token punctuation">,</span> tmp_img<span class="token punctuation">,</span> CV_GRAY2RGB<span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token comment">// show_img灰度图转RGB（tmp_img）</span> <span class="token comment">//显示追踪状态，越红越好，越蓝越不行</span> <span class="token keyword">for</span> <span class="token punctuation">(</span><span class="token keyword">unsigned</span> <span class="token keyword">int</span> j <span class="token operator">=</span> <span class="token number">0</span><span class="token punctuation">;</span> j <span class="token operator"><</span> trackerData<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">.</span>cur_pts<span class="token punctuation">.</span><span class="token function">size</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">;</span> j<span class="token operator">++</span><span class="token punctuation">)</span> <span class="token punctuation">{<!-- --></span> <span class="token keyword">double</span> len <span class="token operator">=</span> std<span class="token operator">::</span><span class="token function">min</span><span class="token punctuation">(</span><span class="token number">1.0</span><span class="token punctuation">,</span> <span class="token number">1.0</span> <span class="token operator">*</span> trackerData<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">.</span>track_cnt<span class="token punctuation">[</span>j<span class="token punctuation">]</span> <span class="token operator">/</span> WINDOW_SIZE<span class="token punctuation">)</span><span class="token punctuation">;</span> cv<span class="token operator">::</span><span class="token function">circle</span><span class="token punctuation">(</span>tmp_img<span class="token punctuation">,</span> trackerData<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">.</span>cur_pts<span class="token punctuation">[</span>j<span class="token punctuation">]</span><span class="token punctuation">,</span> <span class="token number">2</span><span class="token punctuation">,</span> cv<span class="token operator">::</span><span class="token function">Scalar</span><span class="token punctuation">(</span><span class="token number">255</span> <span class="token operator">*</span> <span class="token punctuation">(</span><span class="token number">1</span> <span class="token operator">-</span> len<span class="token punctuation">)</span><span class="token punctuation">,</span> <span class="token number">0</span><span class="token punctuation">,</span> <span class="token number">255</span> <span class="token operator">*</span> len<span class="token punctuation">)</span><span class="token punctuation">,</span> <span class="token number">2</span><span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token comment">//draw speed line</span> <span class="token comment">/* Vector2d tmp_cur_un_pts (trackerData[i].cur_un_pts[j].x, trackerData[i].cur_un_pts[j].y); Vector2d tmp_pts_velocity (trackerData[i].pts_velocity[j].x, trackerData[i].pts_velocity[j].y); Vector3d tmp_prev_un_pts; tmp_prev_un_pts.head(2) = tmp_cur_un_pts - 0.10 * tmp_pts_velocity; tmp_prev_un_pts.z() = 1; Vector2d tmp_prev_uv; trackerData[i].m_camera->spaceToPlane(tmp_prev_un_pts, tmp_prev_uv); cv::line(tmp_img, trackerData[i].cur_pts[j], cv::Point2f(tmp_prev_uv.x(), tmp_prev_uv.y()), cv::Scalar(255 , 0, 0), 1 , 8, 0); */</span> <span class="token comment">//char name[10];</span> <span class="token comment">//sprintf(name, "%d", trackerData[i].ids[j]);</span> <span class="token comment">//cv::putText(tmp_img, name, trackerData[i].cur_pts[j], cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 0, 0));</span> <span class="token punctuation">}</span> <span class="token punctuation">}</span> <span class="token comment">//cv::imshow("vis", stereo_img);</span> <span class="token comment">//cv::waitKey(5);</span> pub_match<span class="token punctuation">.</span><span class="token function">publish</span><span class="token punctuation">(</span>ptr<span class="token operator">-</span><span class="token operator">></span><span class="token function">toImageMsg</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token punctuation">}</span> <span class="token punctuation">}</span> <span class="token function">ROS_INFO</span><span class="token punctuation">(</span><span class="token string">"whole feature tracker processing costs: %f"</span><span class="token punctuation">,</span> t_r<span class="token punctuation">.</span><span class="token function">toc</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">)</span><span class="token punctuation">;</span>
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
二、feature_tracker.cpp
void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time)
主要分为两部分：
1、特征点检测：Frame1，goofFeaturesToTrack()检测MAX_CNT个特征点，添加到forw_pts中
2、特征点跟踪：calOpticalFlowPyrLK跟踪，将跟踪失败的点剔除，跟踪成功的跟踪次数+1，调用goodFeaturesToTracl（）再检测出MAX_CNT-forw_pts.size()个特征点，添加到forw_pts中，调用updateID 更新ids。
undistortedPoints()中cur_un_pts为归一化相机坐标系下坐标，pts_velocity为当前帧相对于前一帧特征点沿着x、y方向的像素移动速度。
对图像使用光流法进行特征点跟踪，具体流程有：
①先调用createCLAHE() 对图像进行自适应直方图均衡化(如果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.
undistortedPoints() 对角点图像坐标去畸变矫正，并计算每个角点的速度
值得注意的是，当前帧为forw_pts,被追踪到的帧；curr_pts实际上是上一帧；
//对图像使用光流法进行特征点跟踪 void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time) { cv::Mat img; TicToc t_r; cur_time = _cur_time; // ①先调用createCLAHE() 对图像进行自适应直方图均衡化(如果EQUALIZE=1，表示太亮或则太暗) // 1.如果EQUALIZE=1，表示太亮或太暗，进行直方图均衡化处理 if (EQUALIZE) { //自适应直方图均衡 //createCLAHE(double clipLimit, Size tileGridSize) //CLAHE 用来对图像进行灰度变换，以达到提高图像对比度的目的。 cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));//size类中的两个数据成员叫做width和heigh TicToc t_c; clahe->apply(_img, img); ROS_DEBUG("CLAHE costs: %fms", t_c.toc()); } else img = _img;<span class="token comment">// 2. 判断当前帧图像forw_img是否为空</span> <span class="token keyword">if</span> <span class="token punctuation">(</span>forw_img<span class="token punctuation">.</span><span class="token function">empty</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">)</span> <span class="token punctuation">{<!-- --></span> <span class="token comment">//如果当前帧的图像数据forw_img为空，说明当前是第一次读入图像数据</span> <span class="token comment">//将读入的图像赋给当前帧forw_img，同时还赋给prev_img、cur_img</span> prev_img <span class="token operator">=</span> cur_img <span class="token operator">=</span> forw_img <span class="token operator">=</span> img<span class="token punctuation">;</span> <span class="token punctuation">}</span> <span class="token keyword">else</span> <span class="token punctuation">{<!-- --></span> <span class="token comment">//否则，说明之前就已经有图像读入，只需要更新当前帧forw_img的数据</span> forw_img <span class="token operator">=</span> img<span class="token punctuation">;</span> <span class="token punctuation">}</span> <span class="token comment">//此时forw_pts还保存的是上一帧图像中的特征点，所以把它清除</span> forw_pts<span class="token punctuation">.</span><span class="token function">clear</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token keyword">if</span> <span class="token punctuation">(</span>cur_pts<span class="token punctuation">.</span><span class="token function">size</span><span class="token punctuation">(</span><span class="token punctuation">)</span> <span class="token operator">></span> <span class="token number">0</span><span class="token punctuation">)</span><span class="token comment">// 前一帧有特征点</span> <span class="token punctuation">{<!-- --></span> TicToc t_o<span class="token punctuation">;</span> vector<span class="token operator"><</span>uchar<span class="token operator">></span> status<span class="token punctuation">;</span> vector<span class="token operator"><</span><span class="token keyword">float</span><span class="token operator">></span> err<span class="token punctuation">;</span> <span class="token comment">// 3. 调用cv::calcOpticalFlowPyrLK()对前一帧的特征点cur_pts进行LK金字塔光流跟踪，得到forw_pts</span> <span class="token comment">//status标记了从前一帧cur_img到forw_img特征点的跟踪状态，无法被追踪到的点标记为0</span> cv<span class="token operator">::</span><span class="token function">calcOpticalFlowPyrLK</span><span class="token punctuation">(</span>cur_img<span class="token punctuation">,</span> forw_img<span class="token punctuation">,</span> cur_pts<span class="token punctuation">,</span> forw_pts<span class="token punctuation">,</span> status<span class="token punctuation">,</span> err<span class="token punctuation">,</span> cv<span class="token operator">::</span><span class="token function">Size</span><span class="token punctuation">(</span><span class="token number">21</span><span class="token punctuation">,</span> <span class="token number">21</span><span class="token punctuation">)</span><span class="token punctuation">,</span> <span class="token number">3</span><span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token keyword">for</span> <span class="token punctuation">(</span><span class="token keyword">int</span> i <span class="token operator">=</span> <span class="token number">0</span><span class="token punctuation">;</span> i <span class="token operator"><</span> <span class="token keyword">int</span><span class="token punctuation">(</span>forw_pts<span class="token punctuation">.</span><span class="token function">size</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">)</span><span class="token punctuation">;</span> i<span class="token operator">++</span><span class="token punctuation">)</span> <span class="token keyword">if</span> <span class="token punctuation">(</span>status<span class="token punctuation">[</span>i<span class="token punctuation">]</span> <span class="token operator">&&</span> <span class="token operator">!</span><span class="token function">inBorder</span><span class="token punctuation">(</span>forw_pts<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">)</span><span class="token punctuation">)</span><span class="token comment">// 将当前帧跟踪的位于图像边界外的点标记为0</span> status<span class="token punctuation">[</span>i<span class="token punctuation">]</span> <span class="token operator">=</span> <span class="token number">0</span><span class="token punctuation">;</span> <span class="token comment">// 4. 根据status,把跟踪失败的点剔除</span> <span class="token comment">//不仅要从当前帧数据forw_pts中剔除，而且还要从cur_un_pts、prev_pts和cur_pts中剔除</span> <span class="token comment">//prev_pts和cur_pts中的特征点是一一对应的</span> <span class="token comment">//记录特征点id的ids，和记录特征点被跟踪次数的track_cnt也要剔除</span> <span class="token function">reduceVector</span><span class="token punctuation">(</span>prev_pts<span class="token punctuation">,</span> status<span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token function">reduceVector</span><span class="token punctuation">(</span>cur_pts<span class="token punctuation">,</span> status<span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token function">reduceVector</span><span class="token punctuation">(</span>forw_pts<span class="token punctuation">,</span> status<span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token function">reduceVector</span><span class="token punctuation">(</span>ids<span class="token punctuation">,</span> status<span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token function">reduceVector</span><span class="token punctuation">(</span>cur_un_pts<span class="token punctuation">,</span> status<span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token function">reduceVector</span><span class="token punctuation">(</span>track_cnt<span class="token punctuation">,</span> status<span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token function">ROS_DEBUG</span><span class="token punctuation">(</span><span class="token string">"temporal optical flow costs: %fms"</span><span class="token punctuation">,</span> t_o<span class="token punctuation">.</span><span class="token function">toc</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token punctuation">}</span> <span class="token comment">// 5. 光流追踪成功,特征点被成功跟踪的次数就加1</span> <span class="token comment">//数值代表被追踪的次数，数值越大，说明被追踪的就越久</span> <span class="token keyword">for</span> <span class="token punctuation">(</span><span class="token keyword">auto</span> <span class="token operator">&</span>n <span class="token operator">:</span> track_cnt<span class="token punctuation">)</span> n<span class="token operator">++</span><span class="token punctuation">;</span> <span class="token comment">//PUB_THIS_FRAME=1 需要发布特征点</span> <span class="token keyword">if</span> <span class="token punctuation">(</span>PUB_THIS_FRAME<span class="token punctuation">)</span> <span class="token punctuation">{<!-- --></span> <span class="token comment">// 6. rejectWithF()通过基本矩阵剔除outliers</span> <span class="token function">rejectWithF</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token comment">// 7. setMask()保证相邻的特征点之间要相隔30个像素,设置mask</span> <span class="token function">ROS_DEBUG</span><span class="token punctuation">(</span><span class="token string">"set mask begins"</span><span class="token punctuation">)</span><span class="token punctuation">;</span> TicToc t_m<span class="token punctuation">;</span> <span class="token function">setMask</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token function">ROS_DEBUG</span><span class="token punctuation">(</span><span class="token string">"set mask costs %fms"</span><span class="token punctuation">,</span> t_m<span class="token punctuation">.</span><span class="token function">toc</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token comment">// 8. 寻找新的特征点 goodFeaturesToTrack()</span> <span class="token function">ROS_DEBUG</span><span class="token punctuation">(</span><span class="token string">"detect feature begins"</span><span class="token punctuation">)</span><span class="token punctuation">;</span> TicToc t_t<span class="token punctuation">;</span> <span class="token comment">//计算是否需要提取新的特征点</span> <span class="token keyword">int</span> n_max_cnt <span class="token operator">=</span> MAX_CNT <span class="token operator">-</span> <span class="token keyword">static_cast</span><span class="token operator"><</span><span class="token keyword">int</span><span class="token operator">></span><span class="token punctuation">(</span>forw_pts<span class="token punctuation">.</span><span class="token function">size</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token keyword">if</span> <span class="token punctuation">(</span>n_max_cnt <span class="token operator">></span> <span class="token number">0</span><span class="token punctuation">)</span> <span class="token punctuation">{<!-- --></span> <span class="token keyword">if</span><span class="token punctuation">(</span>mask<span class="token punctuation">.</span><span class="token function">empty</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">)</span> cout <span class="token operator"><<</span> <span class="token string">"mask is empty "</span> <span class="token operator"><<</span> endl<span class="token punctuation">;</span> <span class="token keyword">if</span> <span class="token punctuation">(</span>mask<span class="token punctuation">.</span><span class="token function">type</span><span class="token punctuation">(</span><span class="token punctuation">)</span> <span class="token operator">!=</span> CV_8UC1<span class="token punctuation">)</span> cout <span class="token operator"><<</span> <span class="token string">"mask type wrong "</span> <span class="token operator"><<</span> endl<span class="token punctuation">;</span> <span class="token keyword">if</span> <span class="token punctuation">(</span>mask<span class="token punctuation">.</span><span class="token function">size</span><span class="token punctuation">(</span><span class="token punctuation">)</span> <span class="token operator">!=</span> forw_img<span class="token punctuation">.</span><span class="token function">size</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">)</span> cout <span class="token operator"><<</span> <span class="token string">"wrong size "</span> <span class="token operator"><<</span> endl<span class="token punctuation">;</span> <span class="token comment">/** *void cv::goodFeaturesToTrack( 在mask中不为0的区域检测新的特征点 * InputArray image, 输入图像 * OutputArray corners, 存放检测到的角点的vector * int maxCorners, 返回的角点的数量的最大值 * double qualityLevel, 角点质量水平的最低阈值（范围为0到1，质量最高角点的水平为1），小于该阈值的角点被拒绝 * double minDistance, 返回角点之间欧式距离的最小值 * InputArray mask = noArray(), 和输入图像具有相同大小，类型必须为CV_8UC1,用来描述图像中感兴趣的区域，只在感兴趣区域中检测角点 * int blockSize = 3, 计算协方差矩阵时的窗口大小 * bool useHarrisDetector = false, 指示是否使用Harris角点检测，如不指定则使用shi-tomasi算法 * double k = 0.04 Harris角点检测需要的k值 *) */</span> cv<span class="token operator">::</span><span class="token function">goodFeaturesToTrack</span><span class="token punctuation">(</span>forw_img<span class="token punctuation">,</span> n_pts<span class="token punctuation">,</span> MAX_CNT <span class="token operator">-</span> forw_pts<span class="token punctuation">.</span><span class="token function">size</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">,</span> <span class="token number">0.01</span><span class="token punctuation">,</span> MIN_DIST<span class="token punctuation">,</span> mask<span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token punctuation">}</span> <span class="token keyword">else</span> n_pts<span class="token punctuation">.</span><span class="token function">clear</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token function">ROS_DEBUG</span><span class="token punctuation">(</span><span class="token string">"detect feature costs: %fms"</span><span class="token punctuation">,</span> t_t<span class="token punctuation">.</span><span class="token function">toc</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token comment">// 9. addPoints()向forw_pts添加新的追踪点</span> <span class="token function">ROS_DEBUG</span><span class="token punctuation">(</span><span class="token string">"add feature begins"</span><span class="token punctuation">)</span><span class="token punctuation">;</span> TicToc t_a<span class="token punctuation">;</span> <span class="token comment">//添将新检测到的特征点n_pts添加到forw_pts中，id初始化-1,track_cnt初始化为1</span> <span class="token function">addPoints</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token function">ROS_DEBUG</span><span class="token punctuation">(</span><span class="token string">"selectFeature costs: %fms"</span><span class="token punctuation">,</span> t_a<span class="token punctuation">.</span><span class="token function">toc</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token punctuation">}</span> <span class="token comment">// 10. 更新帧、特征点</span> <span class="token comment">//当下一帧图像到来时，当前帧数据就成为了上一帧发布的数据</span> prev_img <span class="token operator">=</span> cur_img<span class="token punctuation">;</span> prev_pts <span class="token operator">=</span> cur_pts<span class="token punctuation">;</span> prev_un_pts <span class="token operator">=</span> cur_un_pts<span class="token punctuation">;</span> <span class="token comment">//把当前帧的数据forw_img、forw_pts赋给上一帧cur_img、cur_pts</span> cur_img <span class="token operator">=</span> forw_img<span class="token punctuation">;</span> cur_pts <span class="token operator">=</span> forw_pts<span class="token punctuation">;</span> <span class="token comment">// 11. 根据不同的相机模型去畸变矫正和转换到归一化坐标系上，计算速度</span> <span class="token function">undistortedPoints</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">;</span> prev_time <span class="token operator">=</span> cur_time<span class="token punctuation">;</span>
}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
- 61
- 62
- 63
- 64
- 65
- 66
- 67
- 68
- 69
- 70
- 71
- 72
- 73
- 74
- 75
- 76
- 77
- 78
- 79
- 80
- 81
- 82
- 83
- 84
- 85
- 86
- 87
- 88
- 89
- 90
- 91
- 92
- 93
- 94
- 95
- 96
- 97
- 98
- 99
- 100
- 101
- 102
- 103
- 104
- 105
- 106
- 107
- 108
- 109
- 110
- 111
- 112
- 113
- 114
- 115
- 116
- 117
- 118
- 119
- 120
- 121
- 122
- 123
- 124
- 125
- 126
- 127
- 128
- 129
- 130
- 131
- 132
- 133
- 134
- 135
- 136
- 137
1 inBorder（）函数
//判断跟踪的特特征点是否在图像边界内
bool inBorder(const cv::Point2f &pt)
{
const int BORDER_SIZE = 1;
int img_x = cvRound(pt.x);
int img_y = cvRound(pt.y);
//cvRound()：返回跟参数最接近的整数值，即四舍五入；
// cvFloor()：返回不大于参数的最大整数值，即向下取整；
// cvCeil()：返回不小于参数的最小整数值，即向上取整；
return BORDER_SIZE <= img_x && img_x < COL - BORDER_SIZE && BORDER_SIZE <= img_y && img_y < ROW - BORDER_SIZE;
}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
2 reduceVector()函数
//去除无法跟踪的特征点,status为0的点直接跳过，否则v[j++]=v[i]留下来，最后v.resize(j)根据最新的j安排内存
void reduceVector(vector<cv::Point2f> &v, vector<uchar> status)
{
int j = 0;
for (int i = 0; i < int(v.size()); i++)
if (status[i])
v[j++] = v[i];
v.resize(j);//resize()，设置大小（size）; reserve()，设置容量（capacity）;
}
void reduceVector(vector<int> &v, vector<uchar> status)
{
int j = 0;
for (int i = 0; i < int(v.size()); i++)
if (status[i])
v[j++] = v[i];
v.resize(j);
}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
3 setMask()函数
//对跟踪点进行排序并去除密集点 // 对跟踪到的特征点，按照被追踪到的次数排序并依次选点，使用mask进行类似非极大抑制，半径为30，去掉密集点， //使特征点分布均匀。对跟踪到的特征点从大到小排序并去除密集的点。 void FeatureTracker::setMask() { // 如果是鱼眼镜头直接clone即可，否则创建空白板 if(FISHEYE) mask = fisheye_mask.clone(); else mask = cv::Mat(ROW, COL, CV_8UC1, cv::Scalar(255));<span class="token comment">// 倾向于留下被追踪时间很长的特征点</span> <span class="token comment">// 构造(cnt，pts，id)序列，（追踪次数，当前特征点坐标，id）</span> vector<span class="token operator"><</span>pair<span class="token operator"><</span><span class="token keyword">int</span><span class="token punctuation">,</span> pair<span class="token operator"><</span>cv<span class="token operator">::</span>Point2f<span class="token punctuation">,</span> <span class="token keyword">int</span><span class="token operator">>></span><span class="token operator">></span> cnt_pts_id<span class="token punctuation">;</span> <span class="token keyword">for</span> <span class="token punctuation">(</span><span class="token keyword">unsigned</span> <span class="token keyword">int</span> i <span class="token operator">=</span> <span class="token number">0</span><span class="token punctuation">;</span> i <span class="token operator"><</span> forw_pts<span class="token punctuation">.</span><span class="token function">size</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">;</span> i<span class="token operator">++</span><span class="token punctuation">)</span> cnt_pts_id<span class="token punctuation">.</span><span class="token function">push_back</span><span class="token punctuation">(</span><span class="token function">make_pair</span><span class="token punctuation">(</span>track_cnt<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">,</span> <span class="token function">make_pair</span><span class="token punctuation">(</span>forw_pts<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">,</span> ids<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">)</span><span class="token punctuation">)</span><span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token comment">// 对光流跟踪到的特征点forw_pts，按照被跟踪到的次数cnt从大到小排序（lambda表达式）</span> <span class="token function">sort</span><span class="token punctuation">(</span>cnt_pts_id<span class="token punctuation">.</span><span class="token function">begin</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">,</span> cnt_pts_id<span class="token punctuation">.</span><span class="token function">end</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">,</span> <span class="token punctuation">[</span><span class="token punctuation">]</span><span class="token punctuation">(</span><span class="token keyword">const</span> pair<span class="token operator"><</span><span class="token keyword">int</span><span class="token punctuation">,</span> pair<span class="token operator"><</span>cv<span class="token operator">::</span>Point2f<span class="token punctuation">,</span> <span class="token keyword">int</span><span class="token operator">>></span> <span class="token operator">&</span>a<span class="token punctuation">,</span> <span class="token keyword">const</span> pair<span class="token operator"><</span><span class="token keyword">int</span><span class="token punctuation">,</span> pair<span class="token operator"><</span>cv<span class="token operator">::</span>Point2f<span class="token punctuation">,</span> <span class="token keyword">int</span><span class="token operator">>></span> <span class="token operator">&</span>b<span class="token punctuation">)</span> <span class="token punctuation">{<!-- --></span> <span class="token keyword">return</span> a<span class="token punctuation">.</span>first <span class="token operator">></span> b<span class="token punctuation">.</span>first<span class="token punctuation">;</span> <span class="token punctuation">}</span><span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token comment">//清空cnt，pts，id并重新存入</span> forw_pts<span class="token punctuation">.</span><span class="token function">clear</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">;</span> ids<span class="token punctuation">.</span><span class="token function">clear</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">;</span> track_cnt<span class="token punctuation">.</span><span class="token function">clear</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token keyword">for</span> <span class="token punctuation">(</span><span class="token keyword">auto</span> <span class="token operator">&</span>it <span class="token operator">:</span> cnt_pts_id<span class="token punctuation">)</span> <span class="token punctuation">{<!-- --></span> <span class="token keyword">if</span> <span class="token punctuation">(</span>mask<span class="token punctuation">.</span>at<span class="token operator"><</span>uchar<span class="token operator">></span><span class="token punctuation">(</span>it<span class="token punctuation">.</span>second<span class="token punctuation">.</span>first<span class="token punctuation">)</span> <span class="token operator">==</span> <span class="token number">255</span><span class="token punctuation">)</span><span class="token comment">// 这个特征点对应的mask值为255，表明点是黑的，还没占有</span> <span class="token punctuation">{<!-- --></span> <span class="token comment">//则保留当前特征点，将对应的特征点位置pts，id，被追踪次数cnt分别存入</span> forw_pts<span class="token punctuation">.</span><span class="token function">push_back</span><span class="token punctuation">(</span>it<span class="token punctuation">.</span>second<span class="token punctuation">.</span>first<span class="token punctuation">)</span><span class="token punctuation">;</span> ids<span class="token punctuation">.</span><span class="token function">push_back</span><span class="token punctuation">(</span>it<span class="token punctuation">.</span>second<span class="token punctuation">.</span>second<span class="token punctuation">)</span><span class="token punctuation">;</span> track_cnt<span class="token punctuation">.</span><span class="token function">push_back</span><span class="token punctuation">(</span>it<span class="token punctuation">.</span>first<span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token comment">//在mask中将当前特征点周围半径为MIN_DIST的区域设置为0，后面不再选取该区域内的点（使跟踪点不集中在一个区域上）</span> cv<span class="token operator">::</span><span class="token function">circle</span><span class="token punctuation">(</span>mask<span class="token punctuation">,</span> it<span class="token punctuation">.</span>second<span class="token punctuation">.</span>first<span class="token punctuation">,</span> MIN_DIST<span class="token punctuation">,</span> <span class="token number">0</span><span class="token punctuation">,</span> <span class="token operator">-</span><span class="token number">1</span><span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token punctuation">}</span> <span class="token punctuation">}</span>
}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
4 addPoints()函数
void FeatureTracker::addPoints()
{
for (auto &p : n_pts)
{
forw_pts.push_back(p);
ids.push_back(-1);//新提取的特征点id初始化为-1
track_cnt.push_back(1);//新提取的特征点被跟踪的次数初始化为1
}
}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
5 rejectWithF()函数
// 通过F矩阵去除outliers // 首先把特征点转化到归一化相机坐标系，然后计算F矩阵，再根据status清除为0的特征点。 void FeatureTracker::rejectWithF() { if (forw_pts.size() >= 8)// 当前帧（追踪上）特征点数量足够多 { ROS_DEBUG("FM ransac begins"); TicToc t_f; // 1.遍历所有特征点，转化为归一化相机坐标系 vector<cv::Point2f> un_cur_pts(cur_pts.size()), un_forw_pts(forw_pts.size()); for (unsigned int i = 0; i < cur_pts.size(); i++)//遍历上一帧所有特征点 { Eigen::Vector3d tmp_p;<span class="token comment">//对于PINHOLE（针孔相机）可将像素坐标转换到归一化平面并去畸变。根据不同的相机模型将二维坐标转换到三维坐标</span> m_camera<span class="token operator">-</span><span class="token operator">></span><span class="token function">liftProjective</span><span class="token punctuation">(</span>Eigen<span class="token operator">::</span><span class="token function">Vector2d</span><span class="token punctuation">(</span>cur_pts<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">.</span>x<span class="token punctuation">,</span> cur_pts<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">.</span>y<span class="token punctuation">)</span><span class="token punctuation">,</span> tmp_p<span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token comment">//转换为归一化像素坐标，上一帧和当前帧</span> tmp_p<span class="token punctuation">.</span><span class="token function">x</span><span class="token punctuation">(</span><span class="token punctuation">)</span> <span class="token operator">=</span> FOCAL_LENGTH <span class="token operator">*</span> tmp_p<span class="token punctuation">.</span><span class="token function">x</span><span class="token punctuation">(</span><span class="token punctuation">)</span> <span class="token operator">/</span> tmp_p<span class="token punctuation">.</span><span class="token function">z</span><span class="token punctuation">(</span><span class="token punctuation">)</span> <span class="token operator">+</span> COL <span class="token operator">/</span> <span class="token number">2.0</span><span class="token punctuation">;</span> tmp_p<span class="token punctuation">.</span><span class="token function">y</span><span class="token punctuation">(</span><span class="token punctuation">)</span> <span class="token operator">=</span> FOCAL_LENGTH <span class="token operator">*</span> tmp_p<span class="token punctuation">.</span><span class="token function">y</span><span class="token punctuation">(</span><span class="token punctuation">)</span> <span class="token operator">/</span> tmp_p<span class="token punctuation">.</span><span class="token function">z</span><span class="token punctuation">(</span><span class="token punctuation">)</span> <span class="token operator">+</span> ROW <span class="token operator">/</span> <span class="token number">2.0</span><span class="token punctuation">;</span> un_cur_pts<span class="token punctuation">[</span>i<span class="token punctuation">]</span> <span class="token operator">=</span> cv<span class="token operator">::</span><span class="token function">Point2f</span><span class="token punctuation">(</span>tmp_p<span class="token punctuation">.</span><span class="token function">x</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">,</span> tmp_p<span class="token punctuation">.</span><span class="token function">y</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">)</span><span class="token punctuation">;</span> m_camera<span class="token operator">-</span><span class="token operator">></span><span class="token function">liftProjective</span><span class="token punctuation">(</span>Eigen<span class="token operator">::</span><span class="token function">Vector2d</span><span class="token punctuation">(</span>forw_pts<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">.</span>x<span class="token punctuation">,</span> forw_pts<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">.</span>y<span class="token punctuation">)</span><span class="token punctuation">,</span> tmp_p<span class="token punctuation">)</span><span class="token punctuation">;</span> tmp_p<span class="token punctuation">.</span><span class="token function">x</span><span class="token punctuation">(</span><span class="token punctuation">)</span> <span class="token operator">=</span> FOCAL_LENGTH <span class="token operator">*</span> tmp_p<span class="token punctuation">.</span><span class="token function">x</span><span class="token punctuation">(</span><span class="token punctuation">)</span> <span class="token operator">/</span> tmp_p<span class="token punctuation">.</span><span class="token function">z</span><span class="token punctuation">(</span><span class="token punctuation">)</span> <span class="token operator">+</span> COL <span class="token operator">/</span> <span class="token number">2.0</span><span class="token punctuation">;</span> tmp_p<span class="token punctuation">.</span><span class="token function">y</span><span class="token punctuation">(</span><span class="token punctuation">)</span> <span class="token operator">=</span> FOCAL_LENGTH <span class="token operator">*</span> tmp_p<span class="token punctuation">.</span><span class="token function">y</span><span class="token punctuation">(</span><span class="token punctuation">)</span> <span class="token operator">/</span> tmp_p<span class="token punctuation">.</span><span class="token function">z</span><span class="token punctuation">(</span><span class="token punctuation">)</span> <span class="token operator">+</span> ROW <span class="token operator">/</span> <span class="token number">2.0</span><span class="token punctuation">;</span> un_forw_pts<span class="token punctuation">[</span>i<span class="token punctuation">]</span> <span class="token operator">=</span> cv<span class="token operator">::</span><span class="token function">Point2f</span><span class="token punctuation">(</span>tmp_p<span class="token punctuation">.</span><span class="token function">x</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">,</span> tmp_p<span class="token punctuation">.</span><span class="token function">y</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token punctuation">}</span> vector<span class="token operator"><</span>uchar<span class="token operator">></span> status<span class="token punctuation">;</span> <span class="token comment">// 2. 调用cv::findFundamentalMat对un_cur_pts和un_forw_pts计算F矩阵，需要归一化相机系，z=1</span> cv<span class="token operator">::</span><span class="token function">findFundamentalMat</span><span class="token punctuation">(</span>un_cur_pts<span class="token punctuation">,</span> un_forw_pts<span class="token punctuation">,</span> cv<span class="token operator">::</span>FM_RANSAC<span class="token punctuation">,</span> F_THRESHOLD<span class="token punctuation">,</span> <span class="token number">0.99</span><span class="token punctuation">,</span> status<span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token keyword">int</span> size_a <span class="token operator">=</span> cur_pts<span class="token punctuation">.</span><span class="token function">size</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token comment">// 3. 根据status删除一些特征点</span> <span class="token function">reduceVector</span><span class="token punctuation">(</span>prev_pts<span class="token punctuation">,</span> status<span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token function">reduceVector</span><span class="token punctuation">(</span>cur_pts<span class="token punctuation">,</span> status<span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token function">reduceVector</span><span class="token punctuation">(</span>forw_pts<span class="token punctuation">,</span> status<span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token function">reduceVector</span><span class="token punctuation">(</span>cur_un_pts<span class="token punctuation">,</span> status<span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token function">reduceVector</span><span class="token punctuation">(</span>ids<span class="token punctuation">,</span> status<span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token function">reduceVector</span><span class="token punctuation">(</span>track_cnt<span class="token punctuation">,</span> status<span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token function">ROS_DEBUG</span><span class="token punctuation">(</span><span class="token string">"FM ransac: %d -> %lu: %f"</span><span class="token punctuation">,</span> size_a<span class="token punctuation">,</span> forw_pts<span class="token punctuation">.</span><span class="token function">size</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">,</span> <span class="token number">1.0</span> <span class="token operator">*</span> forw_pts<span class="token punctuation">.</span><span class="token function">size</span><span class="token punctuation">(</span><span class="token punctuation">)</span> <span class="token operator">/</span> size_a<span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token function">ROS_DEBUG</span><span class="token punctuation">(</span><span class="token string">"FM ransac costs: %fms"</span><span class="token punctuation">,</span> t_f<span class="token punctuation">.</span><span class="token function">toc</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token punctuation">}</span>
}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
6 updateID()函数
bool FeatureTracker::updateID(unsigned int i)
{
if (i < ids.size())
{
if (ids[i] == -1)
ids[i] = n_id++;
return true;
}
else
return false;
}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
7 showUndistortion()函数
//显示去畸变矫正后的特征点 name为图像帧名称
void FeatureTracker::showUndistortion(const string &name)
{
cv::Mat undistortedImg(ROW + 600, COL + 600, CV_8UC1, cv::Scalar(0));
vector<Eigen::Vector2d> distortedp, undistortedp;
for (int i = 0; i < COL; i++)
for (int j = 0; j < ROW; j++)
{
Eigen::Vector2d a(i, j);
Eigen::Vector3d b;
m_camera->liftProjective(a, b);
distortedp.push_back(a);
undistortedp.push_back(Eigen::Vector2d(b.x() / b.z(), b.y() / b.z()));
//printf("%f,%f->%f,%f,%f\n)\n", a.x(), a.y(), b.x(), b.y(), b.z());
}
for (int i = 0; i < int(undistortedp.size()); i++)
{
cv::Mat pp(3, 1, CV_32FC1);
pp.at<float>(0, 0) = undistortedp[i].x() * FOCAL_LENGTH + COL / 2;
pp.at<float>(1, 0) = undistortedp[i].y() * FOCAL_LENGTH + ROW / 2;
pp.at<float>(2, 0) = 1.0;
//cout << trackerData[0].K << endl;
//printf("%lf %lf\n", p.at<float>(1, 0), p.at<float>(0, 0));
//printf("%lf %lf\n", pp.at<float>(1, 0), pp.at<float>(0, 0));
if (pp.at<float>(1, 0) + 300 >= 0 && pp.at<float>(1, 0) + 300 < ROW + 600 && pp.at<float>(0, 0) + 300 >= 0 && pp.at<float>(0, 0) + 300 < COL + 600)
{
undistortedImg.at<uchar>(pp.at<float>(1, 0) + 300, pp.at<float>(0, 0) + 300) = cur_img.at<uchar>(distortedp[i].y(), distortedp[i].x());
}
else
{
//ROS_ERROR("(%f %f) -> (%f %f)", distortedp[i].y, distortedp[i].x, pp.at<float>(1, 0), pp.at<float>(0, 0));
}
}
cv::imshow(name, undistortedImg);
cv::waitKey(0);
}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
8 undistortedPoints()函数
//1.对角点图像坐标进行去畸变矫正，转换到归一化坐标系上， //2.并计算每个角点的速度。 void FeatureTracker::undistortedPoints() { cur_un_pts.clear(); cur_un_pts_map.clear(); //cv::undistortPoints(cur_pts, un_pts, K, cv::Mat()); // 1.归一化相机坐标系 for (unsigned int i = 0; i < cur_pts.size(); i++)// 遍历所有特征点 { Eigen::Vector2d a(cur_pts[i].x, cur_pts[i].y); Eigen::Vector3d b; //根据不同的相机模型将二维坐标转换到归一化相机三维坐标系 m_camera->liftProjective(a, b);<span class="token comment">//再延伸到深度归一化平面上</span> cur_un_pts<span class="token punctuation">.</span><span class="token function">push_back</span><span class="token punctuation">(</span>cv<span class="token operator">::</span><span class="token function">Point2f</span><span class="token punctuation">(</span>b<span class="token punctuation">.</span><span class="token function">x</span><span class="token punctuation">(</span><span class="token punctuation">)</span> <span class="token operator">/</span> b<span class="token punctuation">.</span><span class="token function">z</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">,</span> b<span class="token punctuation">.</span><span class="token function">y</span><span class="token punctuation">(</span><span class="token punctuation">)</span> <span class="token operator">/</span> b<span class="token punctuation">.</span><span class="token function">z</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">)</span><span class="token punctuation">)</span><span class="token punctuation">;</span> cur_un_pts_map<span class="token punctuation">.</span><span class="token function">insert</span><span class="token punctuation">(</span><span class="token function">make_pair</span><span class="token punctuation">(</span>ids<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">,</span> cv<span class="token operator">::</span><span class="token function">Point2f</span><span class="token punctuation">(</span>b<span class="token punctuation">.</span><span class="token function">x</span><span class="token punctuation">(</span><span class="token punctuation">)</span> <span class="token operator">/</span> b<span class="token punctuation">.</span><span class="token function">z</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">,</span> b<span class="token punctuation">.</span><span class="token function">y</span><span class="token punctuation">(</span><span class="token punctuation">)</span> <span class="token operator">/</span> b<span class="token punctuation">.</span><span class="token function">z</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">)</span><span class="token punctuation">)</span><span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token comment">//printf("cur pts id %d %f %f", ids[i], cur_un_pts[i].x, cur_un_pts[i].y);</span> <span class="token punctuation">}</span> <span class="token comment">// 2.计算每个特征点的速度到pts_velocity</span> <span class="token comment">// caculate points velocity</span> <span class="token keyword">if</span> <span class="token punctuation">(</span><span class="token operator">!</span>prev_un_pts_map<span class="token punctuation">.</span><span class="token function">empty</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">)</span><span class="token comment">// 2.1 地图不是空的判断是否新的点</span> <span class="token punctuation">{<!-- --></span> <span class="token keyword">double</span> dt <span class="token operator">=</span> cur_time <span class="token operator">-</span> prev_time<span class="token punctuation">;</span> pts_velocity<span class="token punctuation">.</span><span class="token function">clear</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token keyword">for</span> <span class="token punctuation">(</span><span class="token keyword">unsigned</span> <span class="token keyword">int</span> i <span class="token operator">=</span> <span class="token number">0</span><span class="token punctuation">;</span> i <span class="token operator"><</span> cur_un_pts<span class="token punctuation">.</span><span class="token function">size</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">;</span> i<span class="token operator">++</span><span class="token punctuation">)</span> <span class="token punctuation">{<!-- --></span> <span class="token keyword">if</span> <span class="token punctuation">(</span>ids<span class="token punctuation">[</span>i<span class="token punctuation">]</span> <span class="token operator">!=</span> <span class="token operator">-</span><span class="token number">1</span><span class="token punctuation">)</span><span class="token comment">// 2.2 通过id判断不是最新的点</span> <span class="token punctuation">{<!-- --></span> std<span class="token operator">::</span>map<span class="token operator"><</span><span class="token keyword">int</span><span class="token punctuation">,</span> cv<span class="token operator">::</span>Point2f<span class="token operator">></span><span class="token operator">::</span>iterator it<span class="token punctuation">;</span><span class="token comment">// 地图的迭代器</span> it <span class="token operator">=</span> prev_un_pts_map<span class="token punctuation">.</span><span class="token function">find</span><span class="token punctuation">(</span>ids<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">)</span><span class="token punctuation">;</span><span class="token comment">// 找到对应的id</span> <span class="token keyword">if</span> <span class="token punctuation">(</span>it <span class="token operator">!=</span> prev_un_pts_map<span class="token punctuation">.</span><span class="token function">end</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">)</span> <span class="token comment">// 2.3 在地图中寻找是否出现过id判断是否最新点</span> <span class="token punctuation">{<!-- --></span> <span class="token keyword">double</span> v_x <span class="token operator">=</span> <span class="token punctuation">(</span>cur_un_pts<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">.</span>x <span class="token operator">-</span> it<span class="token operator">-</span><span class="token operator">></span>second<span class="token punctuation">.</span>x<span class="token punctuation">)</span> <span class="token operator">/</span> dt<span class="token punctuation">;</span><span class="token comment">// 当前帧-地图点上一帧</span> <span class="token keyword">double</span> v_y <span class="token operator">=</span> <span class="token punctuation">(</span>cur_un_pts<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">.</span>y <span class="token operator">-</span> it<span class="token operator">-</span><span class="token operator">></span>second<span class="token punctuation">.</span>y<span class="token punctuation">)</span> <span class="token operator">/</span> dt<span class="token punctuation">;</span> pts_velocity<span class="token punctuation">.</span><span class="token function">push_back</span><span class="token punctuation">(</span>cv<span class="token operator">::</span><span class="token function">Point2f</span><span class="token punctuation">(</span>v_x<span class="token punctuation">,</span> v_y<span class="token punctuation">)</span><span class="token punctuation">)</span><span class="token punctuation">;</span><span class="token comment">// 之前出现过，push_back即可</span> <span class="token punctuation">}</span> <span class="token keyword">else</span> pts_velocity<span class="token punctuation">.</span><span class="token function">push_back</span><span class="token punctuation">(</span>cv<span class="token operator">::</span><span class="token function">Point2f</span><span class="token punctuation">(</span><span class="token number">0</span><span class="token punctuation">,</span> <span class="token number">0</span><span class="token punctuation">)</span><span class="token punctuation">)</span><span class="token punctuation">;</span><span class="token comment">// 之前没出现过，先放进去但是速度为0</span> <span class="token punctuation">}</span> <span class="token keyword">else</span> <span class="token punctuation">{<!-- --></span> pts_velocity<span class="token punctuation">.</span><span class="token function">push_back</span><span class="token punctuation">(</span>cv<span class="token operator">::</span><span class="token function">Point2f</span><span class="token punctuation">(</span><span class="token number">0</span><span class="token punctuation">,</span> <span class="token number">0</span><span class="token punctuation">)</span><span class="token punctuation">)</span><span class="token punctuation">;</span><span class="token comment">// 是最新的点，速度为0</span> <span class="token punctuation">}</span> <span class="token punctuation">}</span> <span class="token punctuation">}</span> <span class="token keyword">else</span> <span class="token punctuation">{<!-- --></span> <span class="token comment">// 如果地图是空的，速度是0</span> <span class="token keyword">for</span> <span class="token punctuation">(</span><span class="token keyword">unsigned</span> <span class="token keyword">int</span> i <span class="token operator">=</span> <span class="token number">0</span><span class="token punctuation">;</span> i <span class="token operator"><</span> cur_pts<span class="token punctuation">.</span><span class="token function">size</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">;</span> i<span class="token operator">++</span><span class="token punctuation">)</span> <span class="token punctuation">{<!-- --></span> pts_velocity<span class="token punctuation">.</span><span class="token function">push_back</span><span class="token punctuation">(</span>cv<span class="token operator">::</span><span class="token function">Point2f</span><span class="token punctuation">(</span><span class="token number">0</span><span class="token punctuation">,</span> <span class="token number">0</span><span class="token punctuation">)</span><span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token punctuation">}</span> <span class="token punctuation">}</span> <span class="token comment">// 更新地图</span> prev_un_pts_map <span class="token operator">=</span> cur_un_pts_map<span class="token punctuation">;</span>
}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
- 61
- 62
- 63
293
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
