各个部分的讲解如下：
【SLAM】VINS-MONO解析——综述 【SLAM】VINS-MONO解析——feature_tracker 【SLAM】VINS-MONO解析——IMU预积分 【SLAM】VINS-MONO解析——vins_estimator 【SLAM】VINS-MONO解析——初始化(理论部分) 【SLAM】VINS-MONO解析——初始化(代码部分) 【SLAM】VINS-MONO解析——后端优化(理论部分) 【SLAM】VINS-MONO解析——后端优化(代码部分) 【SLAM】VINS-MONO解析——sliding window 【SLAM】VINS-MONO解析——回环检测 【SLAM】VINS-Fusion解析——流程 【SLAM】VINS-MONO解析——对vins-mono的修改使流程逻辑更清晰 【SLAM】VINS-MONO解析——基于vins-mono的slam系统开发
3. feature_tracker
feature_tracker是vins的前端，它的目录在src/ feature_tracker下，功能主要是获取摄像头的图像帧，并按照事先设定的频率，把cur帧上满足要求的特征点以sensor_msg::PointCloudPtr的格式发布出去，以便RVIZ和vins——estimator接收。 这个package下面主要包括4个功能： feature_tracker——包含特征提取/光流追踪的所有算法函数； feature_tracker_node——特征提取的主入口，负责一个特征处理结点的功能； parameters——负责读取来自配置文件的参数； tic_tok——计时器； 注意，还有package.xml和CmakeLists.txt上面定义了所有的外部依赖和可执行文件。流程图如下：
3.1 feature_tracker_node
在开头定义了一些变量，遇到的时候再一一解释。 1、ros初始化和设置句柄，设置logger级别； 2、读取如config->euroc->euroc_config.yaml中的一些配置参数； 3、读取每个相机实例对应的相机内参，NUM_OF_CAM=1为单目；
数据结构: FeatureTracker trackerData[NUM_OF_CAM]
定义在feature_tracker.h中的一个class，除了一系列成员函数外(注意，inBorder(),reduceVector()并没有定义在这个class里)，还包括了最近3帧所有特征点的一些信息。如何理解这个变量？它存储着当前时刻所有相关的数据，内部数据的生命周期仅限当前循环，如果当前的一个循环过去了，里面的数据是下一时刻的了。
它包括以下数据成员，
cv::Mat mask;//图像掩码
cv::Mat fisheye_mask;//鱼眼相机mask，用来去除边缘噪点
cv::Mat prev_img, cur_img, forw_img;//是上上次发布的帧的图像数据/光流跟踪的上一帧的图像数据/当前的图像数据
vector<cv::Point2f> n_pts;//每一帧中新提取的特征点
vector<cv::Point2f> prev_pts, cur_pts, forw_pts;//对应的图像特征点
vector<cv::Point2f> prev_un_pts, cur_un_pts;//归一化相机坐标系下的坐标
vector<cv::Point2f> pts_velocity;//当前帧相对前一帧特征点沿x,y方向的像素移动速
vector<int> ids;//能够被跟踪到的特征点的id
vector<int> track_cnt;//当前帧forw_img中每个特征点被追踪的时间次数
map<int, cv::Point2f> cur_un_pts_map; //构建id与归一化坐标的id，见undistortedPoints()
map<int, cv::Point2f> prev_un_pts_map;
camodocal::CameraPtr m_camera;//相机模型
double cur_time;
double prev_time;
static int n_id;//用来作为特征点id，每检测到一个新的特征点，就将++n_id作为该特征点
4、判断是否加入鱼眼mask来去除边缘噪声； 5、订阅话题IMAGE_TOPIC(如/cam0/image_raw)，执行回调函数img_callback；
ros::Subscriber sub_img = n.subscribe(IMAGE_TOPIC, 100, img_callback);
IMAGE_TOPIC就是配置文件中定义的/cam0/image_raw，一旦订阅到图像信息，那么就执行回调函数img_callback。 6、发布
pub_img = n.advertise<sensor_msgs::PointCloud>("feature", 1000); //跟踪的特征点图像，主要是之后给RVIZ用和调试用
pub_match = n.advertise<sensor_msgs::Image>("feature_img",1000); //即跟踪的特征点信息，由/vins_estimator订阅并进行优化
pub_restart = n.advertise<std_msgs::Bool>("restart",1000); //判断特征跟踪模块是否出错，若有问题则进行复位，由/vins_estimator订阅
数据结构: sensor_msgs
ROS的sensor_msgs格式在http://wiki.ros.org/sensor_msgs有介绍。
sensor_msgs::PointCloudConstPtr(和PointCloud) http://docs.ros.org/api/sensor_msgs/html/msg/PointCloud.html
注意，里面的points数据存放的是归一化坐标！见3.3.6
sensor_msgs::ImuConstPtr(和ImuConst) http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html
sensor_msgs::Image http://docs.ros.org/api/sensor_msgs/html/msg/Image.html
这些数据结构在以上链接里有详细介绍，一般header是时间戳，其余都是相关专门的数据。
3.2 img_callback
该函数是ROS的回调函数，主要功能包括：readImage()函数对新来的图像使用光流法进行特征点跟踪，并将追踪的特征点封装成feature_points发布到pub_img的话题下，将图像封装成ptr发布在pub_match下。
数据结构：
*init_feature 0:当前是第一帧 1:当前不是第一帧
*first_image_flag 0:当前是第一帧 1:当前不是第一帧
*pub_count 每隔delta_t = 1/FREQ 时间内连续(没有中断/没有报错)发布的帧数
*first_image_time 每隔delta_t = 1/FREQ 时间的帧对应的时间戳
*last_image_time 当前帧或上一帧的时间戳
*restart_flag 0:不重启。 1:重启(std_msgs::Bool)
*FREQ 发布特征点的频率(也就是bk/ck帧与bk+1/ck+1帧之间的频率)，注意，FREQ要比实际的照片频率要慢
*init_pub 0:第一帧不把特征发布到buf里 1:发布到buf里
3.2.1对第一帧图像的处理 对于第一帧图像，只记录对应时间戳，不提取特征，因为他没有前一帧图像，无法获取光流。
if(first_image_flag)
{
first_image_flag = false;
first_image_time = img_msg->header.stamp.toSec(); //get the time of the image
last_image_time = img_msg->header.stamp.toSec();
return;
}
3.2.2 频率控制 (1)对于时间戳错乱的帧，重新初始化；
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); //Guyue-ROS21Jiang-10-P4
return;
}
(2) 更新: last_image_time = img_msg->header.stamp.toSec(); (3)首先计算当前累计的pub_count个帧的频率与FREQ的关系 如果实际发布频率大于设定值，肯定就不发了 所以，要想发布图像帧，那么实际频率要比设定值小 但是，如果实际频率与设定频率的累积误差大于0.01了，就不能发布这一帧
if (round(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time)) <= FREQ)
{
//TODO:这句话可以放到下面那个if里面把？
PUB_THIS_FRAME = true;//发布特征点
// reset the frequency control
if (abs(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time) - FREQ) < 0.01 * FREQ)
{ //this "if" make sure the frequece is not so high
first_image_time = img_msg->header.stamp.toSec();
// TODO:我觉得pub_count最大只能是1，因为只要能发布一帧，这个数就会被清0
pub_count = 0;
}
}
else
PUB_THIS_FRAME = false;
3.3.3 图像的格式调整和图像读取 读取sensor_msgs::Image img的数据，并转为MONO8格式，用cv::Mat show_img接收。
cv_bridge::CvImageConstPtr ptr;
if (img_msg->encoding == "8UC1") //8位灰度图像
{
sensor_msgs::Image img; // I think it is to change 'sensor_msgs::ImageConstPtr'(const) to non-const
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";
ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::MONO8);//将图像编码8UC1转换为mono8,即存储下来的图像为单色，8Bit的图片，一般是bmp，jpeg等
}
else
ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::MONO8);
cv::Mat show_img = ptr->image;
数据结构：
构建了CV:Mat与sensor_masg::Image之间的桥梁。
http://wiki.ros.org/cv_bridge/Tutorials/UsingCvBridgeToConvertBetweenROSImagesAndOpenCVImages
注意，img_msg或img都是sensor_msg格式的，我们需要一个桥梁，转换为CV::Mat格式的数据，以供后续图像处理。
3.3.4 对最新帧forw的特征点的提取和光流追踪(核心)
img_callback()最最核心的语句出现在这里，也就是readImage()，这个函数实现了特征的处理和光流的追踪，里面基本上调用了feature_tracker.cpp里面的全部函数。在这里首先进行了单目和双目的判断，由于VINS-Mono是单目的，所以后部分关于双目的判断不用看，作者写在这里我觉得是为VINS-Fusion做准备的。在这里，trackerData[i]再次出现了(见3.1)。
for (int i = 0; i < NUM_OF_CAM; i++)
{
ROS_DEBUG("processing camera %d", i);
if (i != 1 || !STEREO_TRACK)//单目时：FeatureTracker::readImage() 函数读取图像数据进行处理
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));
}
readImage()传了2个参数，当前帧的图像和当前帧的时间戳,以下内容见feature_tracker.cpp，看一下原函数里面是怎么写的。 (1) 自适应直方图均衡化
if (EQUALIZE) //判断是否对图像进行自适应直方图均衡化
{
cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));//对图像进行自适应直方图均衡化
TicToc t_c;
clahe->apply(_img, img); //apply(InputArray src, OutputArray dst)
ROS_DEBUG("CLAHE costs: %fms", t_c.toc());
}
else
img = _img;
(2)首帧判断和对的forw_img更新 有关数据结构见3.1.。
if (forw_img.empty()) //如果当前帧的图像数据forw_img为空，说明当前是第一次读入图像数据
{ //将读入的图像赋给当前帧forw_img
prev_img = cur_img = forw_img = img;//还将读入的图像赋给prev_img、cur_img
} //这是为了避免后面使用到这些数据时，它们是空的
else
{
forw_img = img;
}
(3) forw_pts.clear() (4)光流追踪和失败点剔除 在这里实现了3大功能：光流追踪，outlier的剔除，并且不同容器里的数据仍然是对齐的！在《SLAM十四讲》第13讲的代码里，前端的对齐策略是往容器里面加空指针。这里通过reduceVector()实现对齐更为简单，避免了大量的指针操作，实际上，VINS的数据结构更多采用hash的方式，而《SLAM十四讲》更多地采用了指针。 这里通过inBorder(),status()来定位到outlier的位置，然后把status[i]==0的点统统剔除。
if (cur_pts.size() > 0)
{
TicToc t_o;
vector<uchar> status;
vector<float> err;
cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, cv::Size(21, 21), 3);
for (int i = 0; i < int(forw_pts.size()); i++)//将位于图像边界外的点标记为0
if (status[i] && !inBorder(forw_pts[i]))
status[i] = 0;
reduceVector(prev_pts, status);
reduceVector(cur_pts, status);
reduceVector(forw_pts, status);
reduceVector(ids, status);
reduceVector(cur_un_pts, status);
reduceVector(track_cnt, status);
ROS_DEBUG("temporal optical flow costs: %fms", t_o.toc());
}
(5)更新当前特征点被追踪到的次数 数据结构track_cnt见3.1.。
for (auto &n : track_cnt)//光流追踪成功,特征点被成功跟踪的次数就加1
n++; //数值代表被追踪的次数，数值越大，说明被追踪的就越久
(6) 通过基本矩阵剔除外点rejectWithF()
if (PUB_THIS_FRAME)
{
rejectWithF(); //通过基本矩阵剔除outliers
setMask(); //对跟踪点进行排序并去除密集点
int n_max_cnt = MAX_CNT - static_cast<int>(forw_pts.size());
if (n_max_cnt > 0)
{
cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.01, MIN_DIST, mask);
}
else
n_pts.clear();
addPoints();
}
上一段代码主要干了5件事(没用的代码都已经删除)。rejectWithF()主要是利用了cv::findFundamentalMat()这个函数来进一步剔除outlier。这个函数的功能对应在《SLAM十四讲》第七讲2D-2D对极几何相关知识点，两帧上的一系列对应的特征点能够复原出两帧之间的相对位姿变化，也就是基础矩阵E。但是这些特征点中肯定会有一些outlier，所以通过这个opencv的函数，能够巧妙地剔除这些outlier。补充一点，如果函数传入的是归一化坐标，那么得到的是本质矩阵E，如果传入的是像素坐标，那么得到的是基础矩阵。 首先看一下rejectWithF()，这一段代码的逻辑，我的理解，是先把特征点坐标(像素坐标)转为归一化坐标，再转回到像素坐标，然后再用findFundamentalMat()找outlier。它之所以这么做，是因为需要一个去畸变的过程，在m_camera->liftProjective()里实现，m_camera是定义在camera.h的一个父类的对象，下面有多个相机模型的子类，例如在PinholeCamera.cc里，这里面就重写liftProjective()了方法，实现了去畸变的功能。
void FeatureTracker::rejectWithF()//通过基本矩阵（F）去除外点outliers
{
if (forw_pts.size() >= 8)
{
ROS_DEBUG("FM ransac begins");
TicToc t_f;
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++)
{ //根据不同的相机模型将二维坐标转换到三维坐标;对于PINHOLE（针孔相机）可将像素坐标转换到归一化平面并去畸变
Eigen::Vector3d tmp_p; //对于CATA（卡特鱼眼相机）将像素坐标投影到单位圆内
//TODO 这里是利用像素坐标，基于m_camera的相机模型(pinhole)，获得对应的归一化坐标，
//自己写的时候看看能不能把这个引用的库砍掉
m_camera->liftProjective(Eigen::Vector2d(cur_pts[i].x, cur_pts[i].y), tmp_p);
tmp_p.x() = FOCAL_LENGTH * tmp_p.x() / tmp_p.z() + COL / 2.0; //转换为像素坐标
tmp_p.y() = FOCAL_LENGTH * tmp_p.y() / tmp_p.z() + ROW / 2.0;
un_cur_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y());
m_camera->liftProjective(Eigen::Vector2d(forw_pts[i].x, forw_pts[i].y), tmp_p);
tmp_p.x() = FOCAL_LENGTH * tmp_p.x() / tmp_p.z() + COL / 2.0;
tmp_p.y() = FOCAL_LENGTH * tmp_p.y() / tmp_p.z() + ROW / 2.0;
un_forw_pts[i] = cv::Point2f(tmp_p.x(), tmp_p.y());
}
vector<uchar> status;
cv::findFundamentalMat(un_cur_pts, un_forw_pts, cv::FM_RANSAC, F_THRESHOLD, 0.99, status);
int size_a = cur_pts.size();
reduceVector(prev_pts, status);
reduceVector(cur_pts, status);
reduceVector(forw_pts, status);
reduceVector(cur_un_pts, status);
reduceVector(ids, status);
reduceVector(track_cnt, status);
}
}
数据结构:
vector<cv::Point2f> un_cur_pts(cur_pts.size()), un_forw_pts(forw_pts.size())
分别是上一帧和当前帧去畸变的像素坐标。
(7) 对跟踪点进行排序并去除密集点setMask() 注意，这个函数的操作对象是forw_pts！这里，需要再次明确一下forw_pts里面是什么，它是通过光流法在当前帧中仍然能追踪到的那些特征点在当前帧中的像素坐标！所以forw_pts里面放着的都是老特征点。这里干了2件事，首先对他们根据追踪到的次数进行排序，从而确定了优先级；然后再把特征点周围的密集的特征点剔除掉。所以说，排序的作用就是为了给 剔除操作 提供优先级信息！因为追踪次数多的点在前面，所以在剔除的时候，这些时间长的特征点能够保留下来，而在这些老特征点周围比较近的距离内那些比较新的特征点就剔除掉，从而一方面保证了特征点的稳定，另一方面让特征点的分布更加均匀。
void FeatureTracker::setMask()
{
if(FISHEYE)
mask = fisheye_mask.clone();
else
mask = cv::Mat(ROW, COL, CV_8UC1, cv::Scalar(255));//将图像设置成单一灰度和颜色white
// prefer to keep features that are tracked for long time
vector<pair<int, pair<cv::Point2f, int>>> cnt_pts_id;//构造(cnt，pts，id)序列
for (unsigned int i = 0; i < forw_pts.size(); i++)//让他们在排序的过程中仍然能一一对应
cnt_pts_id.push_back(make_pair(track_cnt[i], make_pair(forw_pts[i], ids[i])));
sort(cnt_pts_id.begin(), cnt_pts_id.end(), [](const pair<int, pair<cv::Point2f, int>> &a, const pair<int, pair<cv::Point2f, int>> &b)
{//对光流跟踪到的特征点forw_pts，按照被跟踪到的次数cnt从大到小排序
return a.first > b.first;
});
forw_pts.clear();
ids.clear();
track_cnt.clear();
for (auto &it : cnt_pts_id)
{
if (mask.at<uchar>(it.second.first) == 255)
{//当前特征点位置对应的mask值为255，则保留当前特征点，将对应的特征点位置pts，id，被追踪次数cnt分别存入
forw_pts.push_back(it.second.first);
ids.push_back(it.second.second);
track_cnt.push_back(it.first);
cv::circle(mask, it.second.first, MIN_DIST, 0, -1);
}//在mask中将当前特征点周围半径为MIN_DIST的区域设置为0，后面不再选取该区域内的点（使跟踪点不集中在一个区域上）
}
}
(8)提取新特征点goodFeaturesToTrack() 一共需要MAX_CNT个特征点，当前有static_cast(forw_pts.size())个特征点，所以需要补充n_max_cnt = MAX_CNT – static_cast(forw_pts.size())个特征点。 (9) 补充新特征点addPoints()
void FeatureTracker::addPoints()
{
for (auto &p : n_pts) { forw_pts.push_back(p); ids.push_back(-1); track_cnt.push_back(1); }
}
(10)数据的更新和归一化坐标的获取
prev_img = cur_img;
prev_pts = cur_pts;
prev_un_pts = cur_un_pts;
cur_img = forw_img;
cur_pts = forw_pts;
undistortedPoints();
prev_time = cur_time;
这块需要小心，undistortedPoints()函数藏在这里，而这个函数不可以缺少。这个函数干了2件事，第一个是获取forw时刻去畸变的归一化坐标(这个是要发布到rosmsg里的points数据)，另一个是获取forw时刻像素运动速度。 为什么是forw时刻的？因为前面有cur_pts = forw_pts，当前还是forw时刻！所以说feature_tracker_node发布的其实是forw的数据！
void FeatureTracker::undistortedPoints()
{// 对特征点的图像坐标根据不同的相机模型进行去畸变矫正和深度归一化，计算每个角点的速度
cur_un_pts.clear();
cur_un_pts_map.clear();
//cv::undistortPoints(cur_pts, un_pts, K, cv::Mat());
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);
//获取cur帧下的归一化坐标和它与id的hash(这个hash唯一的用途也在这个函数里)
cur_un_pts.push_back(cv::Point2f(b.x() / b.z(), b.y() / b.z()));
cur_un_pts_map.insert(make_pair(ids[i], cv::Point2f(b.x() / b.z(), b.y() / b.z())));
//printf("cur pts id %d %f %f", ids[i], cur_un_pts[i].x, cur_un_pts[i].y);
}
// caculate points velocity
if (!prev_un_pts_map.empty())
{
double dt = cur_time - prev_time;
pts_velocity.clear();
for (unsigned int i = 0; i < cur_un_pts.size(); i++)
{
if (ids[i] != -1)
{
std::map<int, cv::Point2f>::iterator it;
it = prev_un_pts_map.find(ids[i]);
if (it != prev_un_pts_map.end())
{
double v_x = (cur_un_pts[i].x - it->second.x) / dt;
double v_y = (cur_un_pts[i].y - it->second.y) / dt;
pts_velocity.push_back(cv::Point2f(v_x, v_y));
}
else //TODO 这个判断可以删掉吧
pts_velocity.push_back(cv::Point2f(0, 0));
}
else
{pts_velocity.push_back(cv::Point2f(0, 0));}
}
}
else
{
for (unsigned int i = 0; i < cur_pts.size(); i++)
{
pts_velocity.push_back(cv::Point2f(0, 0));
}
}
prev_un_pts_map = cur_un_pts_map;
}
3.3.5 对新加入的特征点更新全局id 回到feature_tracker_node.cpp文件的img_callback()函数。completed(或者是update())如果是true，说明没有更新完id，则持续循环，如果是false，说明更新完了则跳出循环。注意n_id是static类型的数据，具有累加的功能。
for (unsigned int i = 0;; i++)//更新全局ID
{
bool completed = false;
for (int j = 0; j < NUM_OF_CAM; j++)
if (j != 1 || !STEREO_TRACK)
completed |= trackerData[j].updateID(i);
if (!completed)
break;
}
bool FeatureTracker::updateID(unsigned int i)
{
if (i < ids.size())
{
if (ids[i] == -1)
ids[i] = n_id++;
return true;
}
else
return false;//means all the pts are identified (i meets the end of ids.size())
}
3.3.6 特征点的发布 经过readimg()操作以后，prev-cur-forw帧所有的数据都基本上获取了也对应上了，然后就需要封装到sensor_msgs发布出去，由vins_estimator_node和rviz接收，分别进行后端操作和可视化。注意，这里发布的是cur帧上的所有被观测次数大于1的特征点。在feature_tracker里面，最新一帧是forw，上一帧是cur，上上帧是prev，但是这里发布的特征点是cur帧，所以说这里，能够帮助我们理解为什么这三帧的英语缩写这样写。实际上，forw是最新帧，但是逻辑上，cur才是当前帧。
if (PUB_THIS_FRAME)//如果PUB_THIS_FRAME=1则进行发布
{
pub_count++;
sensor_msgs::PointCloudPtr feature_points(new sensor_msgs::PointCloud);//归一化坐标
sensor_msgs::ChannelFloat32 id_of_point;
sensor_msgs::ChannelFloat32 u_of_point;//像素坐标x
sensor_msgs::ChannelFloat32 v_of_point;//像素坐标y
sensor_msgs::ChannelFloat32 velocity_x_of_point;
sensor_msgs::ChannelFloat32 velocity_y_of_point;
feature_points->header = img_msg->header;
feature_points->header.frame_id = "world";
vector<set<int>> hash_ids(NUM_OF_CAM);
for (int i = 0; i < NUM_OF_CAM; i++)
{
auto &un_pts = trackerData[i].cur_un_pts;//归一化坐标
auto &cur_pts = trackerData[i].cur_pts;//像素坐标
auto &ids = trackerData[i].ids;
auto &pts_velocity = trackerData[i].pts_velocity;
for (unsigned int j = 0; j < ids.size(); j++)
{
if (trackerData[i].track_cnt[j] > 1)//只发布追踪次数大于1的特征点
{
int p_id = ids[j];
hash_ids[i].insert(p_id);
geometry_msgs::Point32 p;//归一化坐标
p.x = un_pts[j].x;
p.y = un_pts[j].y;
p.z = 1;
feature_points->points.push_back(p);//归一化坐标
id_of_point.values.push_back(p_id * NUM_OF_CAM + i);
u_of_point.values.push_back(cur_pts[j].x);//像素坐标
v_of_point.values.push_back(cur_pts[j].y);//像素坐标
velocity_x_of_point.values.push_back(pts_velocity[j].x);
velocity_y_of_point.values.push_back(pts_velocity[j].y);
}
}//将特征点id，矫正后归一化平面的3D点(x,y,z=1)，像素2D点(u,v)，像素的速度(vx,vy)，
}//封装成sensor_msgs::PointCloudPtr类型的feature_points实例中,发布到pub_img
feature_points->channels.push_back(id_of_point);
feature_points->channels.push_back(u_of_point);
feature_points->channels.push_back(v_of_point);
feature_points->channels.push_back(velocity_x_of_point);
feature_points->channels.push_back(velocity_y_of_point);
ROS_DEBUG("publish %f, at %f", feature_points->header.stamp.toSec(), ros::Time::now().toSec());
//如果是第一帧的话，不发布数据skip the first image; since no optical speed on frist image
if (!init_pub)
{
init_pub = 1;
}
else
pub_img.publish(feature_points);
3.3.7 将图像封装到cv_bridge::cvtColor类型的ptr实例中发布到pub_match
if (SHOW_TRACK) //将图像封装到cv_bridge::cvtColor类型的ptr实例中发布到pub_match
{
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;
for (int i = 0; i < NUM_OF_CAM; i++)
{
cv::Mat tmp_img = stereo_img.rowRange(i * ROW, (i + 1) * ROW);
cv::cvtColor(show_img, tmp_img, CV_GRAY2RGB);
for (unsigned int j = 0; j < trackerData[i].cur_pts.size(); j++)
{
double len = std::min(1.0, 1.0 * trackerData[i].track_cnt[j] / WINDOW_SIZE);
cv::circle(tmp_img, trackerData[i].cur_pts[j], 2, cv::Scalar(255 * (1 - len), 0, 255 * len), 2);
}
}
//cv::imshow("vis", stereo_img);
//cv::waitKey(5);
pub_match.publish(ptr->toImageMsg());
}
}
